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

5
build_visual_studio.bat
View File

@@ -1,6 +1,9 @@
IF NOT EXIST bin mkdir bin
IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin
cd build3 cd build3
premake4 --enable_openvr --targetdir="../bin" vs2010 premake4 --enable_openvr --targetdir="../bin" vs2010
rem premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.6.0a3/include" --python_lib_dir="C:/Python-3.6.0a3/libs" --enable_openvr --targetdir="../bin" vs2010 rem premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010
pause pause

1
data/kuka_iiwa/model.urdf
View File

@@ -284,5 +284,6 @@
</geometry> </geometry>
</collision> </collision>
</link> </link>
</robot> </robot>

30
data/sphere_small.urdf Normal file
View File

@@ -0,0 +1,30 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="base_link">
<contact>
<rolling_friction value="0.001"/>
<spinning_friction value="0.001"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".1"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="textured_sphere_smooth.obj" scale="0.03 0.03 0.03"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
</collision>
</link>
</robot>

12
data/teddy_vhacd.urdf
View File

@@ -8,23 +8,23 @@
<contact_erp value="1.0"/> <contact_erp value="1.0"/>
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0.7 0.5 0.3"/> <origin rpy="0 0 0" xyz="0.07 0.05 0.03"/>
<mass value="1.0"/> <mass value=".1"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/> <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial> </inertial>
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="teddy2_VHACD_CHs.obj" scale="1 1 1"/> <mesh filename="teddy2_VHACD_CHs.obj" scale=".1 .1 .1"/>
</geometry> </geometry>
<material name="white"> <material name="red">
<color rgba="1 1 1 1"/> <color rgba="1 0.4 0.4 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="teddy2_VHACD_CHs.obj" scale="1 1 1"/> <mesh filename="teddy2_VHACD_CHs.obj" scale=".1 .1 .1"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>

37
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -401,15 +401,18 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true; return true;
} }
static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes) static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
{ {
btCompoundShape* compound = new btCompoundShape(); btCompoundShape* compound = new btCompoundShape();
compound->setMargin(gUrdfDefaultCollisionMargin);
btTransform identity; btTransform identity;
identity.setIdentity(); identity.setIdentity();
for (int s = 0; s<(int)shapes.size(); s++) for (int s = 0; s<(int)shapes.size(); s++)
{ {
btConvexHullShape* convexHull = new btConvexHullShape(); btConvexHullShape* convexHull = new btConvexHullShape();
convexHull->setMargin(gUrdfDefaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s]; tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size(); int faceCount = shape.mesh.indices.size();
@@ -420,17 +423,18 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(pt,false);
convexHull->addPoint(pt*geomScale,false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt, false); convexHull->addPoint(pt*geomScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt, false); convexHull->addPoint(pt*geomScale, false);
} }
convexHull->recalcLocalAabb(); convexHull->recalcLocalAabb();
@@ -464,16 +468,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
vertices.push_back(vert); vertices.push_back(vert);
} }
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); btConvexHullShape* convexHull = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(gUrdfDefaultCollisionMargin); convexHull->setMargin(gUrdfDefaultCollisionMargin);
cylZShape->initializePolyhedralFeatures(); convexHull->initializePolyhedralFeatures();
convexHull->optimizeConvexHull();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
shape = cylZShape; shape = convexHull;
break; break;
} }
case URDF_GEOM_BOX: case URDF_GEOM_BOX:
@@ -555,7 +561,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
std::vector<tinyobj::shape_t> shapes; std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix); std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
//create a convex hull for each shape, and store it in a btCompoundShape //create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes);
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
return shape; return shape;
} }
break; break;
@@ -685,13 +692,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
shape = trimesh; shape = trimesh;
} else } else
{ {
//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); convexHull->optimizeConvexHull();
//cylZShape->initializePolyhedralFeatures(); //convexHull->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); convexHull->setMargin(gUrdfDefaultCollisionMargin);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); shape = convexHull;
cylZShape->setMargin(gUrdfDefaultCollisionMargin);
shape = cylZShape;
} }
} else } else
{ {

15
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -1057,12 +1057,15 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit"); TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit");
if (limit_xml) if (limit_xml)
{ {
if (!parseJointLimits(joint, limit_xml,logger)) if (joint.m_type != URDFContinuousJoint)
{ {
logger->reportError("Could not parse limit element for joint:"); if (!parseJointLimits(joint, limit_xml,logger))
logger->reportError(joint.m_name.c_str()); {
return false; logger->reportError("Could not parse limit element for joint:");
} logger->reportError(joint.m_name.c_str());
return false;
}
}
} }
else if (joint.m_type == URDFRevoluteJoint) else if (joint.m_type == URDFRevoluteJoint)
{ {

10
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -78,7 +78,6 @@ public:
m_ikHelper.createKukaIIWA();
bool connected = m_robotSim.connect(m_guiHelper); bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected); b3Printf("robotSim connected = %d",connected);
@@ -114,6 +113,7 @@ public:
*/ */
} }
if (0)
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf"; args.m_fileName = "kiva_shelf/model.sdf";
@@ -195,11 +195,13 @@ public:
ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1]; ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2]; 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[0] = targetOriDataOut.m_floats[0];
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1]; ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2]; ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3]; ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
ikargs.m_dt = dt; ikargs.m_endEffectorLinkIndex = 6;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{ {
@@ -208,8 +210,10 @@ public:
{ {
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD); b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = ikresults.m_calculatedJointPositions[i]; t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
t.m_maxTorqueValue = 1000; t.m_maxTorqueValue = 100;
t.m_kp= 1; t.m_kp= 1;
t.m_targetVelocity = 0;
t.m_kp = 0.5;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t); 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) 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; b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
int numPos=0; int numPos=0;
bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,

13
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -94,15 +94,22 @@ struct b3RobotSimInverseKinematicArgs
// int m_numPositions; // int m_numPositions;
double m_endEffectorTargetPosition[3]; double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[4]; double m_endEffectorTargetOrientation[4];
double m_dt; int m_endEffectorLinkIndex;
int m_flags; int m_flags;
b3RobotSimInverseKinematicArgs() b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1), :m_bodyUniqueId(-1),
// m_currentJointPositions(0), m_endEffectorLinkIndex(-1),
// m_numPositions(0),
m_flags(0) 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;
} }
}; };

