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

Expose Jacobian computation to RobotSimAPI.

Browse Source
This commit is contained in:
yunfeibai
2016-09-07 17:37:38 -07:00
parent c198029cb9
commit f635c64205
7 changed files with 28 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -148,6 +148,11 @@ public:
if (numJoints==7)
{
double q_current[7]={0,0,0,0,0,0,0};
double qdot_current[7]={0,0,0,0,0,0,0};
double qddot_current[7]={0,0,0,0,0,0,0};
double local_position[3]={0,0,0};
double jacobian_linear[21];
double jacobian_angular[21];
b3JointStates jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
@@ -158,6 +163,13 @@ public:
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
// compute body Jacobian
m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular);
for (int i = 0; i < 21; ++i) {
printf("j%d: %f\n", i, jacobian_linear[i]);
}
// m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
double q_new[7];
int ikMethod=IK2_SDLS;

11
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -912,3 +912,14 @@ void b3RobotSimAPI::renderScene()
}
m_data->m_physicsServer.renderScene();
}
void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
{
b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClient, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
{
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
}
}

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -115,6 +115,8 @@ public:
void renderScene();
void debugDraw(int debugDrawMode);
void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
};
#endif //B3_ROBOT_SIM_API_H