diff --git a/build_visual_studio.bat b/build_visual_studio.bat
index 399461122..302194631 100644
--- a/build_visual_studio.bat
+++ b/build_visual_studio.bat
@@ -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
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
diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf
index d8fe60293..a6e53d3ab 100644
--- a/data/kuka_iiwa/model.urdf
+++ b/data/kuka_iiwa/model.urdf
@@ -284,5 +284,6 @@
+
diff --git a/data/sphere_small.urdf b/data/sphere_small.urdf
new file mode 100644
index 000000000..c5712bc35
--- /dev/null
+++ b/data/sphere_small.urdf
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/teddy_vhacd.urdf b/data/teddy_vhacd.urdf
index 7df9e7919..1868cb9ff 100644
--- a/data/teddy_vhacd.urdf
+++ b/data/teddy_vhacd.urdf
@@ -8,23 +8,23 @@
-
-
+
+
-
+
-
-
+
+
-
+
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 320c0b6e6..1f16758a4 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -401,15 +401,18 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
return true;
}
-static btCollisionShape* createConvexHullFromShapes(std::vector& shapes)
+static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale)
{
btCompoundShape* compound = new btCompoundShape();
+ compound->setMargin(gUrdfDefaultCollisionMargin);
+
btTransform identity;
identity.setIdentity();
for (int s = 0; s<(int)shapes.size(); s++)
{
btConvexHullShape* convexHull = new btConvexHullShape();
+ convexHull->setMargin(gUrdfDefaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
@@ -420,17 +423,18 @@ static btCollisionShape* createConvexHullFromShapes(std::vectoraddPoint(pt,false);
+
+ convexHull->addPoint(pt*geomScale,false);
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 + 2]);
- convexHull->addPoint(pt, false);
+ convexHull->addPoint(pt*geomScale, false);
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 + 2]);
- convexHull->addPoint(pt, false);
+ convexHull->addPoint(pt*geomScale, false);
}
convexHull->recalcLocalAabb();
@@ -464,16 +468,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
vertices.push_back(vert);
}
- btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
- cylZShape->setMargin(gUrdfDefaultCollisionMargin);
- cylZShape->initializePolyhedralFeatures();
+ btConvexHullShape* convexHull = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+ convexHull->setMargin(gUrdfDefaultCollisionMargin);
+ convexHull->initializePolyhedralFeatures();
+ convexHull->optimizeConvexHull();
+
//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.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
- shape = cylZShape;
+ shape = convexHull;
break;
}
case URDF_GEOM_BOX:
@@ -555,7 +561,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
std::vector shapes;
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
//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;
}
break;
@@ -685,13 +692,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
shape = trimesh;
} else
{
- //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
- btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
- //cylZShape->initializePolyhedralFeatures();
- //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
- //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
- cylZShape->setMargin(gUrdfDefaultCollisionMargin);
- shape = cylZShape;
+ btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
+ convexHull->optimizeConvexHull();
+ //convexHull->initializePolyhedralFeatures();
+ convexHull->setMargin(gUrdfDefaultCollisionMargin);
+ shape = convexHull;
}
} else
{
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index 3abc77fa2..817cb0ce5 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -1057,12 +1057,15 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger*
TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit");
if (limit_xml)
{
- if (!parseJointLimits(joint, limit_xml,logger))
- {
- logger->reportError("Could not parse limit element for joint:");
- logger->reportError(joint.m_name.c_str());
- return false;
- }
+ if (joint.m_type != URDFContinuousJoint)
+ {
+ if (!parseJointLimits(joint, limit_xml,logger))
+ {
+ logger->reportError("Could not parse limit element for joint:");
+ logger->reportError(joint.m_name.c_str());
+ return false;
+ }
+ }
}
else if (joint.m_type == URDFRevoluteJoint)
{
diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp
index f5735d0e0..fa49f3e2e 100644
--- a/examples/RoboticsLearning/KukaGraspExample.cpp
+++ b/examples/RoboticsLearning/KukaGraspExample.cpp
@@ -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);
}
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp
index 873ddfc36..1a9d9d2c2 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.cpp
+++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp
@@ -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,
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h
index bb5fe6d41..f46e7ec3e 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.h
+++ b/examples/RoboticsLearning/b3RobotSimAPI.h
@@ -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;
}
};
diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp
index 3e4537ea9..45b795aab 100644
--- a/examples/SharedMemory/IKTrajectoryHelper.cpp
+++ b/examples/SharedMemory/IKTrajectoryHelper.cpp
@@ -16,7 +16,7 @@ struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
- Tree m_ikTree;
+
b3AlignedObjectArray m_ikNodes;
Jacobian* m_ikJacobian;
@@ -37,82 +37,22 @@ IKTrajectoryHelper::~IKTrajectoryHelper()
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],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
- const double* q_current, int numQ,
- double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt)
+ const double* q_current, int numQ,int endEffectorIndex,
+ 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;im_ikNodes[i]->SetTheta(q_current[i]);
- }
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
@@ -129,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3);
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
@@ -139,35 +79,49 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
- float angleDot = angle/dt;
+ float angleDot = angle*dampIk;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++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)
- {
- completeJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
- completeJacobian.Set(i+nRow/2,j,angular_jacobian[i*nCol+j]);
- }
+
+ if (useAngularPart)
+ {
+ 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
switch (ikMethod) {
@@ -175,6 +129,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
+ case IK2_VEL_DLS_WITH_ORIENTATION:
+ case IK2_VEL_DLS:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_DLS_SVD:
@@ -186,9 +142,6 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
- case IK2_VEL_DLS:
- m_data->m_ikJacobian->CalcThetasDotDLS(dt); // Damped least square for velocity IK
- break;
default:
m_data->m_ikJacobian->ZeroDeltaThetas();
break;
@@ -206,7 +159,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
for (int i=0;im_ikNodes[i]->GetTheta()*dt + q_current[i];
+ q_new[i] = m_data->m_ikJacobian->dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h
index ca786a104..b4783b41d 100644
--- a/examples/SharedMemory/IKTrajectoryHelper.h
+++ b/examples/SharedMemory/IKTrajectoryHelper.h
@@ -8,7 +8,8 @@ enum IK2_Method
IK2_DLS,
IK2_SDLS ,
IK2_DLS_SVD,
- IK2_VEL_DLS
+ IK2_VEL_DLS,
+ IK2_VEL_DLS_WITH_ORIENTATION,
};
@@ -20,17 +21,13 @@ public:
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],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
- const double* q_old, int numQ,
- double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt);
+ const double* q_old, int numQ,int endEffectorIndex,
+ double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk=1.);
};
#endif //IK_TRAJECTORY_HELPER_H
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index d6e113fe2..61a192fa8 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -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
-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;
b3Assert(cl);
@@ -1330,12 +1330,32 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
-//todo
-// int numJoints = cl->getNumJoints(bodyIndex);
-// for (int i = 0; i < numJoints;i++)
-// {
-// command->m_calculateInverseKinematicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
-// }
+
+ return (b3SharedMemoryCommandHandle)command;
+
+}
+
+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[1] = targetPosition[1];
@@ -1345,12 +1365,10 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
- command->m_calculateInverseKinematicsArguments.m_dt = dt;
-
- return (b3SharedMemoryCommandHandle)command;
}
+
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index f3d9acd9c..a57851fd8 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -121,8 +121,9 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
///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* bodyUniqueId,
int* dofCount,
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 9217ac88c..fb0247dca 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -29,6 +29,8 @@
btVector3 gLastPickPos(0, 0, 0);
bool gEnableRealTimeSimVR=false;
+int gCreateObjectSimVR = -1;
+btScalar simTimeScalingFactor = 1;
struct UrdfLinkNameMapUtil
{
@@ -387,6 +389,8 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody;
int m_huskyId;
+ int m_KukaId;
+ int m_sphereId;
CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback;
@@ -438,7 +442,9 @@ struct PhysicsServerCommandProcessorInternalData
m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0),
m_allowRealTimeSimulation(false),
- m_huskyId(0),
+ m_huskyId(-1),
+ m_KukaId(-1),
+ m_sphereId(-1),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
@@ -558,6 +564,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld();
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))
{
- float maxMotorImpulse = 10000.f;
+ float maxMotorImpulse = 1.f;
int dof = 0;
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
@@ -1880,13 +1887,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
applyJointDamping(i);
}
+ btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
+
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
{
- m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, 0);
+ m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
}
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2546,7 +2555,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd = serverStatusOut;
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)
{
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
@@ -2560,41 +2569,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
else
{
IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
- if (tmpHelper->createFromMultiBody(bodyHandle->m_multiBody))
- {
- m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
- ikHelperPtr = tmpHelper;
- } else
- {
- delete tmpHelper;
- }
+ m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
+ ikHelperPtr = tmpHelper;
}
- //todo: make this generic. Right now, only support/tested KUKA iiwa
- int numJoints = 7;
- int endEffectorLinkIndex = 6;
+ int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
- if (ikHelperPtr && bodyHandle->m_multiBody->getNumLinks()==numJoints)
+
+ if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks()))
{
+ int numJoints1 = bodyHandle->m_multiBody->getNumLinks();
+ const int numDofs = bodyHandle->m_multiBody->getNumDofs();
+
b3AlignedObjectArray jacobian_linear;
- jacobian_linear.resize(3*7);
+ jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray jacobian_angular;
- jacobian_angular.resize(3*7);
+ jacobian_angular.resize(3*numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
- double q_current[7];
+
+
+ btAlignedObjectArray q_current;
+ q_current.resize(numDofs);
if (tree)
{
jacSize = jacobian_linear.size();
// Set jacobian value
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[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))
{
tree->calculateJacobians(q);
- btInverseDynamics::mat3x jac_t(3, num_dofs);
- btInverseDynamics::mat3x jac_r(3,num_dofs);
+ 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 < num_dofs; ++j)
+ for (int j = 0; j < numDofs; ++j)
{
- jacobian_linear[i*num_dofs+j] = jac_t(i,j);
- jacobian_angular[i*num_dofs+j] = jac_r(i,j);
+ jacobian_linear[i*numDofs+j] = jac_t(i,j);
+ jacobian_angular[i*numDofs+j] = jac_r(i,j);
}
}
}
}
- double q_new[7];
- int ikMethod=IK2_VEL_DLS;
+ btAlignedObjectArray q_new;
+ q_new.resize(numDofs);
+ int ikMethod= (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)? IK2_VEL_DLS_WITH_ORIENTATION : IK2_VEL_DLS;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
@@ -2639,15 +2648,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
- q_current,
- numJoints, q_new, ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2,clientCmd.m_calculateInverseKinematicsArguments.m_dt);
+ &q_current[0],
+ 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;
- for (int i=0;im_logPlayback = pb;
}
+
btVector3 gVRGripperPos(0,0,0.2);
btQuaternion gVRGripperOrn(0,0,0,1);
+btVector3 gVRController2Pos(0,0,0.2);;
+btQuaternion gVRController2Orn(0,0,0,1);
+
btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false;
@@ -2864,14 +2878,29 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
static btAlignedObjectArray gBufferServerToClient;
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)
{
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());
@@ -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("sphere2.urdf", btVector3(-2, 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(-2, 0, 3), 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(-5, 0, 2), 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("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());
@@ -2950,7 +2980,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
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));
@@ -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 q_new;
+ btAlignedObjectArray 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;im_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 && (endEffectorLinkIndexm_multiBody->getNumLinks()))
+ {
+ int numJoints1 = bodyHandle->m_multiBody->getNumLinks();
+
+
+ b3AlignedObjectArray jacobian_linear;
+ jacobian_linear.resize(3*numDofs);
+ b3AlignedObjectArray 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;igetNumLinks();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;linkgetNumLinks();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;
if (m_data->m_numSimulationSubSteps)
{
@@ -2999,7 +3220,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
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;
if (numSteps)
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 2dec1401c..540901af9 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -21,8 +21,13 @@ btVector3 gVRTeleportPos(0,0,0);
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
+extern btVector3 gVRController2Pos;
+extern btQuaternion gVRController2Orn;
extern btScalar gVRGripperAnalog;
extern bool gEnableRealTimeSimVR;
+extern int gCreateObjectSimVR;
+static int gGraspingController = -1;
+extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed;
@@ -54,6 +59,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperRegisterGraphicsInstance,
eGUIHelperCreateCollisionShapeGraphicsObject,
eGUIHelperCreateCollisionObjectGraphicsObject,
+ eGUIHelperCreateRigidBodyGraphicsObject,
eGUIHelperRemoveAllGraphicsInstances,
eGUIHelperCopyCameraImageData,
};
@@ -305,7 +311,20 @@ public:
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;
btVector3 m_color2;
@@ -776,6 +795,14 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
m_multiThreadedHelper->getCriticalSection()->unlock();
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:
{
@@ -937,10 +964,15 @@ void PhysicsServerExample::renderScene()
}
}
- if (gDebugRenderToggle)
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
gEnableRealTimeSimVR = true;
+ }
+
+ if (gDebugRenderToggle)
+ if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
+ {
+
B3_PROFILE("Draw Debug HUD");
//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
@@ -970,7 +1002,7 @@ void PhysicsServerExample::renderScene()
count = 0;
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;
worseFps = 1000000;
@@ -996,15 +1028,25 @@ void PhysicsServerExample::renderScene()
m[14]=+gVRTeleportPos[2];
viewTr.setFromOpenGLMatrix(m);
btTransform viewTrInv = viewTr.inverse();
- float upMag = -.6;
+
btVector3 side = viewTrInv.getBasis().getColumn(0);
btVector3 up = viewTrInv.getBasis().getColumn(1);
- up+=0.6*side;
- m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
+ btVector3 fwd = viewTrInv.getBasis().getColumn(2);
+
+
+ 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);
- upMag = -0.7;
- m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
+ up = viewTrInv.getBasis().getColumn(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();
@@ -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])
{
@@ -1130,10 +1171,34 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
{
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)
{
- gDebugRenderToggle = !gDebugRenderToggle;
+ gCreateObjectSimVR = 1;
}
@@ -1183,6 +1248,10 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
}
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_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
}
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 48d4d0dfe..faefc25f5 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -393,8 +393,9 @@ struct CalculateJacobianResultArgs
enum EnumCalculateInverseKinematicsFlags
{
- IK_HAS_TARGET_ORIENTATION=1,
- IK_HAS_CURRENT_JOINT_POSITIONS=2,
+ IK_HAS_TARGET_POSITION=1,
+ IK_HAS_TARGET_ORIENTATION=2,
+ //IK_HAS_CURRENT_JOINT_POSITIONS=4,//not used yet
};
struct CalculateInverseKinematicsArgs
@@ -403,7 +404,7 @@ struct CalculateInverseKinematicsArgs
// double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_targetPosition[3];
double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
- double m_dt;
+ int m_endEffectorLinkIndex;
};
struct CalculateInverseKinematicsResultArgs
diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
index 6fd6c6aa6..6740733a5 100644
--- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
@@ -52,10 +52,11 @@ const double Jacobian::BaseMaxTargetDist = 0.4;
Jacobian::Jacobian(Tree* tree)
{
- Jacobian::tree = tree;
- nEffector = tree->GetNumEffector();
+ m_tree = tree;
+ m_nEffector = tree->GetNumEffector();
nJoint = tree->GetNumJoint();
- nRow = 2 * 3 * nEffector; // Include both linear and angular part
+ nRow = 3 * m_nEffector; // Include only the linear part
+
nCol = nJoint;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
@@ -77,9 +78,52 @@ Jacobian::Jacobian(Tree* tree)
// Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow);
- dSclamp.SetLength(nEffector);
- errorArray.SetLength(nEffector);
- Jnorms.SetSize(nEffector, nCol); // Holds the norms of the active J matrix
+ dSclamp.SetLength(m_nEffector);
+ errorArray.SetLength(m_nEffector);
+ 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();
}
@@ -98,9 +142,12 @@ void Jacobian::Reset()
// Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian(VectorR3* targets)
{
+ if (m_tree==0)
+ return;
+
// Traverse tree to find all end effectors
VectorR3 temp;
- Node* n = tree->GetRoot();
+ Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
@@ -113,10 +160,10 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
// Find all ancestors (they will usually all be joints)
// Set the corresponding entries in the Jacobians J, K.
- Node* m = tree->GetParent(n);
+ Node* m = m_tree->GetParent(n);
while ( m ) {
int j = m->GetJointNum();
- assert ( 0 <=i && iIsFrozen() ) {
Jend.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
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
// Update the joint angles
- Node* n = tree->GetRoot();
+ Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsJoint() ) {
int i = n->GetJointNum();
n->AddToTheta( dTheta[i] );
}
- n = tree->GetSuccessor( n );
+ n = m_tree->GetSuccessor( n );
}
// Update the positions and rotation axes of all joints/effectors
- tree->Compute();
+ m_tree->Compute();
}
void Jacobian::UpdateThetaDot()
{
+ if (m_tree==0)
+ return;
+
// Traverse the tree to find all joints
// Update the joint angles
- Node* n = tree->GetRoot();
+ Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsJoint() ) {
int i = n->GetJointNum();
n->UpdateTheta( dTheta[i] );
}
- n = tree->GetSuccessor( n );
+ n = m_tree->GetSuccessor( n );
}
// Update the positions and rotation axes of all joints/effectors
- tree->Compute();
+ m_tree->Compute();
}
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.
// CalcdTClampedFromdS();
- // VectorRn dTextra(3*nEffector);
+ // VectorRn dTextra(3*m_nEffector);
// U.Solve( dT, &dTextra );
// 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()
{
const MatrixRmn& J = ActiveJacobian();
@@ -366,7 +391,7 @@ void Jacobian::CalcDeltaThetasSDLS()
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
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();
dTheta.SetZero();
@@ -478,7 +503,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
// Traverse tree to find all end effectors
VectorR3 temp;
- Node* n = tree->GetRoot();
+ Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
@@ -489,7 +514,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
errorArray[i] = err;
totalError += err;
}
- n = tree->GetSuccessor( n );
+ n = m_tree->GetSuccessor( n );
}
return totalError;
}
@@ -498,7 +523,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
- Node* n = tree->GetRoot();
+ Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
@@ -517,7 +542,7 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
dSclamp[i] = BaseMaxTargetDist;
}
}
- n = tree->GetSuccessor( n );
+ n = m_tree->GetSuccessor( n );
}
}
diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h
index 38aa475c0..e02a3bee4 100644
--- a/examples/ThirdPartyLibs/BussIK/Jacobian.h
+++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h
@@ -54,6 +54,7 @@ enum UpdateMode {
class Jacobian {
public:
Jacobian(Tree*);
+ Jacobian(bool useAngularJacobian, int nDof);
void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
@@ -69,7 +70,7 @@ public:
void CalcDeltaThetasDLS();
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
- void CalcThetasDotDLS(float dt);
+
void UpdateThetas();
void UpdateThetaDot();
@@ -90,8 +91,8 @@ public:
int GetNumCols() {return nCol;}
public:
- Tree* tree; // tree associated with this Jacobian matrix
- int nEffector; // Number of end effectors
+ Tree* m_tree; // tree associated with this Jacobian matrix
+ int m_nEffector; // Number of end effectors
int nJoint; // Number of joints
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)
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index f73c16a36..9e82744fa 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -1018,6 +1018,24 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
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) {
int size = PySequence_Size(args);
int objectUniqueIdA = -1;
@@ -1658,22 +1676,25 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
if (size == 2)
{
int bodyIndex;
+ int endEffectorLinkIndex;
+
PyObject* targetPosObj;
-
- if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetPosObj))
+ PyObject* targetOrnObj;
+
+ if (PyArg_ParseTuple(args, "iiOO", &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj))
{
double pos[3];
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;
int numPos=0;
int resultBodyIndex;
int result;
- b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex,
- pos,ori,dt);
+
+ b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex);
+ b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
result = b3GetStatusInverseKinematicsJointPositions(statusHandle,