143
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -16,7 +16,7 @@ struct IKTrajectoryHelperInternalData
{ {
VectorR3 m_endEffectorTargetPosition; VectorR3 m_endEffectorTargetPosition;
Tree m_ikTree;
b3AlignedObjectArray<Node*> m_ikNodes; b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian; Jacobian* m_ikJacobian;
@@ -37,82 +37,22 @@ IKTrajectoryHelper::~IKTrajectoryHelper()
delete m_data; delete m_data;
} }
bool IKTrajectoryHelper::createFromMultiBody(class btMultiBody* mb)
{
//todo: implement proper conversion. For now, we only 'detect' a likely KUKA iiwa and hardcode its creation
if (mb->getNumLinks()==7)
{
createKukaIIWA();
return true;
}
return false;
}
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], bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4], const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3], const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4], const double endEffectorWorldOrientation[4],
const double* q_current, int numQ, const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt) double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk)
{ {
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false;
m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikJacobian->Reset();
if (numQ != 7)
{
return false;
}
for (int i=0;i<numQ;i++)
{
m_data->m_ikNodes[i]->SetTheta(q_current[i]);
}
bool UseJacobianTargets1 = false; bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) { if ( UseJacobianTargets1 ) {
@@ -129,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3); VectorRn deltaS(3);
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
deltaS.Set(i,(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])/dt); deltaS.Set(i,dampIk*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
} }
// Set one end effector world orientation from Bullet // Set one end effector world orientation from Bullet
@@ -139,35 +79,49 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse(); btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle(); float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis(); btVector3 axis = deltaQ.getAxis();
float angleDot = angle/dt; float angleDot = angle*dampIk;
btVector3 angularVel = angleDot*axis.normalize(); btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
deltaR.Set(i,angularVel[i]); deltaR.Set(i,angularVel[i]);
} }
VectorRn deltaC(6);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[i]);
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
// Set Jacobian from Bullet body Jacobian
int nRow = m_data->m_ikJacobian->GetNumRows();
int nCol = m_data->m_ikJacobian->GetNumCols();
b3Assert(jacobian_size==nRow*nCol);
MatrixRmn completeJacobian(nRow,nCol);
for (int i = 0; i < nRow/2; ++i)
{ {
for (int j = 0; j < nCol; ++j)
{ if (useAngularPart)
completeJacobian.Set(i,j,linear_jacobian[i*nCol+j]); {
completeJacobian.Set(i+nRow/2,j,angular_jacobian[i*nCol+j]); VectorRn deltaC(6);
} MatrixRmn completeJacobian(6,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
}
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
} else
{
VectorRn deltaC(3);
MatrixRmn completeJacobian(3,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
}
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
}
} }
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
// Calculate the change in theta values // Calculate the change in theta values
switch (ikMethod) { switch (ikMethod) {
@@ -175,6 +129,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break; break;
case IK2_DLS: case IK2_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break; break;
case IK2_DLS_SVD: case IK2_DLS_SVD:
@@ -186,9 +142,6 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_SDLS: case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break; break;
case IK2_VEL_DLS:
m_data->m_ikJacobian->CalcThetasDotDLS(dt); // Damped least square for velocity IK
break;
default: default:
m_data->m_ikJacobian->ZeroDeltaThetas(); m_data->m_ikJacobian->ZeroDeltaThetas();
break; break;
@@ -206,7 +159,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
for (int i=0;i<numQ;i++) for (int i=0;i<numQ;i++)
{ {
// Use for velocity IK // Use for velocity IK
q_new[i] = m_data->m_ikNodes[i]->GetTheta()*dt + q_current[i]; q_new[i] = m_data->m_ikJacobian->dTheta[i] + q_current[i];
// Use for position IK // Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta(); //q_new[i] = m_data->m_ikNodes[i]->GetTheta();

13
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -8,7 +8,8 @@ enum IK2_Method
IK2_DLS, IK2_DLS,
IK2_SDLS , IK2_SDLS ,
IK2_DLS_SVD, IK2_DLS_SVD,
IK2_VEL_DLS IK2_VEL_DLS,
IK2_VEL_DLS_WITH_ORIENTATION,
}; };
@@ -20,17 +21,13 @@ public:
IKTrajectoryHelper(); IKTrajectoryHelper();
virtual ~IKTrajectoryHelper(); virtual ~IKTrajectoryHelper();
///todo: replace createKukaIIWA with a generic way of create an IK tree
void createKukaIIWA();
bool createFromMultiBody(class btMultiBody* mb);
bool computeIK(const double endEffectorTargetPosition[3], bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4], const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3], const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4], const double endEffectorWorldOrientation[4],
const double* q_old, int numQ, const double* q_old, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt); double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk=1.);
}; };
#endif //IK_TRAJECTORY_HELPER_H #endif //IK_TRAJECTORY_HELPER_H

