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

Merge pull request #795 from YunfeiBai/master

Browse Source 
Inverse kinematics with orientation constraint.
This commit is contained in:
erwincoumans
2016-09-20 17:49:35 -07:00
committed by GitHub
parent 8d4c99801e c01f6245d0
commit cca2236f1a
13 changed files with 160 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -27,6 +27,8 @@ class KukaGraspExample : public CommonExampleInterface
int m_targetSphereInstance;
b3Vector3 m_targetPos;
b3Vector3 m_worldPos;
b3Vector4 m_targetOri;
b3Vector4 m_worldOri;
double m_time;
int m_options;
@@ -145,7 +147,7 @@ public:
m_time+=dt;
m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
m_targetOri.setValue(0, 1.0, 0, 0);
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
@@ -155,6 +157,7 @@ public:
double q_current[7]={0,0,0,0,0,0,0};
double world_position[3]={0,0,0};
double world_orientation[4]={0,0,0,0};
b3JointStates jointStates;
if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
@@ -166,14 +169,19 @@ public:
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
// compute body position
m_robotSim.getLinkState(0, 6, world_position);
// compute body position and orientation
m_robotSim.getLinkState(0, 6, world_position, world_orientation);
m_worldPos.setValue(world_position[0], world_position[1], world_position[2]);
m_worldOri.setValue(world_orientation[0], world_orientation[1], world_orientation[2], world_orientation[3]);
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
b3Vector3DoubleData targetPosDataOut;
m_targetPos.serializeDouble(targetPosDataOut);
b3Vector3DoubleData worldPosDataOut;
m_worldPos.serializeDouble(worldPosDataOut);
b3Vector3DoubleData targetOriDataOut;
m_targetOri.serializeDouble(targetOriDataOut);
b3Vector3DoubleData worldOriDataOut;
m_worldOri.serializeDouble(worldOriDataOut);
b3RobotSimInverseKinematicArgs ikargs;
@@ -183,15 +191,15 @@ public:
ikargs.m_bodyUniqueId = m_kukaIndex;
// ikargs.m_currentJointPositions = q_current;
// ikargs.m_numPositions = 7;
ikargs.m_endEffectorTargetPosition[0] = dataOut.m_floats[0];
ikargs.m_endEffectorTargetPosition[1] = dataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = dataOut.m_floats[2];
ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0];
ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
//todo: orientation IK target
// ikargs.m_endEffectorTargetOrientation[0] = 0;
// ikargs.m_endEffectorTargetOrientation[1] = 0;
// ikargs.m_endEffectorTargetOrientation[2] = 0;
// ikargs.m_endEffectorTargetOrientation[3] = 1;
ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
ikargs.m_dt = dt;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{

9
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -481,8 +481,7 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
{
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId,
args.m_endEffectorTargetPosition);
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId, args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation,args.m_dt);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
@@ -961,7 +960,7 @@ void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const doubl
}
}
void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition)
void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation)
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
@@ -973,5 +972,9 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP
worldPosition[0] = linkState.m_worldPosition[0];
worldPosition[1] = linkState.m_worldPosition[1];
worldPosition[2] = linkState.m_worldPosition[2];
worldOrientation[0] = linkState.m_worldOrientation[0];
worldOrientation[1] = linkState.m_worldOrientation[1];
worldOrientation[2] = linkState.m_worldOrientation[2];
worldOrientation[3] = linkState.m_worldOrientation[3];
}
}

5
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -93,7 +93,8 @@ struct b3RobotSimInverseKinematicArgs
// double* m_currentJointPositions;
// int m_numPositions;
double m_endEffectorTargetPosition[3];
// double m_endEffectorTargetOrientation[4];
double m_endEffectorTargetOrientation[4];
double m_dt;
int m_flags;
b3RobotSimInverseKinematicArgs()
@@ -149,7 +150,7 @@ public:
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);
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
};
#endif //B3_ROBOT_SIM_API_H

60
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -97,9 +97,11 @@ void IKTrajectoryHelper::createKukaIIWA()
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobian1, int jacobian_size1)
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt)
{
if (numQ != 7)
@@ -127,26 +129,45 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,endEffectorTargetPosition[i]-endEffectorWorldPosition[i]);
deltaS.Set(i,(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])/dt);
}
m_data->m_ikJacobian->SetDeltaS(deltaS);
// Set one end effector world orientation from Bullet
VectorRn deltaR(3);
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle/dt;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,angularVel[i]);
}
VectorRn deltaC(6);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[i]);
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
// Set Jacobian from Bullet body Jacobian
int nRow = m_data->m_ikJacobian->GetNumRows();
int nCol = m_data->m_ikJacobian->GetNumCols();
if (jacobian_size1)
{
b3Assert(jacobian_size1==nRow*nCol);
MatrixRmn linearJacobian(nRow,nCol);
for (int i = 0; i < nRow; ++i)
b3Assert(jacobian_size==nRow*nCol);
MatrixRmn completeJacobian(nRow,nCol);
for (int i = 0; i < nRow/2; ++i)
{
for (int j = 0; j < nCol; ++j)
{
linearJacobian.Set(i,j,linear_jacobian1[i*nCol+j]);
completeJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
completeJacobian.Set(i+nRow/2,j,angular_jacobian[i*nCol+j]);
}
}
m_data->m_ikJacobian->SetJendTrans(linearJacobian);
}
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
// Calculate the change in theta values
switch (ikMethod) {
@@ -165,19 +186,30 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
case IK2_VEL_DLS:
m_data->m_ikJacobian->CalcThetasDotDLS(dt); // Damped least square for velocity IK
break;
default:
m_data->m_ikJacobian->ZeroDeltaThetas();
break;
}
m_data->m_ikJacobian->UpdateThetas();
// Use for velocity IK, update theta dot
m_data->m_ikJacobian->UpdateThetaDot();
// Use for position IK, incrementally update theta
//m_data->m_ikJacobian->UpdateThetas();
// Apply the change in the theta values
m_data->m_ikJacobian->UpdatedSClampValue(&targets);
//m_data->m_ikJacobian->UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
q_new[i] = m_data->m_ikNodes[i]->GetTheta();
// Use for velocity IK
q_new[i] = m_data->m_ikNodes[i]->GetTheta()*dt + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}

