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

first part of adding spherical joint support in BulletInverseDynamics

Browse Source
This commit is contained in:
Erwin Coumans
2018-11-27 16:30:58 -08:00
parent 192d27743a
commit ecfe8de506
3 changed files with 119 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/BulletInverseDynamics/MultiBodyTree.hpp
View File

@@ -16,7 +16,9 @@ enum JointType
/// one translational degree of freedom relative to parent /// one translational degree of freedom relative to parent
PRISMATIC, PRISMATIC,
/// six degrees of freedom relative to parent /// six degrees of freedom relative to parent
FLOATING FLOATING,
/// three degrees of freedom, relative to parent
SPHERICAL
}; };
/// Interface class for calculating inverse dynamics for tree structured /// Interface class for calculating inverse dynamics for tree structured
@@ -31,12 +33,14 @@ enum JointType
/// - PRISMATIC: displacement [m] /// - PRISMATIC: displacement [m]
/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] /// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
/// (in that order) /// (in that order)
/// - SPHERICAL: Euler x-y-z angles [rad]
/// The u vector contains the generalized speeds, which are /// The u vector contains the generalized speeds, which are
/// - FIXED: none /// - FIXED: none
/// - REVOLUTE: time derivative of angle of rotation [rad/s] /// - REVOLUTE: time derivative of angle of rotation [rad/s]
/// - PRISMATIC: time derivative of displacement [m/s] /// - PRISMATIC: time derivative of displacement [m/s]
/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) /// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles)
/// and time derivative of displacement in parent frame [m/s] /// and time derivative of displacement in parent frame [m/s]
// - SPHERICAL: angular velocity [rad/s]
/// ///
/// The q and u vectors are obtained by stacking contributions of all bodies in one /// The q and u vectors are obtained by stacking contributions of all bodies in one
/// vector in the order of body indices. /// vector in the order of body indices.
@@ -47,7 +51,7 @@ enum JointType
/// - PRISMATIC: force [N], along joint axis /// - PRISMATIC: force [N], along joint axis
/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame /// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame
/// (in that order) /// (in that order)
/// /// - SPHERICAL: moment vector [Nm]
/// TODO - force element interface (friction, springs, dampers, etc) /// TODO - force element interface (friction, springs, dampers, etc)
/// - gears and motor inertia /// - gears and motor inertia
class MultiBodyTree class MultiBodyTree

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

@@ -35,6 +35,8 @@ const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &typ
return "prismatic"; return "prismatic";
case FLOATING: case FLOATING:
return "floating"; return "floating";
case SPHERICAL:
return "spherical";
} }
return "error: invalid"; return "error: invalid";
} }
@@ -88,6 +90,8 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
return 1; return 1;
case FLOATING: case FLOATING:
return 6; return 6;
case SPHERICAL:
return 3;
} }
bt_id_error_message("unknown joint type %d\n", type); bt_id_error_message("unknown joint type %d\n", type);
return 0; return 0;
@@ -150,6 +154,11 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets()
body.m_q_index = q_index; body.m_q_index = q_index;
q_index += 6; q_index += 6;
break; break;
case SPHERICAL:
m_body_spherical_list.push_back(i);
body.m_q_index = q_index;
q_index += 3;
break;
default: default:
bt_id_error_message("unsupported joint type %d\n", body.m_joint_type); bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
return -1; return -1;
@@ -238,6 +247,16 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData()
case FLOATING: case FLOATING:
// no static data // no static data
break; 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.
@@ -352,6 +371,15 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); (*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; return 0;
} }
@@ -413,7 +441,8 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
RigidBody &body = m_body_list[m_body_floating_list[i]]; RigidBody &body = m_body_list[m_body_floating_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * 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(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(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(2) = q(body.m_q_index + 5);
@@ -444,6 +473,32 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
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) // 2. absolute kinematic quantities (vector valued)
// NOTE: this should be optimized by specializing for different body types // NOTE: this should be optimized by specializing for different body types
@@ -560,6 +615,12 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 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); 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; break;
} }
} }
@@ -608,6 +669,32 @@ int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &
} }
#endif #endif
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) static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
{ {
switch (dof) switch (dof)
@@ -664,6 +751,8 @@ static inline int jointNumDoFs(const JointType &type)
return 1; return 1;
case FLOATING: case FLOATING:
return 6; return 6;
case SPHERICAL:
return 3;
} }
// this should never happen // this should never happen
bt_id_error_message("invalid joint type\n"); bt_id_error_message("invalid joint type\n");
@@ -798,6 +887,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
{ {
setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); 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 = vec3 body_eom_rot =
body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
@@ -810,14 +904,19 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// 1. for multi-dof joints, rest of the dofs of this body // 1. for multi-dof joints, rest of the dofs of this body
for (int row = col - 1; row >= q_index_min; row--) for (int row = col - 1; row >= q_index_min; row--)
{ {
if (FLOATING != body.m_joint_type) if (SPHERICAL == body.m_joint_type)
{ {
bt_id_error_message("??\n"); //todo: review
return -1; 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 // 2. ancestor dofs
int child_idx = i; int child_idx = i;
@@ -839,6 +938,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
vec3 Jac_JT = parent_body.m_Jac_JT; 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 // set jacobians for 6-DoF joints
if (FLOATING == parent_body.m_joint_type) if (FLOATING == parent_body.m_joint_type)
{ {

2
src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
View File

@@ -274,6 +274,8 @@ private:
idArray<int>::type m_body_prismatic_list; idArray<int>::type m_body_prismatic_list;
// Indices of floating joints // Indices of floating joints
idArray<int>::type m_body_floating_list; idArray<int>::type m_body_floating_list;
// Indices of spherical joints
idArray<int>::type m_body_spherical_list;
// a user-provided integer // a user-provided integer
idArray<int>::type m_user_int; idArray<int>::type m_user_int;
// a user-provided pointer // a user-provided pointer