38
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1319,7 +1319,7 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
} }
///compute the joint positions to move the end effector to a desired target using inverse kinematics ///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt) b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl); b3Assert(cl);
@@ -1330,12 +1330,32 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS; command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0; command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex; command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
//todo
// int numJoints = cl->getNumJoints(bodyIndex); return (b3SharedMemoryCommandHandle)command;
// for (int i = 0; i < numJoints;i++)
// { }
// command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
// } void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
}
void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
@@ -1345,12 +1365,10 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
command->m_calculateInverseKinematicsArguments.m_dt = dt;
return (b3SharedMemoryCommandHandle)command;
} }
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,
int* dofCount, int* dofCount,

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -121,8 +121,9 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian); int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics ///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt); 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]);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,
int* dofCount, int* dofCount,

303
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -29,6 +29,8 @@
btVector3 gLastPickPos(0, 0, 0); btVector3 gLastPickPos(0, 0, 0);
bool gEnableRealTimeSimVR=false; bool gEnableRealTimeSimVR=false;
int gCreateObjectSimVR = -1;
btScalar simTimeScalingFactor = 1;
struct UrdfLinkNameMapUtil struct UrdfLinkNameMapUtil
{ {
@@ -387,6 +389,8 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody; btMultiBody* m_gripperMultiBody;
int m_huskyId; int m_huskyId;
int m_KukaId;
int m_sphereId;
CommandLogger* m_commandLogger; CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback; CommandLogPlayback* m_logPlayback;
@@ -438,7 +442,9 @@ struct PhysicsServerCommandProcessorInternalData
m_gripperRigidbodyFixed(0), m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0), m_gripperMultiBody(0),
m_allowRealTimeSimulation(false), m_allowRealTimeSimulation(false),
m_huskyId(0), m_huskyId(-1),
m_KukaId(-1),
m_sphereId(-1),
m_commandLogger(0), m_commandLogger(0),
m_logPlayback(0), m_logPlayback(0),
m_physicsDeltaTime(1./240.), m_physicsDeltaTime(1./240.),
@@ -558,6 +564,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
} }
@@ -746,7 +753,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
if (supportsJointMotor(mb,mbLinkIndex)) if (supportsJointMotor(mb,mbLinkIndex))
{ {
float maxMotorImpulse = 10000.f; float maxMotorImpulse = 1.f;
int dof = 0; int dof = 0;
btScalar desiredVelocity = 0.f; btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
@@ -1880,13 +1887,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
applyJointDamping(i); applyJointDamping(i);
} }
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
if (m_data->m_numSimulationSubSteps > 0) if (m_data->m_numSimulationSubSteps > 0)
{ {
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
} }
else else
{ {
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, 0); m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
} }
SharedMemoryStatus& serverCmd =serverStatusOut; SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2546,7 +2555,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED; serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody) if (bodyHandle && bodyHandle->m_multiBody)
{ {
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
@@ -2560,41 +2569,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
else else
{ {
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody)) m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
{ ikHelperPtr = tmpHelper;
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
} else
{
delete tmpHelper;
}
} }
//todo: make this generic. Right now, only support/tested KUKA iiwa int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
int numJoints = 7;
int endEffectorLinkIndex = 6;
if (ikHelperPtr && bodyHandle->m_multiBody->getNumLinks()==numJoints)
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{ {
int numJoints1 = bodyHandle->m_multiBody->getNumLinks();
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
b3AlignedObjectArray<double> jacobian_linear; b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*7); jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular; b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*7); jacobian_angular.resize(3*numDofs);
int jacSize = 0; int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
double q_current[7];
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
if (tree) if (tree)
{ {
jacSize = jacobian_linear.size(); jacSize = jacobian_linear.size();
// Set jacobian value // Set jacobian value
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
for (int i = 0; i < num_dofs; i++) btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++)
{ {
q_current[i] = bodyHandle->m_multiBody->getJointPos(i); q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
@@ -2608,24 +2616,25 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{ {
tree->calculateJacobians(q); tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs); btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,num_dofs); btInverseDynamics::mat3x jac_r(3,numDofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t); tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r); tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
for (int j = 0; j < num_dofs; ++j) for (int j = 0; j < numDofs; ++j)
{ {
jacobian_linear[i*num_dofs+j] = jac_t(i,j); jacobian_linear[i*numDofs+j] = jac_t(i,j);
jacobian_angular[i*num_dofs+j] = jac_r(i,j); jacobian_angular[i*numDofs+j] = jac_r(i,j);
} }
} }
} }
} }
double q_new[7]; btAlignedObjectArray<double> q_new;
int ikMethod=IK2_VEL_DLS; q_new.resize(numDofs);
int ikMethod= (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)? IK2_VEL_DLS_WITH_ORIENTATION : IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation; btVector3DoubleData endEffectorWorldOrientation;
@@ -2639,15 +2648,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
q_current, &q_current[0],
numJoints, q_new, ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2,clientCmd.m_calculateInverseKinematicsArguments.m_dt); numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numJoints;i++) for (int i=0;i<numDofs;i++)
{ {
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i]; serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
} }
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numJoints; serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED; serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
} }
} }
@@ -2848,8 +2858,12 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
m_data->m_logPlayback = pb; m_data->m_logPlayback = pb;
} }
btVector3 gVRGripperPos(0,0,0.2); btVector3 gVRGripperPos(0,0,0.2);
btQuaternion gVRGripperOrn(0,0,0,1); btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);;
btQuaternion gVRController2Orn(0,0,0,1);
btScalar gVRGripperAnalog = 0; btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false; bool gVRGripperClosed = false;
@@ -2864,14 +2878,29 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{ {
static btAlignedObjectArray<char> gBufferServerToClient; static btAlignedObjectArray<char> gBufferServerToClient;
gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int bodyId = 0;
if (gCreateObjectSimVR >= 0)
{
gCreateObjectSimVR = -1;
btMatrix3x3 mat(gVRGripperOrn);
btScalar spawnDistance = 0.1;
btVector3 spawnDir = mat.getColumn(0);
btVector3 shiftPos = spawnDir*spawnDistance;
btVector3 spawnPos = gVRGripperPos + shiftPos;
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_sphereId = bodyId;
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setBaseVel(spawnDir * 3);
}
}
if (!m_data->m_hasGround) if (!m_data->m_hasGround)
{ {
m_data->m_hasGround = true; m_data->m_hasGround = true;
int bodyId = 0;
loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -2913,13 +2942,14 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("cube.urdf", btVector3(3, -2, 0.5+i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("cube.urdf", btVector3(3, -2, 0.5+i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
} }
loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -2950,7 +2980,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
} }
loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
loadUrdf("teddy_vhacd.urdf", btVector3(1, 1, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
@@ -2989,6 +3020,196 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
} }
} }
{
InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btVector3 spherePos(0,0,0);
InternalBodyHandle* sphereBodyHandle = m_data->getHandle(m_data->m_KukaId);
if (sphereBodyHandle && sphereBodyHandle->m_multiBody)
{
spherePos = sphereBodyHandle->m_multiBody->getBasePos();
}
btMultiBody* mb = bodyHandle->m_multiBody;
btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2();
btScalar distanceThreshold = 2;
bool closeToKuka=(sqLen<(distanceThreshold*distanceThreshold));
int numDofs = bodyHandle->m_multiBody->getNumDofs();
btAlignedObjectArray<double> q_new;
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
}
q_new.resize(numDofs);
static btScalar t=0.f;
t+=0.01;
double dampIk = 0.99;
for (int i=0;i<numDofs;i++)
{
btScalar desiredPosition = btSin(t*0.1)*SIMD_HALF_PI;
q_new[i] = dampIk*q_current[i]+(1-dampIk)*desiredPosition;
}
if (closeToKuka)
{
dampIk = 1;
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;
if (ikHelperPtrPtr)
{
ikHelperPtr = *ikHelperPtrPtr;
}
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
ikHelperPtr = tmpHelper;
}
int endEffectorLinkIndex = 6;
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
int numJoints1 = bodyHandle->m_multiBody->getNumLinks();
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
jacSize = jacobian_linear.size();
// Set jacobian value
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++)
{
q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
qdot[i + baseDofs] = 0;
nu[i+baseDofs] = 0;
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[i*numDofs+j] = jac_t(i,j);
jacobian_angular[i*numDofs+j] = jac_r(i,j);
}
}
}
}
//int ikMethod= IK2_VEL_DLS;//IK2_VEL_DLS_WITH_ORIENTATION;//IK2_VEL_DLS;
int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION;//IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3DoubleData targetWorldPosition;
btQuaternionDoubleData targetWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
gVRController2Pos.serializeDouble(targetWorldPosition);
gVRController2Orn.serializeDouble(targetWorldOrientation);
static btScalar time=0.f;
time+=0.01;
btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time));
targetPos +=mb->getBasePos();
btQuaternion fwdOri(btVector3(1,0,0),-SIMD_HALF_PI);
(0, 1.0, 0, 0);
double downOrn[4] = {0,1,0,0};
//double downOrn[4] = {0,1,0,0};
fwdOri.serializeDouble(targetWorldOrientation);
ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk);
}
}
//directly set the position of the links, only for debugging IK, don't use this method!
//if (0)
//{
// for (int i=0;i<mb->getNumLinks();i++)
// {
// mb->setJointPosMultiDof(i,&q_new[i]);
// }
//} else
{
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
{
int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
for (int link=0;link<mb->getNumLinks();link++)
{
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
btScalar desiredVelocity = 0.f;
btScalar desiredPosition = q_new[link];
motor->setVelocityTarget(desiredVelocity,1);
motor->setPositionTarget(desiredPosition,0.6);
btScalar maxImp = 1.f;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
}
velIndex += mb->getLink(link).m_dofCount;
posIndex += mb->getLink(link).m_posVarCount;
}
}
}
}
}
int maxSteps = m_data->m_numSimulationSubSteps+3; int maxSteps = m_data->m_numSimulationSubSteps+3;
if (m_data->m_numSimulationSubSteps) if (m_data->m_numSimulationSubSteps)
{ {
@@ -2999,7 +3220,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
gSubStep = m_data->m_physicsDeltaTime; gSubStep = m_data->m_physicsDeltaTime;
} }
int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep); int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec*simTimeScalingFactor,maxSteps, gSubStep);
gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0; gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
if (numSteps) if (numSteps)

