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

Merge pull request #775 from YunfeiBai/master

Browse Source 
Compute IK with Bullet body Jacobian
This commit is contained in:
erwincoumans
2016-09-12 12:45:05 +01:00
committed by GitHub
parent 81749e7065 ae2d256f57
commit af7c44d360
12 changed files with 251 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
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()
@@ -151,6 +153,12 @@ 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];
double world_position[3]={0,0,0};
b3JointStates jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
{
@@ -161,10 +169,13 @@ public:
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
// 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);
// m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
/*
b3RobotSimInverseKinematicArgs ikargs;
@@ -189,14 +200,14 @@ public:
}
*/
double q_new[7];
int ikMethod=IK2_SDLS;
m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod);
// printf("q_current = %f,%f,%f,%f,%f,%f,%f\n", q_current[0],q_current[1],q_current[2],
// q_current[3],q_current[4],q_current[5],q_current[6]);
// 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]);
int ikMethod=IK2_DLS;
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
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]);
//set the
for (int i=0;i<numJoints;i++)

26
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -939,3 +939,29 @@ 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);
}
}
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];
}
}

4
examples/RoboticsLearning/b3RobotSimAPI.h
View File

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

26
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -3,6 +3,7 @@
#include "BussIK/Tree.h"
#include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h"
#include "BussIK/MatrixRmn.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
@@ -96,8 +97,9 @@ 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)
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size)
{
if (numQ != 7)
@@ -121,6 +123,28 @@ 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 one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,endEffectorTargetPosition[i]-endEffectorWorldPosition[i]);
}
m_data->m_ikJacobian->SetDeltaS(deltaS);
// Set Jacobian from Bullet body Jacobian
int nRow = m_data->m_ikJacobian->GetNumRows();
int nCol = m_data->m_ikJacobian->GetNumCols();
b3Assert(jacobian_size==nRow*nCol);
MatrixRmn linearJacobian(nRow,nCol);
for (int i = 0; i < nRow; ++i)
{
for (int j = 0; j < nCol; ++j)
{
linearJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
}
}
m_data->m_ikJacobian->SetJendTrans(linearJacobian);
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:

3
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -25,8 +25,9 @@ public:
bool createFromMultiBody(class btMultiBody* mb);
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double* q_old, int numQ,
double* q_new, int ikMethod);
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size);
};
#endif //IK_TRAJECTORY_HELPER_H

53
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1266,6 +1266,57 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl
return true;
}
b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_JACOBIAN;
command->m_updateFlags = 0;
command->m_calculateJacobianArguments.m_bodyUniqueId = bodyIndex;
command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
int numJoints = cl->getNumJoints(bodyIndex);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
command->m_calculateJacobianArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateJacobianArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
return (b3SharedMemoryCommandHandle)command;
}
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
return false;
if (linearJacobian)
{
for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)
{
linearJacobian[i] = status->m_jacobianResultArgs.m_linearJacobian[i];
}
}
if (angularJacobian)
{
for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)
{
angularJacobian[i] = status->m_jacobianResultArgs.m_angularJacobian[i];
}
}
return true;
}
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
@@ -1322,4 +1373,4 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
}
return false;
}
}

7
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -116,6 +116,10 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl
int* dofCount,
double* jointForces);
b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double targetPosition[3]);
@@ -125,9 +129,6 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
int* dofCount,
double* jointPositions);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead

77
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2344,7 +2344,82 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_CALCULATE_JACOBIAN:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree** treePtrPtr =
m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
btInverseDynamics::MultiBodyTree* tree = 0;
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
if (treePtrPtr)
{
tree = *treePtrPtr;
}
else
{
btInverseDynamics::btMultiBodyTreeCreator id_creator;
if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
{
b3Error("error creating tree\n");
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
}
else
{
tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
}
}
if (tree)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
for (int i = 0; i < num_dofs; i++)
{
q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i];
qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i];
nu[i+baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_jacobianResultArgs.m_dofCount = num_dofs;
// Set jacobian value
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex, &jac_t);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < num_dofs; ++j)
{
serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j);
}
}
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED;
}
else
{
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
}
}
}
else
{
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
}
hasStatus = true;
break;
}
case CMD_APPLY_EXTERNAL_FORCE:
{
if (m_data->m_verboseOutput)

20
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -374,6 +374,23 @@ struct CalculateInverseDynamicsResultArgs
double m_jointForces[MAX_DEGREE_OF_FREEDOM];
};
struct CalculateJacobianArgs
{
int m_bodyUniqueId;
int m_linkIndex;
double m_localPosition[3];
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
};
struct CalculateJacobianResultArgs
{
int m_dofCount;
double m_linearJacobian[3*MAX_DEGREE_OF_FREEDOM];
double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
};
struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;
@@ -389,7 +406,6 @@ struct CalculateInverseKinematicsResultArgs
double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
};
struct CreateJointArgs
{
int m_parentBodyIndex;
@@ -429,6 +445,7 @@ struct SharedMemoryCommand
struct PickBodyArgs m_pickBodyArguments;
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
@@ -463,6 +480,7 @@ struct SharedMemoryStatus
struct SendPixelDataArgs m_sendPixelDataArguments;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
};

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -29,6 +29,7 @@ enum EnumSharedMemoryClientCommand
CMD_APPLY_EXTERNAL_FORCE,
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_CALCULATE_INVERSE_KINEMATICS,
CMD_CALCULATE_JACOBIAN,
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
//don't go beyond this command!
@@ -67,6 +68,8 @@ enum EnumSharedMemoryServerStatus
CMD_INVALID_STATUS,
CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
CMD_CALCULATED_JACOBIAN_COMPLETED,
CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,

11
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -138,6 +138,17 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
}
}
void Jacobian::SetJendTrans(MatrixRmn& J)
{
Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
Jend.LoadAsSubmatrix(J);
}
void Jacobian::SetDeltaS(VectorRn& S)
{
dS.Set(S);
}
// The delta theta values have been computed in dTheta array
// Apply the delta theta values to the joints
// Nothing is done about joint limits for now.

7
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -59,6 +59,8 @@ public:
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
void SetJtargetActive() { Jactive = &Jtarget; }
void SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S);
void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
void ZeroDeltaThetas();
@@ -82,7 +84,10 @@ public:
static void CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 );
static void CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies );
private:
int GetNumRows() {return nRow;}
int GetNumCols() {return nCol;}
public:
Tree* tree; // tree associated with this Jacobian matrix
int nEffector; // Number of end effectors
int nJoint; // Number of joints