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

Create project file for BussIK inverse kinematics library (premake, cmake)

Browse Source 
URDF/SDF: add a flag to force concave mesh collisiofor static objects. <collision concave="yes" name="pod_collision">
VR: support teleporting using buttong, allow multiple controllers to be used, fast wireframe rendering,
Turn off warnings about deprecated C routine in btScalar.h/b3Scalar.h
Add a dummy return to stop a warning
Expose defaultContactERP in shared memory api/pybullet.
First start to expose IK in shared memory api/pybullet (not working yet)
This commit is contained in:
erwin coumans
2016-09-08 15:15:58 -07:00
parent 630fcda38b
commit 32eccdff61
43 changed files with 791 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
examples/RoboticsLearning/IKTrajectoryHelper.cpp
View File

@@ -1,145 +0,0 @@
#include "IKTrajectoryHelper.h"
#include "BussIK/Node.h"
#include "BussIK/Tree.h"
#include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#define RADIAN(X) ((X)*RadiansToDegrees)
//use BussIK and Reflexxes to convert from Cartesian endeffector future target to
//joint space positions at each real-time (simulation) step
struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
}
};
IKTrajectoryHelper::IKTrajectoryHelper()
{
m_data = new IKTrajectoryHelperInternalData;
}
IKTrajectoryHelper::~IKTrajectoryHelper()
{
delete m_data;
}
void IKTrajectoryHelper::createKukaIIWA()
{
const VectorR3& unitx = VectorR3::UnitX;
const VectorR3& unity = VectorR3::UnitY;
const VectorR3& unitz = VectorR3::UnitZ;
const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
const VectorR3& zero = VectorR3::Zero;
float minTheta = -4 * PI;
float maxTheta = 4 * PI;
m_data->m_ikNodes.resize(8);//7DOF+additional endeffector
m_data->m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.));
m_data->m_ikTree.InsertRoot(m_data->m_ikNodes[0]);
m_data->m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[0], m_data->m_ikNodes[1]);
m_data->m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[1], m_data->m_ikNodes[2]);
m_data->m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[2], m_data->m_ikNodes[3]);
m_data->m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[3], m_data->m_ikNodes[4]);
m_data->m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[4], m_data->m_ikNodes[5]);
m_data->m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.));
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[5], m_data->m_ikNodes[6]);
m_data->m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR);
m_data->m_ikTree.InsertLeftChild(m_data->m_ikNodes[6], m_data->m_ikNodes[7]);
m_data->m_ikJacobian = new Jacobian(&m_data->m_ikTree);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikTree.Init();
m_data->m_ikTree.Compute();
m_data->m_ikJacobian->Reset();
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,
double* q_new, int ikMethod)
{
if (numQ != 7)
{
return false;
}
for (int i=0;i<numQ;i++)
{
m_data->m_ikNodes[i]->SetTheta(q_current[i]);
}
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
m_data->m_ikJacobian->SetJtargetActive();
}
else {
m_data->m_ikJacobian->SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
m_data->m_ikJacobian->ZeroDeltaThetas();
break;
}
m_data->m_ikJacobian->UpdateThetas();
// Apply the change in the theta values
m_data->m_ikJacobian->UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}

30
examples/RoboticsLearning/IKTrajectoryHelper.h
View File

@@ -1,30 +0,0 @@
#ifndef IK_TRAJECTORY_HELPER_H
#define IK_TRAJECTORY_HELPER_H
enum IK2_Method
{
IK2_JACOB_TRANS=0,
IK2_PURE_PSEUDO,
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD
};
class IKTrajectoryHelper
{
struct IKTrajectoryHelperInternalData* m_data;
public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
bool computeIK(const double endEffectorTargetPosition[3],
const double* q_old, int numQ,
double* q_new, int ikMethod);
};
#endif //IK_TRAJECTORY_HELPER_H

38
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -1,6 +1,6 @@
#include "KukaGraspExample.h"
#include "IKTrajectoryHelper.h"
#include "../SharedMemory/IKTrajectoryHelper.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
@@ -138,8 +138,11 @@ public:
}
virtual void stepSimulation(float deltaTime)
{
m_time+=deltaTime;
m_targetPos.setValue(0.5, 0, 0.5+0.4*b3Sin(3 * m_time));
float dt = deltaTime;
btClamp(dt,0.0001f,0.01f);
m_time+=dt;
m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
@@ -158,11 +161,36 @@ public:
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
// m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
/*
b3RobotSimInverseKinematicArgs ikargs;
b3RobotSimInverseKinematicsResults ikresults;
ikargs.m_bodyUniqueId = m_kukaIndex;
ikargs.m_currentJointPositions = q_current;
ikargs.m_numPositions = 7;
ikargs.m_endEffectorTargetPosition[0] = dataOut.m_floats[0];
ikargs.m_endEffectorTargetPosition[1] = dataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = dataOut.m_floats[2];
ikargs.m_endEffectorTargetOrientation[0] = 0;
ikargs.m_endEffectorTargetOrientation[1] = 0;
ikargs.m_endEffectorTargetOrientation[2] = 0;
ikargs.m_endEffectorTargetOrientation[3] = 1;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{
}
*/
double q_new[7];
int ikMethod=IK2_SDLS;
b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut);
m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod);
printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
q_new[3],q_new[4],q_new[5],q_new[6]);

27
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -469,6 +469,33 @@ void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps)
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
}
/*
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, double targetPosition[3]);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointPositions);
*/
bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results)
{
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId,
args.m_currentJointPositions,args.m_endEffectorTargetPosition);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
&results.m_bodyUniqueId,
&results.m_numPositions,
results.m_calculatedJointPositions);
return result;
}
b3RobotSimAPI::~b3RobotSimAPI()
{
delete m_data;

32
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -50,6 +50,7 @@ struct b3RobotSimLoadFileArgs
}
};
struct b3RobotSimLoadFileResults
{
b3AlignedObjectArray<int> m_uniqueObjectIds;
@@ -81,6 +82,35 @@ struct b3JointMotorArgs
}
};
enum b3InverseKinematicsFlags
{
B3_HAS_IK_TARGET_ORIENTATION=1,
};
struct b3RobotSimInverseKinematicArgs
{
int m_bodyUniqueId;
double* m_currentJointPositions;
int m_numPositions;
double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[3];
int m_flags;
b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1),
m_currentJointPositions(0),
m_numPositions(0),
m_flags(0)
{
}
};
struct b3RobotSimInverseKinematicsResults
{
int m_bodyUniqueId;
double* m_calculatedJointPositions;
int m_numPositions;
};
class b3RobotSimAPI
{
@@ -113,6 +143,8 @@ public:
void setNumSimulationSubSteps(int numSubSteps);
bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results);
void renderScene();
void debugDraw(int debugDrawMode);
};