89
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -21,8 +21,13 @@ btVector3 gVRTeleportPos(0,0,0);
btQuaternion gVRTeleportOrn(0, 0, 0,1); btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos; extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn; extern btQuaternion gVRGripperOrn;
extern btVector3 gVRController2Pos;
extern btQuaternion gVRController2Orn;
extern btScalar gVRGripperAnalog; extern btScalar gVRGripperAnalog;
extern bool gEnableRealTimeSimVR; extern bool gEnableRealTimeSimVR;
extern int gCreateObjectSimVR;
static int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed; extern bool gVRGripperClosed;
@@ -54,6 +59,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperRegisterGraphicsInstance, eGUIHelperRegisterGraphicsInstance,
eGUIHelperCreateCollisionShapeGraphicsObject, eGUIHelperCreateCollisionShapeGraphicsObject,
eGUIHelperCreateCollisionObjectGraphicsObject, eGUIHelperCreateCollisionObjectGraphicsObject,
eGUIHelperCreateRigidBodyGraphicsObject,
eGUIHelperRemoveAllGraphicsInstances, eGUIHelperRemoveAllGraphicsInstances,
eGUIHelperCopyCameraImageData, eGUIHelperCopyCameraImageData,
}; };
@@ -305,7 +311,20 @@ public:
return m_cs; return m_cs;
} }
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){} btRigidBody* m_body;
btVector3 m_color3;
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
{
m_body = body;
m_color3 = color;
m_cs->lock();
m_cs->setSharedParam(1,eGUIHelperCreateRigidBodyGraphicsObject);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
{
b3Clock::usleep(1000);
}
}
btCollisionObject* m_obj; btCollisionObject* m_obj;
btVector3 m_color2; btVector3 m_color2;
@@ -776,6 +795,14 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
m_multiThreadedHelper->getCriticalSection()->unlock(); m_multiThreadedHelper->getCriticalSection()->unlock();
break; break;
} }
case eGUIHelperCreateRigidBodyGraphicsObject:
{
m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperRegisterTexture: case eGUIHelperRegisterTexture:
{ {
@@ -937,10 +964,15 @@ void PhysicsServerExample::renderScene()
} }
} }
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{ {
gEnableRealTimeSimVR = true; gEnableRealTimeSimVR = true;
}
if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
B3_PROFILE("Draw Debug HUD"); B3_PROFILE("Draw Debug HUD");
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
@@ -970,7 +1002,7 @@ void PhysicsServerExample::renderScene()
count = 0; count = 0;
sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2); sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2);
sprintf(line1, "Physics Steps = %d, Dropped = %d, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, gDtInSec, gSubStep); sprintf(line1, "Physics Steps = %d, Drop = %d, time scale=%f, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
gDroppedSimulationSteps = 0; gDroppedSimulationSteps = 0;
worseFps = 1000000; worseFps = 1000000;
@@ -996,15 +1028,25 @@ void PhysicsServerExample::renderScene()
m[14]=+gVRTeleportPos[2]; m[14]=+gVRTeleportPos[2];
viewTr.setFromOpenGLMatrix(m); viewTr.setFromOpenGLMatrix(m);
btTransform viewTrInv = viewTr.inverse(); btTransform viewTrInv = viewTr.inverse();
float upMag = -.6;
btVector3 side = viewTrInv.getBasis().getColumn(0); btVector3 side = viewTrInv.getBasis().getColumn(0);
btVector3 up = viewTrInv.getBasis().getColumn(1); btVector3 up = viewTrInv.getBasis().getColumn(1);
up+=0.6*side; btVector3 fwd = viewTrInv.getBasis().getColumn(2);
m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
float upMag = 0;
float sideMag = 2.2;
float fwdMag = -4;
m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
//btVector3 fwd = viewTrInv.getBasis().getColumn(2); //btVector3 fwd = viewTrInv.getBasis().getColumn(2);
upMag = -0.7; up = viewTrInv.getBasis().getColumn(1);
m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1); upMag = -0.3;
m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1);
} }
//m_args[0].m_cs->unlock(); //m_args[0].m_cs->unlock();
@@ -1112,7 +1154,6 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt
} }
static int gGraspingController = -1;
void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4]) void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
{ {
@@ -1130,10 +1171,34 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
{ {
gVRTeleportPos = gLastPickPos; gVRTeleportPos = gLastPickPos;
} }
} else
{
if (button == 1)
{
if (state == 1)
{
gDebugRenderToggle = 1;
} else
{
gDebugRenderToggle = 0;
simTimeScalingFactor *= 0.5;
if (simTimeScalingFactor==0)
{
simTimeScalingFactor = 1;
}
if (simTimeScalingFactor<0.01)
{
simTimeScalingFactor = 0;
}
}
} else
{
}
} }
if (button==32 && state==0) if (button==32 && state==0)
{ {
gDebugRenderToggle = !gDebugRenderToggle; gCreateObjectSimVR = 1;
} }
@@ -1183,6 +1248,10 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
} }
else else
{ {
gVRController2Pos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
gVRController2Orn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]); m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
} }

