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

6
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -78,7 +78,6 @@ public:
m_ikHelper.createKukaIIWA();
bool connected = m_robotSim.connect(m_guiHelper); bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected); b3Printf("robotSim connected = %d",connected);
@@ -114,6 +113,7 @@ public:
*/ */
} }
if (0)
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf"; args.m_fileName = "kiva_shelf/model.sdf";
@@ -195,11 +195,13 @@ public:
ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1]; ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2]; ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION;
ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0]; ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1]; ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2]; ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3]; ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
ikargs.m_dt = dt; ikargs.m_endEffectorLinkIndex = 6;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{ {

15
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -480,12 +480,25 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
*/ */
bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results) bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
{ {
btAssert(args.m_endEffectorLinkIndex>=0);
btAssert(args.m_bodyUniqueId>=0);
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId);
if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
} else
{
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
}
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId, args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation,args.m_dt);
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
int numPos=0; int numPos=0;
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,

13
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -94,15 +94,22 @@ struct b3RobotSimInverseKinematicArgs
// int m_numPositions; // int m_numPositions;
double m_endEffectorTargetPosition[3]; double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[4]; double m_endEffectorTargetOrientation[4];
double m_dt; int m_endEffectorLinkIndex;
int m_flags; int m_flags;
b3RobotSimInverseKinematicArgs() b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1), :m_bodyUniqueId(-1),
// m_currentJointPositions(0), m_endEffectorLinkIndex(-1),
// m_numPositions(0),
m_flags(0) m_flags(0)
{ {
m_endEffectorTargetPosition[0]=0;
m_endEffectorTargetPosition[1]=0;
m_endEffectorTargetPosition[2]=0;
m_endEffectorTargetOrientation[0]=0;
m_endEffectorTargetOrientation[1]=0;
m_endEffectorTargetOrientation[2]=0;
m_endEffectorTargetOrientation[3]=1;
} }
}; };

131
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -16,7 +16,7 @@ struct IKTrajectoryHelperInternalData
{ {
VectorR3 m_endEffectorTargetPosition; VectorR3 m_endEffectorTargetPosition;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes; b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian; Jacobian* m_ikJacobian;
@@ -37,82 +37,22 @@ IKTrajectoryHelper::~IKTrajectoryHelper()
delete m_data; 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], bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4], const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3], const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4], const double endEffectorWorldOrientation[4],
const double* q_current, int numQ, const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt) 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;
if (numQ != 7) m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ);
{
return false; // Reset(m_ikTree,m_ikJacobian);
}
m_data->m_ikJacobian->Reset();
for (int i=0;i<numQ;i++)
{
m_data->m_ikNodes[i]->SetTheta(q_current[i]);
}
bool UseJacobianTargets1 = false; bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) { if ( UseJacobianTargets1 ) {
@@ -129,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3); VectorRn deltaS(3);
for (int i = 0; i < 3; ++i) 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 // Set one end effector world orientation from Bullet
@@ -139,35 +79,49 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse(); btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle(); float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis(); btVector3 axis = deltaQ.getAxis();
float angleDot = angle/dt; float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize(); btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
deltaR.Set(i,angularVel[i]); deltaR.Set(i,angularVel[i]);
} }
{
if (useAngularPart)
{
VectorRn deltaC(6); VectorRn deltaC(6);
MatrixRmn completeJacobian(6,numQ);
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
deltaC.Set(i,deltaS[i]); deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[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->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]);
}
}
m_data->m_ikJacobian->SetJendTrans(completeJacobian); 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);
}
}
// Calculate the change in theta values // Calculate the change in theta values
switch (ikMethod) { switch (ikMethod) {
@@ -175,6 +129,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break; break;
case IK2_DLS: case IK2_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break; break;
case IK2_DLS_SVD: case IK2_DLS_SVD:
@@ -186,9 +142,6 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_SDLS: case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break; break;
case IK2_VEL_DLS:
m_data->m_ikJacobian->CalcThetasDotDLS(dt); // Damped least square for velocity IK
break;
default: default:
m_data->m_ikJacobian->ZeroDeltaThetas(); m_data->m_ikJacobian->ZeroDeltaThetas();
break; break;
@@ -206,7 +159,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
for (int i=0;i<numQ;i++) for (int i=0;i<numQ;i++)
{ {
// Use for velocity IK // 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 // Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta(); //q_new[i] = m_data->m_ikNodes[i]->GetTheta();

11
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -8,7 +8,8 @@ enum IK2_Method
IK2_DLS, IK2_DLS,
IK2_SDLS , IK2_SDLS ,
IK2_DLS_SVD, IK2_DLS_SVD,
IK2_VEL_DLS IK2_VEL_DLS,
IK2_VEL_DLS_WITH_ORIENTATION,
}; };
@@ -20,17 +21,13 @@ public:
IKTrajectoryHelper(); IKTrajectoryHelper();
virtual ~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], bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4], const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3], const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4], const double endEffectorWorldOrientation[4],
const double* q_old, int numQ, const double* q_old, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt); double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size);
}; };
#endif //IK_TRAJECTORY_HELPER_H #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 ///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; PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl); b3Assert(cl);
@@ -1330,12 +1330,32 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS; command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0; command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex; command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
//todo
// int numJoints = cl->getNumJoints(bodyIndex); return (b3SharedMemoryCommandHandle)command;
// for (int i = 0; i < numJoints;i++)
// { }
// command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
// } 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[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; 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[1] = targetOrientation[1];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
command->m_calculateInverseKinematicsArguments.m_dt = dt;
return (b3SharedMemoryCommandHandle)command;
} }
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,
int* dofCount, 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); int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics ///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 b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,
int* dofCount, int* dofCount,

