diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 998e045db..a07d6f643 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -203,27 +203,39 @@ public: 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); + // Settings based on default KUKA arm setting + ikargs.m_lowerLimits.resize(numJoints); + ikargs.m_upperLimits.resize(numJoints); + ikargs.m_jointRanges.resize(numJoints); + ikargs.m_restPoses.resize(numJoints); + ikargs.m_lowerLimits[0] = -2.32; + ikargs.m_lowerLimits[1] = -1.6; + ikargs.m_lowerLimits[2] = -2.32; + ikargs.m_lowerLimits[3] = -1.6; + ikargs.m_lowerLimits[4] = -2.32; + ikargs.m_lowerLimits[5] = -1.6; + ikargs.m_lowerLimits[6] = -2.4; + ikargs.m_upperLimits[0] = 2.32; + ikargs.m_upperLimits[1] = 1.6; + ikargs.m_upperLimits[2] = 2.32; + ikargs.m_upperLimits[3] = 1.6; + ikargs.m_upperLimits[4] = 2.32; + ikargs.m_upperLimits[5] = 1.6; + ikargs.m_upperLimits[6] = 2.4; + ikargs.m_jointRanges[0] = 5.8; + ikargs.m_jointRanges[1] = 4; + ikargs.m_jointRanges[2] = 5.8; + ikargs.m_jointRanges[3] = 4; + ikargs.m_jointRanges[4] = 5.8; + ikargs.m_jointRanges[5] = 4; + ikargs.m_jointRanges[6] = 6; + ikargs.m_restPoses[0] = 0; + ikargs.m_restPoses[1] = 0; + ikargs.m_restPoses[2] = 0; + ikargs.m_restPoses[3] = SIMD_HALF_PI; + ikargs.m_restPoses[4] = 0; + ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66; + ikargs.m_restPoses[6] = 0; ikargs.m_numDegreeOfFreedom = numJoints; if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 84edd5198..068bf1687 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -487,13 +487,13 @@ 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) && (args.m_flags & 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]); + 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], &args.m_restPoses[0]); } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) { b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); } 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]); + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); } else { b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index aa9fbd0eb..798b892e0 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -101,6 +101,7 @@ struct b3RobotSimInverseKinematicArgs b3AlignedObjectArray m_lowerLimits; b3AlignedObjectArray m_upperLimits; b3AlignedObjectArray m_jointRanges; + b3AlignedObjectArray m_restPoses; b3RobotSimInverseKinematicArgs() :m_bodyUniqueId(-1), diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 7c9793c7f..89260d47d 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -174,19 +174,17 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], return true; } -bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range) +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); m_data->m_nullSpaceVelocity.SetZero(); double stayCloseToZeroGain = 0.1; double stayAwayFromLimitsGain = 10.0; - - double q_rest[7] = {0, 0, 0, SIMD_HALF_PI, 0, -SIMD_HALF_PI*0.66, 0}; // Stay close to zero for (int i = 0; i < numQ; ++i) { - m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (q_rest[i]-q_current[i]); + m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i]-q_current[i]); } // Stay away from joint limits diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index 762777d84..71f8eeaf4 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -30,7 +30,7 @@ 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 computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range); + bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose); }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 923c98c6d..95babb3a6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1391,7 +1391,7 @@ void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemory } -void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange) +void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1408,10 +1408,11 @@ void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i]; + command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[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) +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) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1433,6 +1434,7 @@ void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHan command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i]; + command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i]; } } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4952afd60..dcd68c7d6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -133,8 +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); +void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); +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); int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index edda7b3be..f07803694 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2688,15 +2688,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray lower_limit; btAlignedObjectArray upper_limit; btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; lower_limit.resize(numDofs); upper_limit.resize(numDofs); joint_range.resize(numDofs); + rest_pose.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]); + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ff66ec13f..adfd533f1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -409,6 +409,7 @@ struct CalculateInverseKinematicsArgs double m_lowerLimit[MAX_DEGREE_OF_FREEDOM]; double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; double m_jointRange[MAX_DEGREE_OF_FREEDOM]; + double m_restPose[MAX_DEGREE_OF_FREEDOM]; }; struct CalculateInverseKinematicsResultArgs