7
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -393,8 +393,9 @@ struct CalculateJacobianResultArgs
enum EnumCalculateInverseKinematicsFlags enum EnumCalculateInverseKinematicsFlags
{ {
IK_HAS_TARGET_ORIENTATION=1, IK_HAS_TARGET_POSITION=1,
IK_HAS_CURRENT_JOINT_POSITIONS=2, IK_HAS_TARGET_ORIENTATION=2,
//IK_HAS_CURRENT_JOINT_POSITIONS=4,//not used yet
}; };
struct CalculateInverseKinematicsArgs struct CalculateInverseKinematicsArgs
@@ -403,7 +404,7 @@ struct CalculateInverseKinematicsArgs
// double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; // double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_targetPosition[3]; double m_targetPosition[3];
double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
double m_dt; int m_endEffectorLinkIndex;
}; };
struct CalculateInverseKinematicsResultArgs struct CalculateInverseKinematicsResultArgs

121
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -52,10 +52,11 @@ const double Jacobian::BaseMaxTargetDist = 0.4;
Jacobian::Jacobian(Tree* tree) Jacobian::Jacobian(Tree* tree)
{ {
Jacobian::tree = tree; m_tree = tree;
nEffector = tree->GetNumEffector(); m_nEffector = tree->GetNumEffector();
nJoint = tree->GetNumJoint(); nJoint = tree->GetNumJoint();
nRow = 2 * 3 * nEffector; // Include both linear and angular part nRow = 3 * m_nEffector; // Include only the linear part
nCol = nJoint; nCol = nJoint;
Jend.SetSize(nRow, nCol); // The Jocobian matrix Jend.SetSize(nRow, nCol); // The Jocobian matrix
@@ -77,9 +78,52 @@ Jacobian::Jacobian(Tree* tree)
// Used by the Selectively Damped Least Squares Method // Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow); //dT.SetLength(nRow);
dSclamp.SetLength(nEffector); dSclamp.SetLength(m_nEffector);
errorArray.SetLength(nEffector); errorArray.SetLength(m_nEffector);
Jnorms.SetSize(nEffector, nCol); // Holds the norms of the active J matrix Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
Jacobian::Jacobian(bool useAngularJacobian,int nDof)
{
m_tree = 0;
m_nEffector = 1;
if (useAngularJacobian)
{
nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
} else
{
nRow = 3 * m_nEffector; // Include only the linear part
}
nCol = nDof;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetZero();
SetJendActive();
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w .SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dPreTheta.SetLength(nCol);
// Used by Jacobian transpose method & DLS & SDLS
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
// Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow);
dSclamp.SetLength(m_nEffector);
errorArray.SetLength(m_nEffector);
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset(); Reset();
} }
@@ -98,9 +142,12 @@ void Jacobian::Reset()
// Compute the J and K matrices (the Jacobians) // Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian(VectorR3* targets) void Jacobian::ComputeJacobian(VectorR3* targets)
{ {
if (m_tree==0)
return;
// Traverse tree to find all end effectors // Traverse tree to find all end effectors
VectorR3 temp; VectorR3 temp;
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsEffector() ) { if ( n->IsEffector() ) {
int i = n->GetEffectorNum(); int i = n->GetEffectorNum();
@@ -113,10 +160,10 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
// Find all ancestors (they will usually all be joints) // Find all ancestors (they will usually all be joints)
// Set the corresponding entries in the Jacobians J, K. // Set the corresponding entries in the Jacobians J, K.
Node* m = tree->GetParent(n); Node* m = m_tree->GetParent(n);
while ( m ) { while ( m ) {
int j = m->GetJointNum(); int j = m->GetJointNum();
assert ( 0 <=i && i<nEffector && 0<=j && j<nJoint ); assert ( 0 <=i && i<m_nEffector && 0<=j && j<nJoint );
if ( m->IsFrozen() ) { if ( m->IsFrozen() ) {
Jend.SetTriple(i, j, VectorR3::Zero); Jend.SetTriple(i, j, VectorR3::Zero);
Jtarget.SetTriple(i, j, VectorR3::Zero); Jtarget.SetTriple(i, j, VectorR3::Zero);
@@ -131,10 +178,10 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
temp *= m->GetW(); // cross product with joint rotation axis temp *= m->GetW(); // cross product with joint rotation axis
Jtarget.SetTriple(i, j, temp); Jtarget.SetTriple(i, j, temp);
} }
m = tree->GetParent( m ); m = m_tree->GetParent( m );
} }
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
} }
@@ -156,36 +203,39 @@ void Jacobian::UpdateThetas()
{ {
// Traverse the tree to find all joints // Traverse the tree to find all joints
// Update the joint angles // Update the joint angles
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsJoint() ) { if ( n->IsJoint() ) {
int i = n->GetJointNum(); int i = n->GetJointNum();
n->AddToTheta( dTheta[i] ); n->AddToTheta( dTheta[i] );
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
// Update the positions and rotation axes of all joints/effectors // Update the positions and rotation axes of all joints/effectors
tree->Compute(); m_tree->Compute();
} }
void Jacobian::UpdateThetaDot() void Jacobian::UpdateThetaDot()
{ {
if (m_tree==0)
return;
// Traverse the tree to find all joints // Traverse the tree to find all joints
// Update the joint angles // Update the joint angles
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsJoint() ) { if ( n->IsJoint() ) {
int i = n->GetJointNum(); int i = n->GetJointNum();
n->UpdateTheta( dTheta[i] ); n->UpdateTheta( dTheta[i] );
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
// Update the positions and rotation axes of all joints/effectors // Update the positions and rotation axes of all joints/effectors
tree->Compute(); m_tree->Compute();
} }
void Jacobian::CalcDeltaThetas() void Jacobian::CalcDeltaThetas()
@@ -279,7 +329,7 @@ void Jacobian::CalcDeltaThetasDLS()
// Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e. // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
// CalcdTClampedFromdS(); // CalcdTClampedFromdS();
// VectorRn dTextra(3*nEffector); // VectorRn dTextra(3*m_nEffector);
// U.Solve( dT, &dTextra ); // U.Solve( dT, &dTextra );
// J.MultiplyTranspose( dTextra, dTheta ); // J.MultiplyTranspose( dTextra, dTheta );
@@ -294,31 +344,6 @@ void Jacobian::CalcDeltaThetasDLS()
} }
} }
void Jacobian::CalcThetasDotDLS(float dt)
{
const MatrixRmn& J = ActiveJacobian();
MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
U.AddToDiagonal( DampingLambdaSq );
// Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
// CalcdTClampedFromdS();
// VectorRn dTextra(3*nEffector);
// U.Solve( dT, &dTextra );
// J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method
U.Solve( dS, &dT1 );
J.MultiplyTranspose( dT1, dTheta );
// Scale back to not exceed maximum angle changes
double MaxVelDLS = MaxAngleDLS/dt;
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxVelDLS ) {
dTheta *= MaxVelDLS/maxChange;
}
}
void Jacobian::CalcDeltaThetasDLSwithSVD() void Jacobian::CalcDeltaThetasDLSwithSVD()
{ {
const MatrixRmn& J = ActiveJacobian(); const MatrixRmn& J = ActiveJacobian();
@@ -366,7 +391,7 @@ void Jacobian::CalcDeltaThetasSDLS()
// Calculate response vector dTheta that is the SDLS solution. // Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values // Delta target values are the dS values
int nRows = J.GetNumRows(); int nRows = J.GetNumRows();
int numEndEffectors = tree->GetNumEffector(); // Equals the number of rows of J divided by three int numEndEffectors = m_tree->GetNumEffector(); // Equals the number of rows of J divided by three
int nCols = J.GetNumColumns(); int nCols = J.GetNumColumns();
dTheta.SetZero(); dTheta.SetZero();
@@ -478,7 +503,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
// Traverse tree to find all end effectors // Traverse tree to find all end effectors
VectorR3 temp; VectorR3 temp;
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsEffector() ) { if ( n->IsEffector() ) {
int i = n->GetEffectorNum(); int i = n->GetEffectorNum();
@@ -489,7 +514,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
errorArray[i] = err; errorArray[i] = err;
totalError += err; totalError += err;
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
return totalError; return totalError;
} }
@@ -498,7 +523,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
{ {
// Traverse tree to find all end effectors // Traverse tree to find all end effectors
VectorR3 temp; VectorR3 temp;
Node* n = tree->GetRoot(); Node* n = m_tree->GetRoot();
while ( n ) { while ( n ) {
if ( n->IsEffector() ) { if ( n->IsEffector() ) {
int i = n->GetEffectorNum(); int i = n->GetEffectorNum();
@@ -517,7 +542,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
dSclamp[i] = BaseMaxTargetDist; dSclamp[i] = BaseMaxTargetDist;
} }
} }
n = tree->GetSuccessor( n ); n = m_tree->GetSuccessor( n );
} }
} }

