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

Expose rest pose for null space task to API.

Browse Source
This commit is contained in:
yunfeibai
2016-09-30 01:03:40 -07:00
parent 1c1d3db26d
commit 7e8d8b0488
9 changed files with 50 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -203,27 +203,39 @@ public:
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); // Settings based on default KUKA arm setting
ikargs.m_lowerLimits.push_back(-1.6); ikargs.m_lowerLimits.resize(numJoints);
ikargs.m_lowerLimits.push_back(-2.32); ikargs.m_upperLimits.resize(numJoints);
ikargs.m_lowerLimits.push_back(-1.6); ikargs.m_jointRanges.resize(numJoints);
ikargs.m_lowerLimits.push_back(-2.32); ikargs.m_restPoses.resize(numJoints);
ikargs.m_lowerLimits.push_back(-1.6); ikargs.m_lowerLimits[0] = -2.32;
ikargs.m_lowerLimits.push_back(-2.4); ikargs.m_lowerLimits[1] = -1.6;
ikargs.m_upperLimits.push_back(2.32); ikargs.m_lowerLimits[2] = -2.32;
ikargs.m_upperLimits.push_back(1.6); ikargs.m_lowerLimits[3] = -1.6;
ikargs.m_upperLimits.push_back(2.32); ikargs.m_lowerLimits[4] = -2.32;
ikargs.m_upperLimits.push_back(1.6); ikargs.m_lowerLimits[5] = -1.6;
ikargs.m_upperLimits.push_back(2.32); ikargs.m_lowerLimits[6] = -2.4;
ikargs.m_upperLimits.push_back(1.6); ikargs.m_upperLimits[0] = 2.32;
ikargs.m_upperLimits.push_back(2.4); ikargs.m_upperLimits[1] = 1.6;
ikargs.m_jointRanges.push_back(5.8); ikargs.m_upperLimits[2] = 2.32;
ikargs.m_jointRanges.push_back(4); ikargs.m_upperLimits[3] = 1.6;
ikargs.m_jointRanges.push_back(5.8); ikargs.m_upperLimits[4] = 2.32;
ikargs.m_jointRanges.push_back(4); ikargs.m_upperLimits[5] = 1.6;
ikargs.m_jointRanges.push_back(5.8); ikargs.m_upperLimits[6] = 2.4;
ikargs.m_jointRanges.push_back(4); ikargs.m_jointRanges[0] = 5.8;
ikargs.m_jointRanges.push_back(6); 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; ikargs.m_numDegreeOfFreedom = numJoints;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))

4
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -487,13 +487,13 @@ 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) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) 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) } 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 if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) } 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 } else
{ {
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);

1
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -101,6 +101,7 @@ struct b3RobotSimInverseKinematicArgs
b3AlignedObjectArray<double> m_lowerLimits; b3AlignedObjectArray<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits; b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges; b3AlignedObjectArray<double> m_jointRanges;
b3AlignedObjectArray<double> m_restPoses;
b3RobotSimInverseKinematicArgs() b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1), :m_bodyUniqueId(-1),

6
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -174,19 +174,17 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
return true; 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.SetLength(numQ);
m_data->m_nullSpaceVelocity.SetZero(); m_data->m_nullSpaceVelocity.SetZero();
double stayCloseToZeroGain = 0.1; double stayCloseToZeroGain = 0.1;
double stayAwayFromLimitsGain = 10.0; 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 // Stay close to zero
for (int i = 0; i < numQ; ++i) 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 // Stay away from joint limits

2
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -30,7 +30,7 @@ public:
const double* q_old, int numQ, int endEffectorIndex, 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]); 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 #endif //IK_TRAJECTORY_HELPER_H

6
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
@@ -1408,10 +1408,11 @@ void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[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; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
@@ -1433,6 +1434,7 @@ void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHan
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i]; command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
} }
} }

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -133,8 +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 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); 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 b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,
int* dofCount, int* dofCount,

5
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2688,15 +2688,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> lower_limit; btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit; btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range; btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs); lower_limit.resize(numDofs);
upper_limit.resize(numDofs); upper_limit.resize(numDofs);
joint_range.resize(numDofs); joint_range.resize(numDofs);
rest_pose.resize(numDofs);
for (int i = 0; i < numDofs; ++i) for (int i = 0; i < numDofs; ++i)
{ {
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[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]);
} }
} }

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -409,6 +409,7 @@ struct CalculateInverseKinematicsArgs
double m_lowerLimit[MAX_DEGREE_OF_FREEDOM]; double m_lowerLimit[MAX_DEGREE_OF_FREEDOM];
double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
double m_jointRange[MAX_DEGREE_OF_FREEDOM]; double m_jointRange[MAX_DEGREE_OF_FREEDOM];
double m_restPose[MAX_DEGREE_OF_FREEDOM];
}; };
struct CalculateInverseKinematicsResultArgs struct CalculateInverseKinematicsResultArgs