From ca7d599838d43dccf4fb16afa54fb1ba9b58f98f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 23 Oct 2017 12:25:04 -0700 Subject: [PATCH 1/3] add jointFrictionDamping.py example modify testrender.py to add shadow for tiny renderer --- .../pybullet/examples/jointFrictionDamping.py | 28 +++++++++++++++++++ examples/pybullet/examples/testrender.py | 7 +++-- 2 files changed, 32 insertions(+), 3 deletions(-) create mode 100644 examples/pybullet/examples/jointFrictionDamping.py diff --git a/examples/pybullet/examples/jointFrictionDamping.py b/examples/pybullet/examples/jointFrictionDamping.py new file mode 100644 index 000000000..efe34e12f --- /dev/null +++ b/examples/pybullet/examples/jointFrictionDamping.py @@ -0,0 +1,28 @@ +import pybullet as p +import time + +p.connect(p.GUI) +p.loadURDF("plane.urdf",[0,0,-0.25]) +minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf",useFixedBase=True) +print(p.getNumJoints(minitaur)) +p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6,cameraTargetPosition=[-0.064,.621,-0.2]) +motorJointId = 1 +p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=100000,force=0) + +p.resetJointState(minitaur,motorJointId,targetValue=0, targetVelocity=1) +angularDampingSlider = p.addUserDebugParameter("angularDamping", 0,1,0) +jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0,0.1,0) + +textId = p.addUserDebugText("jointVelocity=0",[0,0,-0.2]) +p.setRealTimeSimulation(1) +while (1): + frictionForce = p.readUserDebugParameter(jointFrictionForceSlider) + angularDamping = p.readUserDebugParameter(angularDampingSlider) + p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce) + p.changeDynamics(minitaur,motorJointId,linearDamping=0, angularDamping=angularDamping) + + time.sleep(0.01) + txt = "jointVelocity="+str(p.getJointState(minitaur,motorJointId)[1]) + prevTextId = textId + textId = p.addUserDebugText(txt,[0,0,-0.2]) + p.removeUserDebugItem(prevTextId) diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py index 72fa9e27e..55eecd07c 100644 --- a/examples/pybullet/examples/testrender.py +++ b/examples/pybullet/examples/testrender.py @@ -3,6 +3,7 @@ import matplotlib.pyplot as plt import pybullet pybullet.connect(pybullet.GUI) +pybullet.loadURDF("plane.urdf") pybullet.loadURDF("r2d2.urdf") camTargetPos = [0.,0.,0.] @@ -18,8 +19,8 @@ pixelWidth = 320 pixelHeight = 240 nearPlane = 0.01 farPlane = 1000 -lightDirection = [0,1,0] -lightColor = [1,1,1]#optional argument +lightDirection = [0.4,0.4,0] +lightColor = [.3,.3,.3]#1,1,1]#optional argument fov = 60 #img_arr = pybullet.renderImage(pixelWidth, pixelHeight) @@ -30,7 +31,7 @@ for pitch in range (0,360,10) : aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); #getCameraImage can also use renderer=pybullet.ER_BULLET_HARDWARE_OPENGL - img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pybullet.ER_TINY_RENDERER) + img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightAmbientCoeff = 0.3, lightDiffuseCoeff = 0.4, shadow=1, renderer=pybullet.ER_TINY_RENDERER) w=img_arr[0] h=img_arr[1] rgb=img_arr[2] From 41a1362bc551320b333a65825a4c59e03346a5af Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 24 Oct 2017 21:06:44 -0700 Subject: [PATCH 2/3] perform IK in local body-fixed frame For now, Jacobian, mass matrix and inverse dynamics return results in local coordinates of the tree. --- .../btMultiBodyTreeCreator.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 6 +++ .../PhysicsServerCommandProcessor.cpp | 43 ++++++++++++++++--- 3 files changed, 45 insertions(+), 8 deletions(-) diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index ef3ebf6b8..e432cf438 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -27,7 +27,9 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const } else { link.joint_type = FLOATING; } - btTransform transform(btmb->getBaseWorldTransform()); + btTransform transform=(btmb->getBaseWorldTransform()); + //compute inverse dynamics in body-fixed frame + transform.setIdentity(); link.parent_r_parent_body_ref(0) = transform.getOrigin()[0]; link.parent_r_parent_body_ref(1) = transform.getOrigin()[1]; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b27d7b6f2..847f42f8f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3638,6 +3638,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; + + + command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6c4461d4e..d7ea993ba 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7769,15 +7769,44 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); btVector3DoubleData endEffectorWorldPosition; - btVector3DoubleData endEffectorWorldOrientation; + btQuaternionDoubleData endEffectorWorldOrientation; - btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation(); - btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); + btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); + btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); + btTransform endEffectorWorld; + endEffectorWorld.setOrigin(endEffectorPosWorldOrg); + endEffectorWorld.setRotation(endEffectorOriWorldOrg); + + btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + + btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; + + btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); + + btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); - endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); - endEffectorOri.serializeDouble(endEffectorWorldOrientation); + endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); + endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); + btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); + btTransform targetWorld; + targetWorld.setOrigin(targetPosWorld); + targetWorld.setRotation(targetOrnWorld); + btTransform targetBaseCoord; + targetBaseCoord = tr.inverse()*targetWorld; + + btVector3DoubleData targetPosBaseCoord; + btQuaternionDoubleData targetOrnBaseCoord; + targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); + targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); + // Set joint damping coefficents. A small default // damping constant is added to prevent singularity // with pseudo inverse. The user can set joint damping @@ -7796,7 +7825,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, + ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, &q_current[0], numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, From 7a1dbf2e59186fb751f8846d8fce758c16fad1c6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 25 Oct 2017 00:32:47 -0700 Subject: [PATCH 3/3] 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