7
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -54,6 +54,7 @@ enum UpdateMode {
class Jacobian { class Jacobian {
public: public:
Jacobian(Tree*); Jacobian(Tree*);
Jacobian(bool useAngularJacobian, int nDof);
void ComputeJacobian(VectorR3* targets); void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; } const MatrixRmn& ActiveJacobian() const { return *Jactive; }
@@ -69,7 +70,7 @@ public:
void CalcDeltaThetasDLS(); void CalcDeltaThetasDLS();
void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS(); void CalcDeltaThetasSDLS();
void CalcThetasDotDLS(float dt);
void UpdateThetas(); void UpdateThetas();
void UpdateThetaDot(); void UpdateThetaDot();
@@ -90,8 +91,8 @@ public:
int GetNumCols() {return nCol;} int GetNumCols() {return nCol;}
public: public:
Tree* tree; // tree associated with this Jacobian matrix Tree* m_tree; // tree associated with this Jacobian matrix
int nEffector; // Number of end effectors int m_nEffector; // Number of end effectors
int nJoint; // Number of joints int nJoint; // Number of joints
int nRow; // Total number of rows the real J (= 3*number of end effectors for now) int nRow; // Total number of rows the real J (= 3*number of end effectors for now)
int nCol; // Total number of columns in the real J (= number of joints for now) int nCol; // Total number of columns in the real J (= number of joints for now)

33
examples/pybullet/pybullet.c
View File

@@ -1018,6 +1018,24 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
return 0; return 0;
} }
// vector - double[3] which will be set by values from obVec
static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
int i, len;
PyObject* seq;
seq = PySequence_Fast(obVec, "expected a sequence");
len = PySequence_Size(obVec);
if (len == 4) {
for (i = 0; i < len; i++) {
vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
}
Py_DECREF(seq);
return 1;
}
Py_DECREF(seq);
return 0;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
int size = PySequence_Size(args); int size = PySequence_Size(args);
int objectUniqueIdA = -1; int objectUniqueIdA = -1;
@@ -1658,22 +1676,25 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
if (size == 2) if (size == 2)
{ {
int bodyIndex; int bodyIndex;
int endEffectorLinkIndex;
PyObject* targetPosObj; PyObject* targetPosObj;
PyObject* targetOrnObj;
if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetPosObj))
if (PyArg_ParseTuple(args, "iiOO", &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj))
{ {
double pos[3]; double pos[3];
double ori[4]={0,1.0,0,0}; double ori[4]={0,1.0,0,0};
double dt=0.0001;
if (pybullet_internalSetVectord(targetPosObj,pos)) if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4(targetOrnObj,ori))
{ {
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int numPos=0; int numPos=0;
int resultBodyIndex; int resultBodyIndex;
int result; int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex,
pos,ori,dt); b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex);
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle, result = b3GetStatusInverseKinematicsJointPositions(statusHandle,