AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/RoboticsLearning/IKTrajectoryHelper.cpp
View File

@ -86,6 +86,7 @@ void IKTrajectoryHelper::createKukaIIWA()
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size)
{
@ -111,6 +112,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Set end effector world position from Bullet
// Set Jacobian from Bullet body Jacobian
int nRow = m_data->m_ikJacobian->GetNumRows();
int nCol = m_data->m_ikJacobian->GetNumCols();

1
examples/RoboticsLearning/IKTrajectoryHelper.h
View File

@ -23,6 +23,7 @@ public:
void createKukaIIWA();
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double* q_old, int numQ,
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size);

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]);

15
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@ -923,3 +923,18 @@ void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const doubl
b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
}
}
void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition)
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3LinkState linkState;
b3GetLinkState(m_data->m_physicsClient, statusHandle, linkIndex, &linkState);
worldPosition[0] = linkState.m_worldPosition[0];
worldPosition[1] = linkState.m_worldPosition[1];
worldPosition[2] = linkState.m_worldPosition[2];
}
}

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@ -117,6 +117,8 @@ public:
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);
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition);
};
#endif //B3_ROBOT_SIM_API_H