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

few fixes in inverse dynamics

Browse Source
This commit is contained in:
erwincoumans
2018-12-19 20:15:30 -08:00
parent 78de1f070f
commit 8e8c3fb315
5 changed files with 35 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
View File

@@ -202,8 +202,16 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
break;
case btMultibodyLink::eSpherical:
bt_id_error_message("spherical joints not implemented\n");
return -1;
link.joint_type = SPHERICAL;
link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
// random unit vector
link.body_axis_of_motion(0) = 0;
link.body_axis_of_motion(1) = 0;
link.body_axis_of_motion(2) = 1;
break;
case btMultibodyLink::ePlanar:
bt_id_error_message("planar joints not implemented\n");
return -1;

2
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -9756,7 +9756,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
}
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId;
for (int i = 0; i < numDofs; i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];

9
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -134,6 +134,15 @@ public:
return m_baseCollider;
}
const btMultiBodyLinkCollider *getLinkCollider(int index) const
{
if (index >= 0 && index < getNumLinks())
{
return getLink(index).m_collider;
}
return 0;
}
btMultiBodyLinkCollider *getLinkCollider(int index)
{
if (index >= 0 && index < getNumLinks())

12
src/BulletInverseDynamics/MultiBodyTree.cpp
View File

@@ -281,6 +281,8 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
break;
case FLOATING:
break;
case SPHERICAL:
break;
default:
bt_id_error_message("unknown joint type %d\n", joint_type);
return -1;
@@ -437,6 +439,16 @@ int MultiBodyTree::finalize()
rigid_body.m_Jac_JT(1) = 0.0;
rigid_body.m_Jac_JT(2) = 0.0;
break;
case SPHERICAL:
// NOTE/TODO: this is not really correct.
// the Jacobians should be 3x3 matrices here !
rigid_body.m_Jac_JR(0) = 0.0;
rigid_body.m_Jac_JR(1) = 0.0;
rigid_body.m_Jac_JR(2) = 0.0;
rigid_body.m_Jac_JT(0) = 0.0;
rigid_body.m_Jac_JT(1) = 0.0;
rigid_body.m_Jac_JT(2) = 0.0;
break;
case FLOATING:
// NOTE/TODO: this is not really correct.
// the Jacobians should be 3x3 matrices here !

3
src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
View File

@@ -28,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
// does not add a degree of freedom
// m_num_dofs+=0;
break;
case SPHERICAL:
m_num_dofs += 3;
break;
case FLOATING:
m_num_dofs += 6;
break;