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

Merge pull request #2322 from erwincoumans/master

Browse Source 
PyBullet.calculateInverseKinematics2 for IK with multiple end-effector locations
This commit is contained in:
erwincoumans
2019-07-10 19:58:17 -07:00
committed by GitHub
parent 67cab069c3 ee9575167d
commit d220101c5a
11 changed files with 549 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -46,7 +46,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
{
bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ);
Jacobian ikJacobian(useAngularPart, numQ, 1);
ikJacobian.Reset();
@@ -185,6 +185,115 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
return true;
}
bool IKTrajectoryHelper::computeIK2(
const double* endEffectorTargetPositions,
const double* endEffectorCurrentPositions,
int numEndEffectors,
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6])
{
bool useAngularPart = false;//for now (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors);
ikJacobian.Reset();
bool UseJacobianTargets1 = false;
if (UseJacobianTargets1)
{
ikJacobian.SetJtargetActive();
}
else
{
ikJacobian.SetJendActive();
}
VectorRn deltaC(numEndEffectors *3);
MatrixRmn completeJacobian(numEndEffectors*3, numQ);
for (int ne = 0; ne < numEndEffectors; ne++)
{
VectorR3 targets;
targets.Set(endEffectorTargetPositions[ne*3+0], endEffectorTargetPositions[ne * 3 + 1], endEffectorTargetPositions[ne * 3 + 2]);
// Set one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i, dampIk[i] * (endEffectorTargetPositions[ne*3+i] - endEffectorCurrentPositions[ne*3+i]));
}
{
for (int i = 0; i < 3; ++i)
{
deltaC.Set(ne*3+i, deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(ne * 3 + i, j, linear_jacobians[(ne*3+i * numQ) + j]);
}
}
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
// Calculate the change in theta values
switch (ikMethod)
{
case IK2_JACOB_TRANS:
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
case IK2_VEL_SDLS:
case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
ikJacobian.ZeroDeltaThetas();
break;
}
// Use for velocity IK, update theta dot
//ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta
//ikJacobian.UpdateThetas();
// Apply the change in the theta values
//ikJacobian.UpdatedSClampValue(&targets);
for (int i = 0; i < numQ; i++)
{
// Use for velocity IK
q_new[i] = ikJacobian.dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}
bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
{
m_data->m_nullSpaceVelocity.SetLength(numQ);

7
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -31,6 +31,13 @@ public:
const double* q_old, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
bool computeIK2(
const double* endEffectorTargetPositions,
const double* endEffectorCurrentPositions,
int numEndEffectors,
const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6]);
bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
bool setDampingCoeff(int numQ, const double* coeff);
};

59
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -4905,28 +4905,51 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem
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];
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
}
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle, int numEndEffectorLinkIndices, const int* endEffectorIndices, const double* targetPositions)
{
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_numEndEffectorLinkIndices = numEndEffectorLinkIndices;
for (int i = 0; i < numEndEffectorLinkIndices; i++)
{
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[i] = endEffectorIndices[i];
command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 0] = targetPositions[i * 3 + 0];
command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 1] = targetPositions[i * 3 + 1];
command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 2] = targetPositions[i * 3 + 2];
}
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
}
B3_SHARED_API 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_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
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_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
@@ -4940,11 +4963,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemor
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
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_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
for (int i = 0; i < numDof; ++i)
{
@@ -4961,11 +4985,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMe
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION + IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
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_targetPositions[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -424,6 +424,7 @@ extern "C"
///compute the joint positions to move the end effector to a desired target using inverse kinematics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle, int numEndEffectorLinkIndices, const int* endEffectorIndices, const double* targetPositions);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]);
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);

180
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -10227,34 +10227,10 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
ikHelperPtr = tmpHelper;
}
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
btAlignedObjectArray<double> startingPositions;
startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]);
btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]);
btTransform targetBaseCoord;
if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
{
targetBaseCoord.setOrigin(targetPosWorld);
targetBaseCoord.setRotation(targetOrnWorld);
}
else
{
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
targetBaseCoord = tr.inverse() * targetWorld;
}
{
int DofIndex = 0;
@@ -10290,6 +10266,9 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
}
btScalar currentDiff = 1e30f;
b3AlignedObjectArray<double> endEffectorTargetWorldPositions;
b3AlignedObjectArray<double> endEffectorTargetWorldOrientations;
b3AlignedObjectArray<double> endEffectorCurrentWorldPositions;
b3AlignedObjectArray<double> jacobian_linear;
b3AlignedObjectArray<double> jacobian_angular;
btAlignedObjectArray<double> q_current;
@@ -10302,13 +10281,64 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
btInverseDynamics::vecx nu(numDofs + baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
endEffectorTargetWorldPositions.resize(0);
endEffectorTargetWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3);
endEffectorTargetWorldOrientations.resize(0);
endEffectorTargetWorldOrientations.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 4);
bool validEndEffectorLinkIndices = true;
for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
validEndEffectorLinkIndices = validEndEffectorLinkIndices && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks());
btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne*3+0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 2]);
btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 2],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 3]);
btTransform targetBaseCoord;
if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
{
targetBaseCoord.setOrigin(targetPosWorld);
targetBaseCoord.setRotation(targetOrnWorld);
}
else
{
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
targetBaseCoord = tr.inverse() * targetWorld;
}
btVector3DoubleData targetPosBaseCoord;
btQuaternionDoubleData targetOrnBaseCoord;
targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[0]);
endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[1]);
endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[2]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[0]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[1]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[2]);
endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[3]);
}
for (int i = 0; i < numIterations && currentDiff > residualThreshold; i++)
{
BT_PROFILE("InverseKinematics1Step");
if (ikHelperPtr && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks()))
if (ikHelperPtr && validEndEffectorLinkIndices)
{
jacobian_linear.resize(3 * numDofs);
jacobian_angular.resize(3 * numDofs);
jacobian_linear.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices*3 * numDofs);
jacobian_angular.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices*3 * numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
@@ -10329,7 +10359,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
{
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = startingPositions[DofIndex];
q_current[DofIndex] = curPos;
q[DofIndex + baseDofs] = curPos;
@@ -10348,21 +10377,44 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex + 1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex + 1, &jac_r);
//calculatePositionKinematics is already done inside calculateInverseDynamics
tree->getBodyOrigin(endEffectorLinkIndex + 1, &world_origin);
tree->getBodyTransform(endEffectorLinkIndex + 1, &world_rot);
for (int i = 0; i < 3; ++i)
currentDiff = 0;
endEffectorCurrentWorldPositions.resize(0);
endEffectorCurrentWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3);
for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{
for (int j = 0; j < numDofs; ++j)
int endEffectorLinkIndex2 = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex2 + 1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex2 + 1, &jac_r);
//calculatePositionKinematics is already done inside calculateInverseDynamics
tree->getBodyOrigin(endEffectorLinkIndex2 + 1, &world_origin);
tree->getBodyTransform(endEffectorLinkIndex2 + 1, &world_rot);
for (int i = 0; i < 3; ++i)
{
jacobian_linear[i * numDofs + j] = jac_t(i, (baseDofs + j));
jacobian_angular[i * numDofs + j] = jac_r(i, (baseDofs + j));
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[(ne*3+i) * numDofs + j] = jac_t(i, (baseDofs + j));
jacobian_angular[(ne * 3 + i) * numDofs + j] = jac_r(i, (baseDofs + j));
}
}
endEffectorCurrentWorldPositions.push_back(world_origin[0]);
endEffectorCurrentWorldPositions.push_back(world_origin[1]);
endEffectorCurrentWorldPositions.push_back(world_origin[2]);
btInverseDynamics::vec3 targetPos(btVector3(endEffectorTargetWorldPositions[ne*3+0],
endEffectorTargetWorldPositions[ne * 3 + 1],
endEffectorTargetWorldPositions[ne * 3 + 2]));
//diff
currentDiff = btMax(currentDiff, (world_origin - targetPos).length());
}
}
}
@@ -10451,13 +10503,8 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
//diff
currentDiff = (endEffectorBaseCoord.getOrigin() - targetBaseCoord.getOrigin()).length();
btVector3DoubleData targetPosBaseCoord;
btQuaternionDoubleData targetOrnBaseCoord;
targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
@@ -10477,25 +10524,48 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
double targetDampCoeff[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
bool performedIK = false;
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices==1)
{
BT_PROFILE("computeIK");
ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
ikHelperPtr->computeIK(&endEffectorTargetWorldPositions[0],
&endEffectorTargetWorldOrientations[0],
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0],
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
performedIK = true;
}
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i = 0; i < numDofs; i++)
else
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices>1)
{
ikHelperPtr->computeIK2(&endEffectorTargetWorldPositions[0],
&endEffectorCurrentWorldPositions[0],
clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices,
//endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs,
&q_new[0], ikMethod, &jacobian_linear[0], targetDampCoeff);
performedIK = true;
}
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
for (int i = 0; i < numDofs; i++)
if (performedIK)
{
startingPositions[i] = q_new[i];
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i = 0; i < numDofs; i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
for (int i = 0; i < numDofs; i++)
{
startingPositions[i] = q_new[i];
}
}
}
}

7
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -30,7 +30,7 @@ typedef unsigned long long int smUint64_t;
#endif
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 128
#define MAX_DEGREE_OF_FREEDOM 128
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_SDF_FILENAME_LENGTH 1024
@@ -740,9 +740,10 @@ struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;
// double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_targetPosition[3];
double m_targetPositions[MAX_DEGREE_OF_FREEDOM*3];
int m_numEndEffectorLinkIndices;
double m_targetOrientation[4]; //orientation represented as quaternion, x,y,z,w
int m_endEffectorLinkIndex;
int m_endEffectorLinkIndices[MAX_DEGREE_OF_FREEDOM];
double m_lowerLimit[MAX_DEGREE_OF_FREEDOM];
double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
double m_jointRange[MAX_DEGREE_OF_FREEDOM];