summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp')
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp933
1 files changed, 582 insertions, 351 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
index e8563238c3..befbc2e2a4 100644
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
+++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
@@ -1,16 +1,16 @@
#include "MultiBodyTreeImpl.hpp"
-namespace btInverseDynamics {
-
+namespace btInverseDynamics
+{
MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
- ,m_m3x(3,m_num_dofs)
+ ,
+ m_m3x(3, m_num_dofs)
#endif
{
-
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
- resize(m_m3x,m_num_dofs);
+ resize(m_m3x, m_num_dofs);
#endif
m_body_list.resize(num_bodies_);
m_parent_index.resize(num_bodies_);
@@ -23,8 +23,10 @@ MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
m_world_gravity(2) = -9.8;
}
-const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const {
- switch (type) {
+const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const
+{
+ switch (type)
+ {
case FIXED:
return "fixed";
case REVOLUTE:
@@ -33,22 +35,28 @@ const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &typ
return "prismatic";
case FLOATING:
return "floating";
+ case SPHERICAL:
+ return "spherical";
}
return "error: invalid";
}
-inline void indent(const int &level) {
+inline void indent(const int &level)
+{
for (int j = 0; j < level; j++)
id_printf(" "); // indent
}
-void MultiBodyTree::MultiBodyImpl::printTree() {
+void MultiBodyTree::MultiBodyImpl::printTree()
+{
id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type));
printTree(0, 0);
}
-void MultiBodyTree::MultiBodyImpl::printTreeData() {
- for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+void MultiBodyTree::MultiBodyImpl::printTreeData()
+{
+ for (idArrayIdx i = 0; i < m_body_list.size(); i++)
+ {
RigidBody &body = m_body_list[i];
id_printf("body: %d\n", static_cast<int>(i));
id_printf("type: %s\n", jointTypeToString(body.m_joint_type));
@@ -59,19 +67,22 @@ void MultiBodyTree::MultiBodyImpl::printTreeData() {
id_printf("mass = %f\n", body.m_mass);
id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1),
body.m_body_mass_com(2));
- id_printf("I_o= [%f %f %f;\n"
- " %f %f %f;\n"
- " %f %f %f]\n",
- body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2),
- body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2),
- body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2));
+ id_printf(
+ "I_o= [%f %f %f;\n"
+ " %f %f %f;\n"
+ " %f %f %f]\n",
+ body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2),
+ body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2),
+ body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2));
id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0),
body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2));
}
}
-int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const {
- switch (type) {
+int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
+{
+ switch (type)
+ {
case FIXED:
return 0;
case REVOLUTE:
@@ -79,12 +90,15 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const {
return 1;
case FLOATING:
return 6;
+ case SPHERICAL:
+ return 3;
}
bt_id_error_message("unknown joint type %d\n", type);
return 0;
}
-void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) {
+void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation)
+{
// this is adapted from URDF2Bullet.
// TODO: fix this and print proper graph (similar to git --log --graph)
int num_children = m_child_indices[index].size();
@@ -92,7 +106,8 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) {
indentation += 2;
int count = 0;
- for (int i = 0; i < num_children; i++) {
+ for (int i = 0; i < num_children; i++)
+ {
int child_index = m_child_indices[index][i];
indent(indentation);
id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index,
@@ -104,19 +119,23 @@ void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) {
}
}
-int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) {
+int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity)
+{
m_world_gravity = gravity;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
+int MultiBodyTree::MultiBodyImpl::generateIndexSets()
+{
m_body_revolute_list.resize(0);
m_body_prismatic_list.resize(0);
int q_index = 0;
- for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_list.size(); i++)
+ {
RigidBody &body = m_body_list[i];
body.m_q_index = -1;
- switch (body.m_joint_type) {
+ switch (body.m_joint_type)
+ {
case REVOLUTE:
m_body_revolute_list.push_back(i);
body.m_q_index = q_index;
@@ -135,28 +154,41 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
body.m_q_index = q_index;
q_index += 6;
break;
+ case SPHERICAL:
+ m_body_spherical_list.push_back(i);
+ body.m_q_index = q_index;
+ q_index += 3;
+ break;
default:
bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
return -1;
}
}
// sanity check
- if (q_index != m_num_dofs) {
+ if (q_index != m_num_dofs)
+ {
bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
return -1;
}
m_child_indices.resize(m_body_list.size());
- for (idArrayIdx child = 1; child < m_parent_index.size(); child++) {
+ for (idArrayIdx child = 1; child < m_parent_index.size(); child++)
+ {
const int &parent = m_parent_index[child];
- if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1)) {
+ if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1))
+ {
m_child_indices[parent].push_back(child);
- } else {
- if (-1 == parent) {
+ }
+ else
+ {
+ if (-1 == parent)
+ {
// multiple bodies are directly linked to the environment, ie, not a single root
bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
- } else {
+ }
+ else
+ {
// should never happen
bt_id_error_message(
"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
@@ -169,11 +201,14 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() {
return 0;
}
-void MultiBodyTree::MultiBodyImpl::calculateStaticData() {
+void MultiBodyTree::MultiBodyImpl::calculateStaticData()
+{
// relative kinematics that are not a function of q, u, dot_u
- for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_list.size(); i++)
+ {
RigidBody &body = m_body_list[i];
- switch (body.m_joint_type) {
+ switch (body.m_joint_type)
+ {
case REVOLUTE:
body.m_parent_vel_rel(0) = 0;
body.m_parent_vel_rel(1) = 0;
@@ -212,41 +247,56 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() {
case FLOATING:
// no static data
break;
+ case SPHERICAL:
+ //todo: review
+ body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
+ body.m_parent_vel_rel(0) = 0;
+ body.m_parent_vel_rel(1) = 0;
+ body.m_parent_vel_rel(2) = 0;
+ body.m_parent_acc_rel(0) = 0;
+ body.m_parent_acc_rel(1) = 0;
+ body.m_parent_acc_rel(2) = 0;
+ break;
}
- // resize & initialize jacobians to zero.
+ // resize & initialize jacobians to zero.
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
- body.m_body_dot_Jac_T_u(0) = 0.0;
- body.m_body_dot_Jac_T_u(1) = 0.0;
- body.m_body_dot_Jac_T_u(2) = 0.0;
- body.m_body_dot_Jac_R_u(0) = 0.0;
- body.m_body_dot_Jac_R_u(1) = 0.0;
- body.m_body_dot_Jac_R_u(2) = 0.0;
- resize(body.m_body_Jac_T,m_num_dofs);
- resize(body.m_body_Jac_R,m_num_dofs);
- body.m_body_Jac_T.setZero();
- body.m_body_Jac_R.setZero();
-#endif //
+ body.m_body_dot_Jac_T_u(0) = 0.0;
+ body.m_body_dot_Jac_T_u(1) = 0.0;
+ body.m_body_dot_Jac_T_u(2) = 0.0;
+ body.m_body_dot_Jac_R_u(0) = 0.0;
+ body.m_body_dot_Jac_R_u(1) = 0.0;
+ body.m_body_dot_Jac_R_u(2) = 0.0;
+ resize(body.m_body_Jac_T, m_num_dofs);
+ resize(body.m_body_Jac_R, m_num_dofs);
+ body.m_body_Jac_T.setZero();
+ body.m_body_Jac_R.setZero();
+#endif //
}
}
int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u,
- const vecx &dot_u, vecx *joint_forces) {
+ const vecx &dot_u, vecx *joint_forces)
+{
if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
- joint_forces->size() != m_num_dofs) {
- bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
- "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
- m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
- static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
+ joint_forces->size() != m_num_dofs)
+ {
+ bt_id_error_message(
+ "wrong vector dimension. system has %d DOFs,\n"
+ "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
+ m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
+ static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
return -1;
}
// 1. relative kinematics
- if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
- bt_id_error_message("error in calculateKinematics\n");
- return -1;
- }
- // 2. update contributions to equations of motion for every body.
- for (idArrayIdx i = 0; i < m_body_list.size(); i++) {
+ if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION))
+ {
+ bt_id_error_message("error in calculateKinematics\n");
+ return -1;
+ }
+ // 2. update contributions to equations of motion for every body.
+ for (idArrayIdx i = 0; i < m_body_list.size(); i++)
+ {
RigidBody &body = m_body_list[i];
// 3.4 update dynamic terms (rate of change of angular & linear momentum)
body.m_eom_lhs_rotational =
@@ -268,14 +318,16 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
// Also, this enables adding zero weight bodies as a way to calculate frame poses
// for force elements, etc.
- for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) {
+ for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--)
+ {
// sum of forces and moments acting on this body from its children
vec3 sum_f_children;
vec3 sum_m_children;
setZero(sum_f_children);
setZero(sum_m_children);
for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
- child_list_idx++) {
+ child_list_idx++)
+ {
const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
vec3 child_joint_force_in_this_frame =
child.m_body_T_parent.transpose() * child.m_force_at_joint;
@@ -293,19 +345,22 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
// These are the components of force_at_joint/moment_at_joint
// in the free directions given by Jac_JT/Jac_JR
// 4.1 revolute joints
- for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_revolute_list[i]];
// (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
(*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
}
// 4.2 for prismatic joints
- for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
// (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
(*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
}
// 4.3 floating bodies (6-DoF joints)
- for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_floating_list[i]];
(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
@@ -316,84 +371,133 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
}
+ // 4.4 spherical bodies (3-DoF joints)
+ for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+ {
+ //todo: review
+ RigidBody &body = m_body_list[m_body_spherical_list[i]];
+ (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
+ (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
+ (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
+ }
return 0;
}
-int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u,
- const KinUpdateType type) {
- if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) {
- bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
- "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
- m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
- static_cast<int>(dot_u.size()));
+int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u,
+ const KinUpdateType type)
+{
+ if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs)
+ {
+ bt_id_error_message(
+ "wrong vector dimension. system has %d DOFs,\n"
+ "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
+ m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
+ static_cast<int>(dot_u.size()));
+ return -1;
+ }
+ if (type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION)
+ {
+ bt_id_error_message("invalid type %d\n", type);
return -1;
}
- if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) {
- bt_id_error_message("invalid type %d\n", type);
- return -1;
- }
// 1. update relative kinematics
// 1.1 for revolute
- for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_revolute_list[i]];
mat33 T;
bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
body.m_body_T_parent = T * body.m_body_T_parent_ref;
- if(type >= POSITION_VELOCITY) {
- body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
- }
- if(type >= POSITION_VELOCITY_ACCELERATION) {
- body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
- }
+ if (type >= POSITION_VELOCITY)
+ {
+ body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
+ }
+ if (type >= POSITION_VELOCITY_ACCELERATION)
+ {
+ body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
+ }
}
// 1.2 for prismatic
- for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
body.m_parent_pos_parent_body =
body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
- if(type >= POSITION_VELOCITY) {
- body.m_parent_vel_rel =
- body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
- }
- if(type >= POSITION_VELOCITY_ACCELERATION) {
- body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
- }
+ if (type >= POSITION_VELOCITY)
+ {
+ body.m_parent_vel_rel =
+ body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
+ }
+ if (type >= POSITION_VELOCITY_ACCELERATION)
+ {
+ body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
+ }
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
- for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_floating_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
- transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
+ transformY(q(body.m_q_index + 1)) *
+ transformX(q(body.m_q_index));
body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
- if(type >= POSITION_VELOCITY) {
- body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
- body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
- body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
+ if (type >= POSITION_VELOCITY)
+ {
+ body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
+ body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
+ body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
- body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
- body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
- body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
+ body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
+ body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
+ body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
- body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
- }
- if(type >= POSITION_VELOCITY_ACCELERATION) {
- body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
- body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
- body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
+ body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
+ }
+ if (type >= POSITION_VELOCITY_ACCELERATION)
+ {
+ body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
+ body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
+ body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
- body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
- body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
- body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
+ body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
+ body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
+ body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
- body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
- }
+ body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+ }
+ }
+
+ for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+ {
+ //todo: review
+ RigidBody &body = m_body_list[m_body_spherical_list[i]];
+
+ body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
+ transformY(q(body.m_q_index + 1)) *
+ transformX(q(body.m_q_index));
+ body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
+
+ if (type >= POSITION_VELOCITY)
+ {
+ body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
+ body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
+ body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
+ body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
+ }
+ if (type >= POSITION_VELOCITY_ACCELERATION)
+ {
+ body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
+ body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
+ body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
+ body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+ }
}
// 2. absolute kinematic quantities (vector valued)
@@ -410,26 +514,29 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_body_T_world = body.m_body_T_parent;
- if(type >= POSITION_VELOCITY) {
- // 3.2 update absolute velocities
- body.m_body_ang_vel = body.m_body_ang_vel_rel;
- body.m_body_vel = body.m_parent_vel_rel;
- }
- if(type >= POSITION_VELOCITY_ACCELERATION) {
- // 3.3 update absolute accelerations
- // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
- body.m_body_ang_acc = body.m_body_ang_acc_rel;
- body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
- // add gravitational acceleration to root body
- // this is an efficient way to add gravitational terms,
- // but it does mean that the kinematics are no longer
- // correct at the acceleration level
- // NOTE: To get correct acceleration kinematics, just set world_gravity to zero
- body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
- }
+ if (type >= POSITION_VELOCITY)
+ {
+ // 3.2 update absolute velocities
+ body.m_body_ang_vel = body.m_body_ang_vel_rel;
+ body.m_body_vel = body.m_parent_vel_rel;
+ }
+ if (type >= POSITION_VELOCITY_ACCELERATION)
+ {
+ // 3.3 update absolute accelerations
+ // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
+ body.m_body_ang_acc = body.m_body_ang_acc_rel;
+ body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
+ // add gravitational acceleration to root body
+ // this is an efficient way to add gravitational terms,
+ // but it does mean that the kinematics are no longer
+ // correct at the acceleration level
+ // NOTE: To get correct acceleration kinematics, just set world_gravity to zero
+ body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
+ }
}
- for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
+ for (idArrayIdx i = 1; i < m_body_list.size(); i++)
+ {
RigidBody &body = m_body_list[i];
RigidBody &parent = m_body_list[m_parent_index[i]];
// 2.1 update absolute positions and orientations:
@@ -439,121 +546,159 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
- if(type >= POSITION_VELOCITY) {
- // 2.2 update absolute velocities
- body.m_body_ang_vel =
- body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
-
- body.m_body_vel =
- body.m_body_T_parent *
- (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
- body.m_parent_vel_rel);
- }
- if(type >= POSITION_VELOCITY_ACCELERATION) {
- // 2.3 update absolute accelerations
- // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
- body.m_body_ang_acc =
- body.m_body_T_parent * parent.m_body_ang_acc -
- body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
- body.m_body_ang_acc_rel;
- body.m_body_acc =
- body.m_body_T_parent *
- (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
- parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
- 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
- }
+ if (type >= POSITION_VELOCITY)
+ {
+ // 2.2 update absolute velocities
+ body.m_body_ang_vel =
+ body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
+
+ body.m_body_vel =
+ body.m_body_T_parent *
+ (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
+ body.m_parent_vel_rel);
+ }
+ if (type >= POSITION_VELOCITY_ACCELERATION)
+ {
+ // 2.3 update absolute accelerations
+ // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
+ body.m_body_ang_acc =
+ body.m_body_T_parent * parent.m_body_ang_acc -
+ body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
+ body.m_body_ang_acc_rel;
+ body.m_body_acc =
+ body.m_body_T_parent *
+ (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
+ parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
+ 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
+ }
}
- return 0;
+ return 0;
}
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) {
- const int& idx=body.m_q_index;
- switch(body.m_joint_type) {
- case FIXED:
- break;
- case REVOLUTE:
- setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
- setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
- setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
- break;
- case PRISMATIC:
- setMat3xElem(0,idx, body.m_body_T_parent_ref(0,0)*body.m_Jac_JT(0)
- +body.m_body_T_parent_ref(1,0)*body.m_Jac_JT(1)
- +body.m_body_T_parent_ref(2,0)*body.m_Jac_JT(2),
- &body.m_body_Jac_T);
- setMat3xElem(1,idx,body.m_body_T_parent_ref(0,1)*body.m_Jac_JT(0)
- +body.m_body_T_parent_ref(1,1)*body.m_Jac_JT(1)
- +body.m_body_T_parent_ref(2,1)*body.m_Jac_JT(2),
- &body.m_body_Jac_T);
- setMat3xElem(2,idx, body.m_body_T_parent_ref(0,2)*body.m_Jac_JT(0)
- +body.m_body_T_parent_ref(1,2)*body.m_Jac_JT(1)
- +body.m_body_T_parent_ref(2,2)*body.m_Jac_JT(2),
- &body.m_body_Jac_T);
- break;
- case FLOATING:
- setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R);
- setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R);
- setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R);
- // body_Jac_T = body_T_parent.transpose();
- setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T);
- setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T);
- setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T);
-
- setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T);
- setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T);
- setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T);
-
- setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T);
- setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T);
- setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T);
-
- break;
- }
+void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
+{
+ const int &idx = body.m_q_index;
+ switch (body.m_joint_type)
+ {
+ case FIXED:
+ break;
+ case REVOLUTE:
+ setMat3xElem(0, idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
+ setMat3xElem(1, idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
+ setMat3xElem(2, idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
+ break;
+ case PRISMATIC:
+ setMat3xElem(0, idx, body.m_body_T_parent_ref(0, 0) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 0) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 0) * body.m_Jac_JT(2),
+ &body.m_body_Jac_T);
+ setMat3xElem(1, idx, body.m_body_T_parent_ref(0, 1) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 1) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 1) * body.m_Jac_JT(2),
+ &body.m_body_Jac_T);
+ setMat3xElem(2, idx, body.m_body_T_parent_ref(0, 2) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 2) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 2) * body.m_Jac_JT(2),
+ &body.m_body_Jac_T);
+ break;
+ case FLOATING:
+ setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
+ setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
+ setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
+ // body_Jac_T = body_T_parent.transpose();
+ setMat3xElem(0, idx + 3, body.m_body_T_parent(0, 0), &body.m_body_Jac_T);
+ setMat3xElem(0, idx + 4, body.m_body_T_parent(1, 0), &body.m_body_Jac_T);
+ setMat3xElem(0, idx + 5, body.m_body_T_parent(2, 0), &body.m_body_Jac_T);
+
+ setMat3xElem(1, idx + 3, body.m_body_T_parent(0, 1), &body.m_body_Jac_T);
+ setMat3xElem(1, idx + 4, body.m_body_T_parent(1, 1), &body.m_body_Jac_T);
+ setMat3xElem(1, idx + 5, body.m_body_T_parent(2, 1), &body.m_body_Jac_T);
+
+ setMat3xElem(2, idx + 3, body.m_body_T_parent(0, 2), &body.m_body_Jac_T);
+ setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
+ setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
+
+ break;
+ case SPHERICAL:
+ //todo: review
+ setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
+ setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
+ setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
+ break;
+ }
}
-int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
- if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
- bt_id_error_message("wrong vector dimension. system has %d DOFs,\n"
- "but dim(q)= %d, dim(u)= %d\n",
- m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
- return -1;
- }
- if(type != POSITION_ONLY && type != POSITION_VELOCITY) {
- bt_id_error_message("invalid type %d\n", type);
- return -1;
- }
-
- addRelativeJacobianComponent(m_body_list[0]);
- for (idArrayIdx i = 1; i < m_body_list.size(); i++) {
- RigidBody &body = m_body_list[i];
- RigidBody &parent = m_body_list[m_parent_index[i]];
-
- mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R);
- body.m_body_Jac_T = parent.m_body_Jac_T;
- mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x);
- sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T);
-
- addRelativeJacobianComponent(body);
- mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T);
-
- if(type >= POSITION_VELOCITY) {
- body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
- body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
- body.m_body_dot_Jac_T_u = body.m_body_T_parent *
- (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
- parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
- 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
- }
- }
- return 0;
+int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type)
+{
+ if (q.size() != m_num_dofs || u.size() != m_num_dofs)
+ {
+ bt_id_error_message(
+ "wrong vector dimension. system has %d DOFs,\n"
+ "but dim(q)= %d, dim(u)= %d\n",
+ m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
+ return -1;
+ }
+ if (type != POSITION_ONLY && type != POSITION_VELOCITY)
+ {
+ bt_id_error_message("invalid type %d\n", type);
+ return -1;
+ }
+
+ addRelativeJacobianComponent(m_body_list[0]);
+ for (idArrayIdx i = 1; i < m_body_list.size(); i++)
+ {
+ RigidBody &body = m_body_list[i];
+ RigidBody &parent = m_body_list[m_parent_index[i]];
+
+ mul(body.m_body_T_parent, parent.m_body_Jac_R, &body.m_body_Jac_R);
+ body.m_body_Jac_T = parent.m_body_Jac_T;
+ mul(tildeOperator(body.m_parent_pos_parent_body), parent.m_body_Jac_R, &m_m3x);
+ sub(body.m_body_Jac_T, m_m3x, &body.m_body_Jac_T);
+
+ addRelativeJacobianComponent(body);
+ mul(body.m_body_T_parent, body.m_body_Jac_T, &body.m_body_Jac_T);
+
+ if (type >= POSITION_VELOCITY)
+ {
+ body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
+ body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
+ body.m_body_dot_Jac_T_u = body.m_body_T_parent *
+ (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
+ parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
+ 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
+ }
+ }
+ return 0;
}
#endif
-static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) {
- switch (dof) {
+static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
+{
+ switch (dof)
+ {
+ // rotational part
+ case 0:
+ Jac_JR(0) = 1;
+ Jac_JR(1) = 0;
+ Jac_JR(2) = 0;
+ setZero(Jac_JT);
+ break;
+ case 1:
+ Jac_JR(0) = 0;
+ Jac_JR(1) = 1;
+ Jac_JR(2) = 0;
+ setZero(Jac_JT);
+ break;
+ case 2:
+ Jac_JR(0) = 0;
+ Jac_JR(1) = 0;
+ Jac_JR(2) = 1;
+ setZero(Jac_JT);
+ break;
+ }
+}
+
+static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
+{
+ switch (dof)
+ {
// rotational part
case 0:
Jac_JR(0) = 1;
@@ -595,8 +740,10 @@ static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
}
}
-static inline int jointNumDoFs(const JointType &type) {
- switch (type) {
+static inline int jointNumDoFs(const JointType &type)
+{
+ switch (type)
+ {
case FIXED:
return 0;
case REVOLUTE:
@@ -604,6 +751,8 @@ static inline int jointNumDoFs(const JointType &type) {
return 1;
case FLOATING:
return 6;
+ case SPHERICAL:
+ return 3;
}
// this should never happen
bt_id_error_message("invalid joint type\n");
@@ -615,37 +764,45 @@ static inline int jointNumDoFs(const JointType &type) {
int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
const bool initialize_matrix,
const bool set_lower_triangular_matrix,
- matxx *mass_matrix) {
-// This calculates the joint space mass matrix for the multibody system.
-// The algorithm is essentially an implementation of "method 3"
-// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982)
-// (Later named "Composite Rigid Body Algorithm" by Featherstone).
-//
-// This implementation, however, handles branched systems and uses a formulation centered
-// on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
+ matxx *mass_matrix)
+{
+ // This calculates the joint space mass matrix for the multibody system.
+ // The algorithm is essentially an implementation of "method 3"
+ // in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982)
+ // (Later named "Composite Rigid Body Algorithm" by Featherstone).
+ //
+ // This implementation, however, handles branched systems and uses a formulation centered
+ // on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
- mass_matrix->cols() != m_num_dofs) {
- bt_id_error_message("Dimension error. System has %d DOFs,\n"
- "but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
- m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
- static_cast<int>(mass_matrix->cols()));
+ mass_matrix->cols() != m_num_dofs)
+ {
+ bt_id_error_message(
+ "Dimension error. System has %d DOFs,\n"
+ "but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
+ m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
+ static_cast<int>(mass_matrix->cols()));
return -1;
}
// TODO add optimized zeroing function?
- if (initialize_matrix) {
- for (int i = 0; i < m_num_dofs; i++) {
- for (int j = 0; j < m_num_dofs; j++) {
- setMatxxElem(i, j, 0.0, mass_matrix);
+ if (initialize_matrix)
+ {
+ for (int i = 0; i < m_num_dofs; i++)
+ {
+ for (int j = 0; j < m_num_dofs; j++)
+ {
+ setMatxxElem(i, j, 0.0, mass_matrix);
}
}
}
- if (update_kinematics) {
+ if (update_kinematics)
+ {
// 1. update relative kinematics
// 1.1 for revolute joints
- for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_revolute_list[i]];
// from reference orientation (q=0) of body-fixed frame to current orientation
mat33 body_T_body_ref;
@@ -653,7 +810,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref;
}
// 1.2 for prismatic joints
- for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_prismatic_list[i]];
// body.m_body_T_parent= fixed
body.m_parent_pos_parent_body =
@@ -661,7 +819,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
- for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) {
+ for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
+ {
RigidBody &body = m_body_list[m_body_floating_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
@@ -674,7 +833,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
}
}
- for (int i = m_body_list.size() - 1; i >= 0; i--) {
+ for (int i = m_body_list.size() - 1; i >= 0; i--)
+ {
RigidBody &body = m_body_list[i];
// calculate mass, center of mass and inertia of "composite rigid body",
// ie, sub-tree starting at current body
@@ -682,7 +842,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
body.m_body_subtree_mass_com = body.m_body_mass_com;
body.m_body_subtree_I_body = body.m_body_I_body;
- for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) {
+ for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++)
+ {
RigidBody &child = m_body_list[m_child_indices[i][c]];
mat33 body_T_child = child.m_body_T_parent.transpose();
@@ -692,7 +853,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
body.m_body_subtree_I_body +=
body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent;
- if (child.m_subtree_mass > 0) {
+ if (child.m_subtree_mass > 0)
+ {
// Shift the reference point for the child subtree inertia using the
// Huygens-Steiner ("parallel axis") theorem.
// (First shift from child origin to child com, then from there to this body's
@@ -707,7 +869,8 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
}
}
- for (int i = m_body_list.size() - 1; i >= 0; i--) {
+ for (int i = m_body_list.size() - 1; i >= 0; i--)
+ {
const RigidBody &body = m_body_list[i];
// determine DoF-range for body
@@ -717,11 +880,18 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// local joint jacobians (ok as is for 1-DoF joints)
vec3 Jac_JR = body.m_Jac_JR;
vec3 Jac_JT = body.m_Jac_JT;
- for (int col = q_index_max; col >= q_index_min; col--) {
+ for (int col = q_index_max; col >= q_index_min; col--)
+ {
// set jacobians for 6-DoF joints
- if (FLOATING == body.m_joint_type) {
+ if (FLOATING == body.m_joint_type)
+ {
setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
}
+ if (SPHERICAL == body.m_joint_type)
+ {
+ //todo: review
+ setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
+ }
vec3 body_eom_rot =
body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
@@ -732,19 +902,27 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// rest of the mass matrix column upwards
{
// 1. for multi-dof joints, rest of the dofs of this body
- for (int row = col - 1; row >= q_index_min; row--) {
- if (FLOATING != body.m_joint_type) {
- bt_id_error_message("??\n");
- return -1;
+ for (int row = col - 1; row >= q_index_min; row--)
+ {
+ if (SPHERICAL == body.m_joint_type)
+ {
+ //todo: review
+ setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
+ const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
+ setMatxxElem(col, row, Mrc, mass_matrix);
+ }
+ if (FLOATING == body.m_joint_type)
+ {
+ setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
+ const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
+ setMatxxElem(col, row, Mrc, mass_matrix);
}
- setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
- const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
- setMatxxElem(col, row, Mrc, mass_matrix);
}
// 2. ancestor dofs
int child_idx = i;
int parent_idx = m_parent_index[i];
- while (parent_idx >= 0) {
+ while (parent_idx >= 0)
+ {
const RigidBody &child_body = m_body_list[child_idx];
const RigidBody &parent_body = m_body_list[parent_idx];
@@ -758,9 +936,16 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1;
vec3 Jac_JR = parent_body.m_Jac_JR;
vec3 Jac_JT = parent_body.m_Jac_JT;
- for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) {
+ for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
+ {
+ if (SPHERICAL == parent_body.m_joint_type)
+ {
+ //todo: review
+ setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
+ }
// set jacobians for 6-DoF joints
- if (FLOATING == parent_body.m_joint_type) {
+ if (FLOATING == parent_body.m_joint_type)
+ {
setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
}
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
@@ -774,10 +959,13 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
}
}
- if (set_lower_triangular_matrix) {
- for (int col = 0; col < m_num_dofs; col++) {
- for (int row = 0; row < col; row++) {
- setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
+ if (set_lower_triangular_matrix)
+ {
+ for (int col = 0; col < m_num_dofs; col++)
+ {
+ for (int row = 0; row < col; row++)
+ {
+ setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
}
}
}
@@ -785,76 +973,91 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
}
// utility macro
-#define CHECK_IF_BODY_INDEX_IS_VALID(index) \
- do { \
- if (index < 0 || index >= m_num_bodies) { \
- bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
- return -1; \
- } \
+#define CHECK_IF_BODY_INDEX_IS_VALID(index) \
+ do \
+ { \
+ if (index < 0 || index >= m_num_bodies) \
+ { \
+ bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
+ return -1; \
+ } \
} while (0)
-int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) {
+int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*p = m_parent_index[body_index];
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const {
+int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*user_int = m_user_int[body_index];
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const {
+int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*user_ptr = m_user_ptr[body_index];
return 0;
}
-int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) {
+int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
m_user_int[body_index] = user_int;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) {
+int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
m_user_ptr[body_index] = user_ptr;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const {
+int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_origin = body.m_body_T_world.transpose() * body.m_body_pos;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const {
+int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
- if (body.m_mass > 0) {
+ if (body.m_mass > 0)
+ {
*world_com = body.m_body_T_world.transpose() *
(body.m_body_pos + body.m_body_mass_com / body.m_mass);
- } else {
+ }
+ else
+ {
*world_com = body.m_body_T_world.transpose() * (body.m_body_pos);
}
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const {
+int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_T_body = body.m_body_T_world.transpose();
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const {
+int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index,
- vec3 *world_velocity) const {
+ vec3 *world_velocity) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_velocity = body.m_body_T_world.transpose() * body.m_body_vel;
@@ -862,13 +1065,17 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index,
}
int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index,
- vec3 *world_velocity) const {
+ vec3 *world_velocity) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
vec3 com;
- if (body.m_mass > 0) {
+ if (body.m_mass > 0)
+ {
com = body.m_body_mass_com / body.m_mass;
- } else {
+ }
+ else
+ {
com(0) = 0;
com(1) = 0;
com(2) = 0;
@@ -880,149 +1087,173 @@ int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index,
}
int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index,
- vec3 *world_dot_omega) const {
+ vec3 *world_dot_omega) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index,
- vec3 *world_acceleration) const {
+ vec3 *world_acceleration) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
const RigidBody &body = m_body_list[body_index];
*world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const {
+int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*joint_type = m_body_list[body_index].m_joint_type;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
- const char **joint_type) const {
+ const char **joint_type) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*joint_type = jointTypeToString(m_body_list[body_index].m_joint_type);
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3* r) const{
- CHECK_IF_BODY_INDEX_IS_VALID(body_index);
- *r=m_body_list[body_index].m_parent_pos_parent_body_ref;
- return 0;
+int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3 *r) const
+{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ *r = m_body_list[body_index].m_parent_pos_parent_body_ref;
+ return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T) const{
- CHECK_IF_BODY_INDEX_IS_VALID(body_index);
- *T=m_body_list[body_index].m_body_T_parent_ref;
- return 0;
+int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33 *T) const
+{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ *T = m_body_list[body_index].m_body_T_parent_ref;
+ return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis) const{
- CHECK_IF_BODY_INDEX_IS_VALID(body_index);
- if(m_body_list[body_index].m_joint_type == REVOLUTE) {
- *axis = m_body_list[body_index].m_Jac_JR;
- return 0;
- }
- if(m_body_list[body_index].m_joint_type == PRISMATIC) {
- *axis = m_body_list[body_index].m_Jac_JT;
- return 0;
- }
- setZero(*axis);
- return 0;
+int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
+{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ if (m_body_list[body_index].m_joint_type == REVOLUTE)
+ {
+ *axis = m_body_list[body_index].m_Jac_JR;
+ return 0;
+ }
+ if (m_body_list[body_index].m_joint_type == PRISMATIC)
+ {
+ *axis = m_body_list[body_index].m_Jac_JT;
+ return 0;
+ }
+ setZero(*axis);
+ return 0;
}
-int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const {
+int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*q_index = m_body_list[body_index].m_q_index;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) {
+int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
m_body_list[body_index].m_mass = mass;
return 0;
}
int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index,
- const vec3& first_mass_moment) {
+ const vec3 &first_mass_moment)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
m_body_list[body_index].m_body_mass_com = first_mass_moment;
return 0;
}
int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index,
- const mat33& second_mass_moment) {
+ const mat33 &second_mass_moment)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
m_body_list[body_index].m_body_I_body = second_mass_moment;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const {
+int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*mass = m_body_list[body_index].m_mass;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index,
- vec3 *first_mass_moment) const {
+ vec3 *first_mass_moment) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*first_mass_moment = m_body_list[body_index].m_body_mass_com;
return 0;
}
int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index,
- mat33 *second_mass_moment) const {
+ mat33 *second_mass_moment) const
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*second_mass_moment = m_body_list[body_index].m_body_I_body;
return 0;
}
-void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() {
- for (int index = 0; index < m_num_bodies; index++) {
+void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments()
+{
+ for (int index = 0; index < m_num_bodies; index++)
+ {
RigidBody &body = m_body_list[index];
setZero(body.m_body_force_user);
setZero(body.m_body_moment_user);
}
}
-int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) {
+int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
m_body_list[body_index].m_body_force_user += body_force;
return 0;
}
-int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) {
+int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment)
+{
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
m_body_list[body_index].m_body_moment_user += body_moment;
return 0;
}
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const {
- CHECK_IF_BODY_INDEX_IS_VALID(body_index);
- const RigidBody &body = m_body_list[body_index];
- *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
- return 0;
+int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
+{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
+ return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{
- CHECK_IF_BODY_INDEX_IS_VALID(body_index);
- const RigidBody &body = m_body_list[body_index];
- *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
- return 0;
+int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
+{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
+ return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{
- CHECK_IF_BODY_INDEX_IS_VALID(body_index);
- const RigidBody &body = m_body_list[body_index];
- mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans);
- return 0;
+int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
+{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans);
+ return 0;
}
-int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{
- CHECK_IF_BODY_INDEX_IS_VALID(body_index);
- const RigidBody &body = m_body_list[body_index];
- mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot);
- return 0;
+int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
+{
+ CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+ const RigidBody &body = m_body_list[body_index];
+ mul(body.m_body_T_world.transpose(), body.m_body_Jac_R, world_jac_rot);
+ return 0;
}
#endif
-}
+} // namespace btInverseDynamics
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-11 23:34:26 +0000