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

66
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_size==nRow*nCol);
MatrixRmn completeJacobian(nRow,nCol);
for (int i = 0; i < nRow/2; ++i)
{
b3Assert(jacobian_size1==nRow*nCol);
MatrixRmn linearJacobian(nRow,nCol);
for (int i = 0; i < nRow; ++i)
for (int j = 0; j < nCol; ++j)
{
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);
@@ -1341,6 +1340,12 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
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