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

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

View File

@@ -325,11 +325,16 @@ struct b3VisualShapeInformation
///use URDF link frame = link COM frame * inertiaFrame.inverse()
struct b3LinkState
{
//m_worldPosition and m_worldOrientation of the Center Of Mass (COM)
double m_worldPosition[3];
double m_worldOrientation[4];
double m_localInertialPosition[3];
double m_localInertialOrientation[4];
///world position and orientation of the (URDF) link frame
double m_worldLinkFramePosition[3];
double m_worldLinkFrameOrientation[4];
};
//todo: discuss and decide about control mode and combinations