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

Merge pull request #800 from bulletphysics/iktest

Browse Source 
Iktest
This commit is contained in:
erwincoumans
2016-09-23 07:40:09 -07:00
committed by GitHub
parent 3df9d6c4ee b6bb937dc0
commit 666c8f47b7
19 changed files with 635 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -78,7 +78,6 @@ public:
m_ikHelper.createKukaIIWA();
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
@@ -114,6 +113,7 @@ public:
*/
}
if (0)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
@@ -195,11 +195,13 @@ 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_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_dt = dt;
ikargs.m_endEffectorLinkIndex = 6;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{
@@ -208,8 +210,10 @@ public:
{
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
t.m_maxTorqueValue = 1000;
t.m_maxTorqueValue = 100;
t.m_kp= 1;
t.m_targetVelocity = 0;
t.m_kp = 0.5;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
}

15
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -480,12 +480,25 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
*/
bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
{
btAssert(args.m_endEffectorLinkIndex>=0);
btAssert(args.m_bodyUniqueId>=0);
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId, args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation,args.m_dt);
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId);
if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
} else
{
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
}
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
int numPos=0;
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,

13
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -94,15 +94,22 @@ struct b3RobotSimInverseKinematicArgs
// int m_numPositions;
double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[4];
double m_dt;
int m_endEffectorLinkIndex;
int m_flags;
b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1),
// m_currentJointPositions(0),
// m_numPositions(0),
m_endEffectorLinkIndex(-1),
m_flags(0)
{
m_endEffectorTargetPosition[0]=0;
m_endEffectorTargetPosition[1]=0;
m_endEffectorTargetPosition[2]=0;
m_endEffectorTargetOrientation[0]=0;
m_endEffectorTargetOrientation[1]=0;
m_endEffectorTargetOrientation[2]=0;
m_endEffectorTargetOrientation[3]=1;
}
};