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

Expose link state in RobotSimAPI.

Browse Source
This commit is contained in:
yunfeibai
2016-09-10 18:48:57 -07:00
parent c94a8e0d35
commit 1b72b91bcf
5 changed files with 30 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -26,6 +26,7 @@ class KukaGraspExample : public CommonExampleInterface
IKTrajectoryHelper m_ikHelper;
int m_targetSphereInstance;
b3Vector3 m_targetPos;
b3Vector3 m_worldPos;
double m_time;
int m_options;
@@ -45,6 +46,7 @@ public:
m_time(0)
{
m_targetPos.setValue(0.5,0,1);
m_worldPos.setValue(0, 0, 0);
m_app->setUpAxis(2);
}
virtual ~KukaGraspExample()
@@ -153,6 +155,7 @@ public:
double local_position[3]={0,0,0};
double jacobian_linear[21];
double jacobian_angular[21];
double world_position[3]={0,0,0};
b3JointStates jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
@@ -164,6 +167,9 @@ public:
}
}
// compute body position
m_robotSim.getLinkState(0, 6, world_position);
m_worldPos.setValue(world_position[0], world_position[1], world_position[2]);
// compute body Jacobian
m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular);
@@ -172,7 +178,9 @@ public:
int ikMethod=IK2_SDLS;
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod,jacobian_linear,(sizeof(jacobian_linear)/sizeof(*jacobian_linear)));
b3Vector3DoubleData worldPosDataOut;
m_worldPos.serializeDouble(worldPosDataOut);
m_ikHelper.computeIK(dataOut.m_floats,worldPosDataOut.m_floats,q_current, numJoints, q_new, ikMethod,jacobian_linear,(sizeof(jacobian_linear)/sizeof(*jacobian_linear)));
printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
q_new[3],q_new[4],q_new[5],q_new[6]);