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

Add inverse kinematics example with implementations by Sam Buss.

Browse Source 
Uses Kuka IIWA model description and 4 methods:
Selectively Damped Least Squares,Damped Least Squares,
Jacobi Transpose, Jacobi Pseudo Inverse
Tweak some PD values in Inverse Dynamics example and Robot example.
This commit is contained in:
erwin coumans
2016-07-24 22:22:42 -07:00
parent 77b9e1a3e2
commit 75e86051c2
29 changed files with 9926 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@@ -157,7 +157,7 @@ void InverseDynamicsExample::initPhysics()
BulletURDFImporter u2b(m_guiHelper,0);
bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf");
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
if (loadOk)
{
int rootLinkIndex = u2b.getRootLinkIndex();
@@ -225,13 +225,13 @@ void InverseDynamicsExample::initPhysics()
{
qd[dof] = 0;
char tmp[25];
sprintf(tmp,"q_desired[%zu]",dof);
sprintf(tmp,"q_desired[%u]",dof);
qd_name[dof] = tmp;
SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
slider.m_minVal=-3.14;
slider.m_maxVal=3.14;
sprintf(tmp,"q[%zu]",dof);
sprintf(tmp,"q[%u]",dof);
q_name[dof] = tmp;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
btVector4 color = sJointCurveColors[dof&7];
@@ -340,6 +340,21 @@ void InverseDynamicsExample::stepSimulation(float deltaTime)
// todo(thomas) check that this is correct:
// want to advance by 10ms, with 1ms timesteps
m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
m_multiBody->forwardKinematics(scratch_q, scratch_m);
for (int i = 0; i < m_multiBody->getNumLinks(); i++)
{
//btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin();
btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform;
btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector);
btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec;
//printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
}
}
}