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

fix InverseKinematics+API for a case without tree (custom build Jacobian)

Browse Source
This commit is contained in:
erwin coumans
2016-09-22 13:27:09 -07:00
parent 46b32f17bb
commit 310a330572
12 changed files with 251 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files

143
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -16,7 +16,7 @@ struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
@@ -37,82 +37,22 @@ IKTrajectoryHelper::~IKTrajectoryHelper()
delete m_data;
}
bool IKTrajectoryHelper::createFromMultiBody(class btMultiBody* mb)
{
//todo: implement proper conversion. For now, we only 'detect' a likely KUKA iiwa and hardcode its creation
if (mb->getNumLinks()==7)
{
createKukaIIWA();
return true;
}
return false;
}
void IKTrajectoryHelper::createKukaIIWA()
{
const VectorR3& unitx = VectorR3::UnitX;
const VectorR3& unity = VectorR3::UnitY;
const VectorR3& unitz = VectorR3::UnitZ;
const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
const VectorR3& zero = VectorR3::Zero;
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_data->m_ikNodes.resize(8);//7DOF+additional endeffector
m_data->m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
m_data->m_ikTree.InsertRoot(m_data->m_ikNodes[0]);
m_data->m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[0], m_data->m_ikNodes[1]);
m_data->m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[1], m_data->m_ikNodes[2]);
m_data->m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[2], m_data->m_ikNodes[3]);
m_data->m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[3], m_data->m_ikNodes[4]);
m_data->m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[4], m_data->m_ikNodes[5]);
m_data->m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[5], m_data->m_ikNodes[6]);
m_data->m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[6], m_data->m_ikNodes[7]);
m_data->m_ikJacobian = new Jacobian(&m_data->m_ikTree);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikTree.Init();
m_data->m_ikTree.Compute();
m_data->m_ikJacobian->Reset();
}
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_jacobian, const double* angular_jacobian, int jacobian_size, float dt)
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size)
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false;
m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikJacobian->Reset();
if (numQ != 7)
{
return false;
}
for (int i=0;i<numQ;i++)
{
m_data->m_ikNodes[i]->SetTheta(q_current[i]);
}
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
@@ -129,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])/dt);
deltaS.Set(i,(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
@@ -139,35 +79,49 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle/dt;
float angleDot = angle;
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();
b3Assert(jacobian_size==nRow*nCol);
MatrixRmn completeJacobian(nRow,nCol);
for (int i = 0; i < nRow/2; ++i)
{
for (int j = 0; j < nCol; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
completeJacobian.Set(i+nRow/2,j,angular_jacobian[i*nCol+j]);
}
if (useAngularPart)
{
VectorRn deltaC(6);
MatrixRmn completeJacobian(6,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
}
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
} else
{
VectorRn deltaC(3);
MatrixRmn completeJacobian(3,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
}
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
}
}
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
// Calculate the change in theta values
switch (ikMethod) {
@@ -175,6 +129,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_DLS_SVD:
@@ -186,9 +142,6 @@ 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;
@@ -206,7 +159,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
for (int i=0;i<numQ;i++)
{
// Use for velocity IK
q_new[i] = m_data->m_ikNodes[i]->GetTheta()*dt + q_current[i];
q_new[i] = m_data->m_ikJacobian->dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();

13
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -8,7 +8,8 @@ enum IK2_Method
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD,
IK2_VEL_DLS
IK2_VEL_DLS,
IK2_VEL_DLS_WITH_ORIENTATION,
};
@@ -20,17 +21,13 @@ public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
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, const double* angular_jacobian, int jacobian_size, float dt);
const double* q_old, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size);
};
#endif //IK_TRAJECTORY_HELPER_H

38
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1319,7 +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], const double targetOrientation[4], const double dt)
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -1330,12 +1330,32 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
//todo
// int numJoints = cl->getNumJoints(bodyIndex);
// for (int i = 0; i < numJoints;i++)
// {
// command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
// }
return (b3SharedMemoryCommandHandle)command;
}
void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
}
void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
@@ -1345,12 +1365,10 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
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;
}
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

5
examples/SharedMemory/PhysicsClientC_API.h
View File

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

61
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2551,7 +2551,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
@@ -2565,41 +2565,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody))
{
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
} else
{
delete tmpHelper;
}
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
}
//todo: make this generic. Right now, only support/tested KUKA iiwa
int numJoints = 7;
int endEffectorLinkIndex = 6;
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
if (ikHelperPtr && bodyHandle->m_multiBody->getNumLinks()==numJoints)
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
int numJoints1 = bodyHandle->m_multiBody->getNumLinks();
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*7);
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*7);
jacobian_angular.resize(3*numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
double q_current[7];
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
if (tree)
{
jacSize = jacobian_linear.size();
// Set jacobian value
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++)
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
@@ -2613,24 +2612,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
btInverseDynamics::mat3x jac_r(3,num_dofs);
btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
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)
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[i*num_dofs+j] = jac_t(i,j);
jacobian_angular[i*num_dofs+j] = jac_r(i,j);
jacobian_linear[i*numDofs+j] = jac_t(i,j);
jacobian_angular[i*numDofs+j] = jac_r(i,j);
}
}
}
}
double q_new[7];
int ikMethod=IK2_VEL_DLS;
btAlignedObjectArray<double> q_new;
q_new.resize(numDofs);
int ikMethod= (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)? IK2_VEL_DLS_WITH_ORIENTATION : IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
@@ -2644,15 +2644,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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], &jacobian_angular[0], jacSize*2,clientCmd.m_calculateInverseKinematicsArguments.m_dt);
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numJoints;i++)
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numJoints;
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
}

7
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -393,8 +393,9 @@ struct CalculateJacobianResultArgs
enum EnumCalculateInverseKinematicsFlags
{
IK_HAS_TARGET_ORIENTATION=1,
IK_HAS_CURRENT_JOINT_POSITIONS=2,
IK_HAS_TARGET_POSITION=1,
IK_HAS_TARGET_ORIENTATION=2,
//IK_HAS_CURRENT_JOINT_POSITIONS=4,//not used yet
};
struct CalculateInverseKinematicsArgs
@@ -403,7 +404,7 @@ struct CalculateInverseKinematicsArgs
// 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_dt;
int m_endEffectorLinkIndex;
};
struct CalculateInverseKinematicsResultArgs