Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[InverseDynamics] Support for Jacobians & derivatives

Browse Source 
This change adds support for calculating Jacobians and
dot(Jacobian)*u terms, along with the required support for
the 3xN matrices in the standalone math library.
It also adds functions to compute kinematics only (position, velocity, accel).
To facilitate tests, the Cl also adds a RandomTreeCreator to create
randomized multibody trees.
Thanks to Thomas Buschmann for this contribution!
This commit is contained in:
Erwin Coumans
2016-08-25 16:24:28 -07:00
parent 7db9a020b9
commit ba8964c4ac
25 changed files with 1474 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files

465
src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
View File

@@ -3,7 +3,15 @@
namespace btInverseDynamics {
MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
: m_num_bodies(num_bodies_), m_num_dofs(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)
#endif
{
#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
resize(m_m3x,m_num_dofs);
#endif
m_body_list.resize(num_bodies_);
m_parent_index.resize(num_bodies_);
m_child_indices.resize(num_bodies_);
@@ -205,6 +213,20 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData() {
// no static data
break;
}
// 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 //
}
}
@@ -218,131 +240,12 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
return -1;
}
// 1. update relative kinematics
// 1.1 for revolute
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;
// body.m_parent_r_parent_body= fixed
body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
// body.m_parent_dot_r_rel = fixed;
// NOTE: this assumes that Jac_JR is constant, which is true for revolute
// joints, but not in the general case (eg, slider-crank type mechanisms)
body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
// body.m_parent_ddot_r_rel = fixed;
}
// 1.2 for prismatic
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 =
body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
// body.m_parent_omega_rel = 0;
body.m_parent_vel_rel =
body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
// body.parent_dot_omega_rel = 0;
// NOTE: this assumes that Jac_JT is constant, which is true for
// prismatic joints, but not in the general case
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++) {
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));
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_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_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_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
}
// 3. absolute kinematic quantities & dynamic quantities
// NOTE: this should be optimized by specializing for different body types
// (e.g., relative rotation is always zero for prismatic joints, etc.)
// calculations for root body
{
RigidBody &body = m_body_list[0];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts)
// not required right now, added here for debugging purposes
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;
// 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;
// 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++) {
RigidBody &body = m_body_list[i];
RigidBody &parent = m_body_list[m_parent_index[i]];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts) not required right now added here for debugging purposes
body.m_body_pos =
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;
// 3.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);
// 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_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);
}
// 1. relative kinematics
if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) {
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)
@@ -356,7 +259,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
body.m_body_force_user;
}
// 4. calculate full set of forces at parent joint
// 3. calculate full set of forces at parent joint
// (not directly calculating the joint force along the free direction
// simplifies inclusion of fixed joints.
// An alternative would be to fuse bodies in a pre-processing step,
@@ -416,6 +319,239 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
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 ) {
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) {
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++) {
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);
}
}
// 1.2 for prismatic
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);
}
}
// 1.3 fixed joints: nothing to do
// 1.4 6dof joints:
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));
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);
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_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;
}
}
// 2. absolute kinematic quantities (vector valued)
// NOTE: this should be optimized by specializing for different body types
// (e.g., relative rotation is always zero for prismatic joints, etc.)
// calculations for root body
{
RigidBody &body = m_body_list[0];
// 3.1 update absolute positions and orientations:
// will be required if we add force elements (eg springs between bodies,
// or contacts)
// not required right now, added here for debugging purposes
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;
}
}
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:
// will be required if we add force elements (eg springs between bodies,
// or contacts) not required right now added here for debugging purposes
body.m_body_pos =
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);
}
}
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;
}
}
int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
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) {
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) {
// rotational part
@@ -487,13 +623,6 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// 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.
// Macro for setting matrix elements depending on underlying math library.
#ifdef ID_LINEAR_MATH_USE_BULLET
#define setMassMatrixElem(row, col, val) mass_matrix->setElem(row, col, val)
#else
#define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val
#endif
if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
mass_matrix->cols() != m_num_dofs) {
error_message("Dimension error. System has %d DOFs,\n"
@@ -507,7 +636,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
if (initialize_matrix) {
for (int i = 0; i < m_num_dofs; i++) {
for (int j = 0; j < m_num_dofs; j++) {
setMassMatrixElem(i, j, 0.0);
setMatxxElem(i, j, 0.0, mass_matrix);
}
}
}
@@ -597,7 +726,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
vec3 body_eom_trans =
body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans));
setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
// rest of the mass matrix column upwards
{
@@ -609,7 +738,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
}
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
setMassMatrixElem(col, row, Mrc);
setMatxxElem(col, row, Mrc, mass_matrix);
}
// 2. ancestor dofs
int child_idx = i;
@@ -634,7 +763,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
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);
setMassMatrixElem(col, row, Mrc);
setMatxxElem(col, row, Mrc, mass_matrix);
}
child_idx = parent_idx;
@@ -647,12 +776,10 @@ 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++) {
setMassMatrixElem(row, col, (*mass_matrix)(col, row));
setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
}
}
}
#undef setMassMatrixElem
return 0;
}
@@ -779,6 +906,32 @@ int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
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::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 {
CHECK_IF_BODY_INDEX_IS_VALID(body_index);
*q_index = m_body_list[body_index].m_q_index;
@@ -841,4 +994,34 @@ int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3
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::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::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
}