From fd3cb061cbc59d5fd669c557bfd59b8438841b94 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 30 Sep 2016 00:05:00 -0700 Subject: [PATCH] Expose IK with null space task to shared memory API and RobotSimAPI. --- .../RoboticsLearning/KukaGraspExample.cpp | 27 ++++++++++- examples/RoboticsLearning/b3RobotSimAPI.cpp | 10 ++++- examples/RoboticsLearning/b3RobotSimAPI.h | 2 + examples/SharedMemory/IKTrajectoryHelper.cpp | 3 +- examples/SharedMemory/IKTrajectoryHelper.h | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 45 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 35 ++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 6 ++- 9 files changed, 124 insertions(+), 7 deletions(-) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index fa49f3e2e..e3c71c738 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -195,14 +195,37 @@ public: ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1]; 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[1] = targetOriDataOut.m_floats[1]; ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2]; ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3]; 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)) { //copy the IK result to the desired state of the motor/actuator diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 1a9d9d2c2..e11103dcf 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -485,10 +485,16 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin 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); - } 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); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index bd650b33b..aa9fbd0eb 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -85,6 +85,7 @@ struct b3JointMotorArgs enum b3InverseKinematicsFlags { B3_HAS_IK_TARGET_ORIENTATION=1, + B3_HAS_NULL_SPACE_VELOCITY=2, }; struct b3RobotSimInverseKinematicArgs @@ -96,6 +97,7 @@ struct b3RobotSimInverseKinematicArgs double m_endEffectorTargetOrientation[4]; int m_endEffectorLinkIndex; int m_flags; + int m_numDegreeOfFreedom; b3AlignedObjectArray m_lowerLimits; b3AlignedObjectArray m_upperLimits; b3AlignedObjectArray m_jointRanges; diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 113aa84ae..8ae7e64b1 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -46,7 +46,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], 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]) { - 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); @@ -133,6 +133,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method break; case IK2_VEL_DLS_WITH_NULLSPACE: + case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: assert(m_data->m_nullSpaceVelocity.GetLength()==numQ); m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); break; diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index 76145e122..762777d84 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -11,6 +11,7 @@ enum IK2_Method IK2_VEL_DLS, IK2_VEL_DLS_WITH_ORIENTATION, IK2_VEL_DLS_WITH_NULLSPACE, + IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE, }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a9f7cc56f..923c98c6d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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* bodyUniqueId, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a998ddea4..4952afd60 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -133,6 +133,8 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ 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]); +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* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index de0da7109..9f3160895 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2665,7 +2665,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray q_new; 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 lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray 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 endEffectorWorldOrientation; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index faefc25f5..ff66ec13f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -395,7 +395,8 @@ enum EnumCalculateInverseKinematicsFlags { IK_HAS_TARGET_POSITION=1, 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 @@ -405,6 +406,9 @@ struct CalculateInverseKinematicsArgs double m_targetPosition[3]; double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w 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