7
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -7,7 +7,8 @@ enum IK2_Method
IK2_PURE_PSEUDO,
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD
IK2_DLS_SVD,
IK2_VEL_DLS
};
@@ -25,9 +26,11 @@ public:
bool createFromMultiBody(class btMultiBody* mb);
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_old, int numQ,
double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size);
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt);
};
#endif //IK_TRAJECTORY_HELPER_H

9
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1319,8 +1319,7 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
}
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double targetPosition[3])
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -1342,6 +1341,12 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
command->m_calculateInverseKinematicsArguments.m_dt = dt;
return (b3SharedMemoryCommandHandle)command;
}

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -121,8 +121,7 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle
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 targetPosition[3]);
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,

18
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2577,6 +2577,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*7);
b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*7);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
@@ -2606,12 +2608,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
btInverseDynamics::mat3x jac_r(3,num_dofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < num_dofs; ++j)
{
jacobian_linear[i*num_dofs+j] = jac_t(i,j);
jacobian_angular[i*num_dofs+j] = jac_r(i,j);
}
}
}
@@ -2619,19 +2624,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
double q_new[7];
int ikMethod=IK2_DLS;
int ikMethod=IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition,
endEffectorWorldPosition.m_floats,
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
q_current,
numJoints, q_new, ikMethod, &jacobian_linear[0],jacSize);
numJoints, q_new, ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2,clientCmd.m_calculateInverseKinematicsArguments.m_dt);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numJoints;i++)

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -402,7 +402,8 @@ struct CalculateInverseKinematicsArgs
int m_bodyUniqueId;
// double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_targetPosition[3];
// double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
double m_dt;
};
struct CalculateInverseKinematicsResultArgs

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

@@ -55,7 +55,7 @@ Jacobian::Jacobian(Tree* tree)
Jacobian::tree = tree;
nEffector = tree->GetNumEffector();
nJoint = tree->GetNumJoint();
nRow = 3 * nEffector;
nRow = 2 * 3 * nEffector; // Include both linear and angular part
nCol = nJoint;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
@@ -170,6 +170,24 @@ void Jacobian::UpdateThetas()
tree->Compute();
}
void Jacobian::UpdateThetaDot()
{
// Traverse the tree to find all joints
// Update the joint angles
Node* n = tree->GetRoot();
while ( n ) {
if ( n->IsJoint() ) {
int i = n->GetJointNum();
n->UpdateTheta( dTheta[i] );
}
n = tree->GetSuccessor( n );
}
// Update the positions and rotation axes of all joints/effectors
tree->Compute();
}
void Jacobian::CalcDeltaThetas()
{
switch (CurrentUpdateMode) {
@@ -276,6 +294,31 @@ void Jacobian::CalcDeltaThetasDLS()
}
}
void Jacobian::CalcThetasDotDLS(float dt)
{
const MatrixRmn& J = ActiveJacobian();
MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
U.AddToDiagonal( DampingLambdaSq );
// Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
// CalcdTClampedFromdS();
// VectorRn dTextra(3*nEffector);
// U.Solve( dT, &dTextra );
// J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method
U.Solve( dS, &dT1 );
J.MultiplyTranspose( dT1, dTheta );
// Scale back to not exceed maximum angle changes
double MaxVelDLS = MaxAngleDLS/dt;
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxVelDLS ) {
dTheta *= MaxVelDLS/maxChange;
}
}
void Jacobian::CalcDeltaThetasDLSwithSVD()
{
const MatrixRmn& J = ActiveJacobian();

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

@@ -69,8 +69,10 @@ public:
void CalcDeltaThetasDLS();
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
void CalcThetasDotDLS(float dt);
void UpdateThetas();
void UpdateThetaDot();
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
const VectorRn& GetErrorArray() const { return errorArray; }
void UpdatedSClampValue(VectorR3* targets);

4
examples/ThirdPartyLibs/BussIK/Node.h
View File

@@ -57,6 +57,10 @@ public:
#endif
return theta; }
double UpdateTheta( double& delta ) {
theta = delta;
return theta; }
const VectorR3& GetS() const { return s; }
const VectorR3& GetW() const { return w; }

4
examples/pybullet/pybullet.c
View File

@@ -1663,6 +1663,8 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetPosObj))
{
double pos[3];
double ori[4]={0,1.0,0,0};
double dt=0.0001;
if (pybullet_internalSetVectord(targetPosObj,pos))
{
@@ -1671,7 +1673,7 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
int resultBodyIndex;
int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex,
pos);
pos,ori,dt);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,