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

Expose IK with null space task to shared memory API and RobotSimAPI.

Browse Source
This commit is contained in:
yunfeibai
2016-09-30 00:05:00 -07:00
parent 29f890ae10
commit fd3cb061cb
9 changed files with 124 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -195,14 +195,37 @@ 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_flags |= B3_HAS_IK_TARGET_ORIENTATION + B3_HAS_NULL_SPACE_VELOCITY;
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_endEffectorLinkIndex = 6; ikargs.m_endEffectorLinkIndex = 6;
ikargs.m_lowerLimits.push_back(-2.32);
ikargs.m_lowerLimits.push_back(-1.6);
ikargs.m_lowerLimits.push_back(-2.32);
ikargs.m_lowerLimits.push_back(-1.6);
ikargs.m_lowerLimits.push_back(-2.32);
ikargs.m_lowerLimits.push_back(-1.6);
ikargs.m_lowerLimits.push_back(-2.4);
ikargs.m_upperLimits.push_back(2.32);
ikargs.m_upperLimits.push_back(1.6);
ikargs.m_upperLimits.push_back(2.32);
ikargs.m_upperLimits.push_back(1.6);
ikargs.m_upperLimits.push_back(2.32);
ikargs.m_upperLimits.push_back(1.6);
ikargs.m_upperLimits.push_back(2.4);
ikargs.m_jointRanges.push_back(5.8);
ikargs.m_jointRanges.push_back(4);
ikargs.m_jointRanges.push_back(5.8);
ikargs.m_jointRanges.push_back(4);
ikargs.m_jointRanges.push_back(5.8);
ikargs.m_jointRanges.push_back(4);
ikargs.m_jointRanges.push_back(6);
ikargs.m_numDegreeOfFreedom = numJoints;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{ {
//copy the IK result to the desired state of the motor/actuator //copy the IK result to the desired state of the motor/actuator

10
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -485,10 +485,16 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId);
if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) if (args.m_flags & (B3_HAS_IK_TARGET_ORIENTATION + B3_HAS_NULL_SPACE_VELOCITY))
{
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]);
} else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{ {
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
} else } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
{
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]);
} else
{ {
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
} }

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -85,6 +85,7 @@ struct b3JointMotorArgs
enum b3InverseKinematicsFlags enum b3InverseKinematicsFlags
{ {
B3_HAS_IK_TARGET_ORIENTATION=1, B3_HAS_IK_TARGET_ORIENTATION=1,
B3_HAS_NULL_SPACE_VELOCITY=2,
}; };
struct b3RobotSimInverseKinematicArgs struct b3RobotSimInverseKinematicArgs
@@ -96,6 +97,7 @@ struct b3RobotSimInverseKinematicArgs
double m_endEffectorTargetOrientation[4]; double m_endEffectorTargetOrientation[4];
int m_endEffectorLinkIndex; int m_endEffectorLinkIndex;
int m_flags; int m_flags;
int m_numDegreeOfFreedom;
b3AlignedObjectArray<double> m_lowerLimits; b3AlignedObjectArray<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits; b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges; b3AlignedObjectArray<double> m_jointRanges;

3
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -46,7 +46,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,int endEffectorIndex, const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]) double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{ {
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false; bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ); m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ);
@@ -133,6 +133,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break; break;
case IK2_VEL_DLS_WITH_NULLSPACE: case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength()==numQ); assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break; break;

1
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -11,6 +11,7 @@ enum IK2_Method
IK2_VEL_DLS, IK2_VEL_DLS,
IK2_VEL_DLS_WITH_ORIENTATION, IK2_VEL_DLS_WITH_ORIENTATION,
IK2_VEL_DLS_WITH_NULLSPACE, IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
}; };

45
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1391,6 +1391,51 @@ void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemory
} }
void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange)
{
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_NULL_SPACE_VELOCITY;
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];
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
}
}
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)
{
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+IK_HAS_NULL_SPACE_VELOCITY;
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_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];
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
}
}
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -133,6 +133,8 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]); void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]);
void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]); void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange);
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);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,
int* dofCount, int* dofCount,

35
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2665,7 +2665,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> q_new; btAlignedObjectArray<double> q_new;
q_new.resize(numDofs); q_new.resize(numDofs);
int ikMethod= (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)? IK2_VEL_DLS_WITH_ORIENTATION : IK2_VEL_DLS; int ikMethod = 0;
if (clientCmd.m_updateFlags& (IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY))
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
ikMethod = IK2_VEL_DLS;
}
if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
for (int i = 0; i < numDofs; ++i)
{
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0]);
}
}
btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation; btVector3DoubleData endEffectorWorldOrientation;

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -395,7 +395,8 @@ enum EnumCalculateInverseKinematicsFlags
{ {
IK_HAS_TARGET_POSITION=1, IK_HAS_TARGET_POSITION=1,
IK_HAS_TARGET_ORIENTATION=2, IK_HAS_TARGET_ORIENTATION=2,
//IK_HAS_CURRENT_JOINT_POSITIONS=4,//not used yet IK_HAS_NULL_SPACE_VELOCITY=4,
//IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet
}; };
struct CalculateInverseKinematicsArgs struct CalculateInverseKinematicsArgs
@@ -405,6 +406,9 @@ struct CalculateInverseKinematicsArgs
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
int m_endEffectorLinkIndex; int m_endEffectorLinkIndex;
double m_lowerLimit[MAX_DEGREE_OF_FREEDOM];
double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
double m_jointRange[MAX_DEGREE_OF_FREEDOM];
}; };
struct CalculateInverseKinematicsResultArgs struct CalculateInverseKinematicsResultArgs