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

pybullet.calculateInverseKinematics: expose null space method with and without orientation

Browse Source 
add inverse_kinematics.py and hello_pybullet.py pybullet examples
add m_worldLinkFramePosition/Orientation fields to b3LinkState, and in pybullet.getLinkState (URDF link frame in Cartesian/world coordinates)
This commit is contained in:
Erwin Coumans
2017-01-11 21:39:22 -08:00
parent 3d6584962a
commit 4897139dad
5 changed files with 223 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3,6 +3,7 @@
#include "Bullet3Common/b3Scalar.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Common/b3Transform.h"
#include <string.h>
#include "SharedMemoryCommands.h"
@@ -534,6 +535,10 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
b3Assert(bodyIndex>=0);
if (bodyIndex>=0)
{
b3Transform wlf,com,inertial;
for (int i = 0; i < 3; ++i)
{
state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
@@ -544,6 +549,20 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
}
com.setOrigin(b3MakeVector3(state->m_worldPosition[0],state->m_worldPosition[1],state->m_worldPosition[2]));
com.setRotation(b3Quaternion(state->m_worldOrientation[0],state->m_worldOrientation[1],state->m_worldOrientation[2],state->m_worldOrientation[3]));
inertial.setOrigin(b3MakeVector3(state->m_localInertialPosition[0],state->m_localInertialPosition[1],state->m_localInertialPosition[2]));
inertial.setRotation(b3Quaternion(state->m_localInertialOrientation[0],state->m_localInertialOrientation[1],state->m_localInertialOrientation[2],state->m_localInertialOrientation[3]));
wlf = com*inertial.inverse();
for (int i = 0; i < 3; ++i)
{
state->m_worldLinkFramePosition[i] = wlf.getOrigin()[i];
}
b3Quaternion wlfOrn = wlf.getRotation();
for (int i = 0; i < 4; ++i)
{
state->m_worldLinkFrameOrientation[i] = wlfOrn[i];
}
}
}