57
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

7
examples/SharedMemory/SharedMemoryCommands.h
View File

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

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

@@ -52,10 +52,11 @@ const double Jacobian::BaseMaxTargetDist = 0.4;
Jacobian::Jacobian(Tree* tree) Jacobian::Jacobian(Tree* tree)
{ {
Jacobian::tree = tree; m_tree = tree;
nEffector = tree->GetNumEffector(); m_nEffector = tree->GetNumEffector();
nJoint = tree->GetNumJoint(); nJoint = tree->GetNumJoint();
nRow = 2 * 3 * nEffector; // Include both linear and angular part nRow = 3 * m_nEffector; // Include only the linear part
nCol = nJoint; nCol = nJoint;
Jend.SetSize(nRow, nCol); // The Jocobian matrix Jend.SetSize(nRow, nCol); // The Jocobian matrix
@@ -77,9 +78,52 @@ Jacobian::Jacobian(Tree* tree)
// Used by the Selectively Damped Least Squares Method // Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow); //dT.SetLength(nRow);
dSclamp.SetLength(nEffector); dSclamp.SetLength(m_nEffector);
errorArray.SetLength(nEffector); errorArray.SetLength(m_nEffector);
Jnorms.SetSize(nEffector, nCol); // Holds the norms of the active J matrix Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
Jacobian::Jacobian(bool useAngularJacobian,int nDof)
{
m_tree = 0;
m_nEffector = 1;
if (useAngularJacobian)
{
nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
} else
{
nRow = 3 * m_nEffector; // Include only the linear part
}
nCol = nDof;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetZero();
SetJendActive();
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w .SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dPreTheta.SetLength(nCol);
// Used by Jacobian transpose method & DLS & SDLS
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
// Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow);
dSclamp.SetLength(m_nEffector);
errorArray.SetLength(m_nEffector);
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset(); Reset();
} }
@@ -98,9 +142,12 @@ void Jacobian::Reset()
// Compute the J and K matrices (the Jacobians) // Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian(VectorR3* targets) void Jacobian::ComputeJacobian(VectorR3* targets)
{ {
if (m_tree==0)
return;
// Traverse tree to find all end effectors // Traverse tree to find all end effectors
VectorR3 temp; VectorR3 temp;
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsEffector() ) { if ( n->IsEffector() ) {
int i = n->GetEffectorNum(); int i = n->GetEffectorNum();
@@ -113,10 +160,10 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
// Find all ancestors (they will usually all be joints) // Find all ancestors (they will usually all be joints)
// Set the corresponding entries in the Jacobians J, K. // Set the corresponding entries in the Jacobians J, K.
Node* m = tree->GetParent(n); Node* m = m_tree->GetParent(n);
while ( m ) { while ( m ) {
int j = m->GetJointNum(); int j = m->GetJointNum();
assert ( 0 <=i && i<nEffector && 0<=j && j<nJoint ); assert ( 0 <=i && i<m_nEffector && 0<=j && j<nJoint );
if ( m->IsFrozen() ) { if ( m->IsFrozen() ) {
Jend.SetTriple(i, j, VectorR3::Zero); Jend.SetTriple(i, j, VectorR3::Zero);
Jtarget.SetTriple(i, j, VectorR3::Zero); Jtarget.SetTriple(i, j, VectorR3::Zero);
@@ -131,10 +178,10 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
temp *= m->GetW(); // cross product with joint rotation axis temp *= m->GetW(); // cross product with joint rotation axis
Jtarget.SetTriple(i, j, temp); Jtarget.SetTriple(i, j, temp);
} }
m = tree->GetParent( m ); m = m_tree->GetParent( m );
} }
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
} }
@@ -156,36 +203,39 @@ void Jacobian::UpdateThetas()
{ {
// Traverse the tree to find all joints // Traverse the tree to find all joints
// Update the joint angles // Update the joint angles
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsJoint() ) { if ( n->IsJoint() ) {
int i = n->GetJointNum(); int i = n->GetJointNum();
n->AddToTheta( dTheta[i] ); n->AddToTheta( dTheta[i] );
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
// Update the positions and rotation axes of all joints/effectors // Update the positions and rotation axes of all joints/effectors
tree->Compute(); m_tree->Compute();
} }
void Jacobian::UpdateThetaDot() void Jacobian::UpdateThetaDot()
{ {
if (m_tree==0)
return;
// Traverse the tree to find all joints // Traverse the tree to find all joints
// Update the joint angles // Update the joint angles
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsJoint() ) { if ( n->IsJoint() ) {
int i = n->GetJointNum(); int i = n->GetJointNum();
n->UpdateTheta( dTheta[i] ); n->UpdateTheta( dTheta[i] );
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
// Update the positions and rotation axes of all joints/effectors // Update the positions and rotation axes of all joints/effectors
tree->Compute(); m_tree->Compute();
} }
void Jacobian::CalcDeltaThetas() void Jacobian::CalcDeltaThetas()
@@ -279,7 +329,7 @@ void Jacobian::CalcDeltaThetasDLS()
// Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e. // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
// CalcdTClampedFromdS(); // CalcdTClampedFromdS();
// VectorRn dTextra(3*nEffector); // VectorRn dTextra(3*m_nEffector);
// U.Solve( dT, &dTextra ); // U.Solve( dT, &dTextra );
// J.MultiplyTranspose( dTextra, dTheta ); // J.MultiplyTranspose( dTextra, dTheta );
@@ -294,31 +344,6 @@ 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() void Jacobian::CalcDeltaThetasDLSwithSVD()
{ {
const MatrixRmn& J = ActiveJacobian(); const MatrixRmn& J = ActiveJacobian();
@@ -366,7 +391,7 @@ void Jacobian::CalcDeltaThetasSDLS()
// Calculate response vector dTheta that is the SDLS solution. // Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values // Delta target values are the dS values
int nRows = J.GetNumRows(); int nRows = J.GetNumRows();
int numEndEffectors = tree->GetNumEffector(); // Equals the number of rows of J divided by three int numEndEffectors = m_tree->GetNumEffector(); // Equals the number of rows of J divided by three
int nCols = J.GetNumColumns(); int nCols = J.GetNumColumns();
dTheta.SetZero(); dTheta.SetZero();
@@ -478,7 +503,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
// Traverse tree to find all end effectors // Traverse tree to find all end effectors
VectorR3 temp; VectorR3 temp;
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsEffector() ) { if ( n->IsEffector() ) {
int i = n->GetEffectorNum(); int i = n->GetEffectorNum();
@@ -489,7 +514,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
errorArray[i] = err; errorArray[i] = err;
totalError += err; totalError += err;
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
return totalError; return totalError;
} }
@@ -498,7 +523,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
{ {
// Traverse tree to find all end effectors // Traverse tree to find all end effectors
VectorR3 temp; VectorR3 temp;
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsEffector() ) { if ( n->IsEffector() ) {
int i = n->GetEffectorNum(); int i = n->GetEffectorNum();
@@ -517,7 +542,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
dSclamp[i] = BaseMaxTargetDist; dSclamp[i] = BaseMaxTargetDist;
} }
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
} }

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

@@ -54,6 +54,7 @@ enum UpdateMode {
class Jacobian { class Jacobian {
public: public:
Jacobian(Tree*); Jacobian(Tree*);
Jacobian(bool useAngularJacobian, int nDof);
void ComputeJacobian(VectorR3* targets); void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; } const MatrixRmn& ActiveJacobian() const { return *Jactive; }
@@ -69,7 +70,7 @@ public:
void CalcDeltaThetasDLS(); void CalcDeltaThetasDLS();
void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS(); void CalcDeltaThetasSDLS();
void CalcThetasDotDLS(float dt);
void UpdateThetas(); void UpdateThetas();
void UpdateThetaDot(); void UpdateThetaDot();
@@ -90,8 +91,8 @@ public:
int GetNumCols() {return nCol;} int GetNumCols() {return nCol;}
public: public:
Tree* tree; // tree associated with this Jacobian matrix Tree* m_tree; // tree associated with this Jacobian matrix
int nEffector; // Number of end effectors int m_nEffector; // Number of end effectors
int nJoint; // Number of joints int nJoint; // Number of joints
int nRow; // Total number of rows the real J (= 3*number of end effectors for now) int nRow; // Total number of rows the real J (= 3*number of end effectors for now)
int nCol; // Total number of columns in the real J (= number of joints for now) int nCol; // Total number of columns in the real J (= number of joints for now)

33
examples/pybullet/pybullet.c
View File

@@ -1018,6 +1018,24 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
return 0; return 0;
} }
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
int size = PySequence_Size(args); int size = PySequence_Size(args);
int objectUniqueIdA = -1; int objectUniqueIdA = -1;
@@ -1658,22 +1676,25 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
if (size == 2) if (size == 2)
{ {
int bodyIndex; int bodyIndex;
PyObject* targetPosObj; int endEffectorLinkIndex;
if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetPosObj)) PyObject* targetPosObj;
PyObject* targetOrnObj;
if (PyArg_ParseTuple(args, "iiOO", &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj))
{ {
double pos[3]; double pos[3];
double ori[4]={0,1.0,0,0}; double ori[4]={0,1.0,0,0};
double dt=0.0001;
if (pybullet_internalSetVectord(targetPosObj,pos)) if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4(targetOrnObj,ori))
{ {
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int numPos=0; int numPos=0;
int resultBodyIndex; int resultBodyIndex;
int result; int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex,
pos,ori,dt); b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex);
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle, result = b3GetStatusInverseKinematicsJointPositions(statusHandle,