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[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

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);
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);
}

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -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<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges;