From 7a1dbf2e59186fb751f8846d8fce758c16fad1c6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 25 Oct 2017 00:32:47 -0700 Subject: [PATCH] allow IK on a floating base, see video: add inverse_kinematics_husky_kuka.py example fix spacing in inverse_dynamics.py --- .../PhysicsServerCommandProcessor.cpp | 16 +- .../pybullet/examples/inverse_dynamics.py | 2 +- .../examples/inverse_kinematics_husky_kuka.py | 169 ++++++++++++++++++ 3 files changed, 179 insertions(+), 8 deletions(-) create mode 100644 examples/pybullet/examples/inverse_kinematics_husky_kuka.py diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d7ea993ba..aefe51eec 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7671,7 +7671,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) { const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; b3AlignedObjectArray jacobian_linear; jacobian_linear.resize(3*numDofs); b3AlignedObjectArray jacobian_angular; @@ -7685,11 +7685,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray q_current; q_current.resize(numDofs); - if (tree && (numDofs == tree->numDoFs())) + + + if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) { 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); @@ -7709,8 +7711,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) { tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, numDofs); - btInverseDynamics::mat3x jac_r(3,numDofs); + btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); + btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); @@ -7718,8 +7720,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { 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); + jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); + jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); } } } diff --git a/examples/pybullet/examples/inverse_dynamics.py b/examples/pybullet/examples/inverse_dynamics.py index e6af3011b..8b86cc3b9 100644 --- a/examples/pybullet/examples/inverse_dynamics.py +++ b/examples/pybullet/examples/inverse_dynamics.py @@ -143,7 +143,7 @@ if plot: ax_tor.set_ylim(-20., 20.) ax_tor.legend() - plt.pause(0.01) + plt.pause(0.01) while (1): diff --git a/examples/pybullet/examples/inverse_kinematics_husky_kuka.py b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py new file mode 100644 index 000000000..eb28102b8 --- /dev/null +++ b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py @@ -0,0 +1,169 @@ +import pybullet as p +import time +import math +from datetime import datetime +from datetime import datetime + +clid = p.connect(p.SHARED_MEMORY) +if (clid<0): + p.connect(p.GUI) +p.loadURDF("plane.urdf",[0,0,-0.3]) +husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659]) +for i in range (p.getNumJoints(husky)): + print(p.getJointInfo(husky,i)) +kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659) +ob = kukaId +jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ] +for jointIndex in range (p.getNumJoints(ob)): + p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) + + +#put kuka on top of husky + +cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1]) + + +baseorn = p.getQuaternionFromEuler([3.1415,0,0.3]) +baseorn = [0,0,0,1] +#[0, 0, 0.707, 0.707] + +#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1]) +kukaEndEffectorIndex = 6 +numJoints = p.getNumJoints(kukaId) +if (numJoints!=7): + exit() + + +#lower limits for null space +ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] +#upper limits for null space +ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] +#joint ranges for null space +jr=[5.8,4,5.8,4,5.8,4,6] +#restposes for null space +rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] +#joint damping coefficents +jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] + +for i in range (numJoints): + p.resetJointState(kukaId,i,rp[i]) + +p.setGravity(0,0,-10) +t=0. +prevPose=[0,0,0] +prevPose1=[0,0,0] +hasPrevPose = 0 +useNullSpace = 0 + +useOrientation =0 +#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. +#This can be used to test the IK result accuracy. +useSimulation = 0 +useRealTimeSimulation = 1 +p.setRealTimeSimulation(useRealTimeSimulation) +#trailDuration is duration (in seconds) after debug lines will be removed automatically +#use 0 for no-removal +trailDuration = 15 +basepos =[0,0,0] +ang = 0 +ang=0 + +def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter): + closeEnough = False + iter = 0 + dist2 = 1e30 + while (not closeEnough and iter