diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 56fe09ee6..7082f0abb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3,6 +3,7 @@ #include "Bullet3Common/b3Scalar.h" #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Matrix3x3.h" +#include "Bullet3Common/b3Transform.h" #include #include "SharedMemoryCommands.h" @@ -534,6 +535,10 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle b3Assert(bodyIndex>=0); if (bodyIndex>=0) { + b3Transform wlf,com,inertial; + + + for (int i = 0; i < 3; ++i) { state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i]; @@ -544,6 +549,20 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i]; state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i]; } + com.setOrigin(b3MakeVector3(state->m_worldPosition[0],state->m_worldPosition[1],state->m_worldPosition[2])); + com.setRotation(b3Quaternion(state->m_worldOrientation[0],state->m_worldOrientation[1],state->m_worldOrientation[2],state->m_worldOrientation[3])); + inertial.setOrigin(b3MakeVector3(state->m_localInertialPosition[0],state->m_localInertialPosition[1],state->m_localInertialPosition[2])); + inertial.setRotation(b3Quaternion(state->m_localInertialOrientation[0],state->m_localInertialOrientation[1],state->m_localInertialOrientation[2],state->m_localInertialOrientation[3])); + wlf = com*inertial.inverse(); + for (int i = 0; i < 3; ++i) + { + state->m_worldLinkFramePosition[i] = wlf.getOrigin()[i]; + } + b3Quaternion wlfOrn = wlf.getRotation(); + for (int i = 0; i < 4; ++i) + { + state->m_worldLinkFrameOrientation[i] = wlfOrn[i]; + } } } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a517659bb..2cf694afe 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -325,11 +325,16 @@ struct b3VisualShapeInformation ///use URDF link frame = link COM frame * inertiaFrame.inverse() struct b3LinkState { + //m_worldPosition and m_worldOrientation of the Center Of Mass (COM) double m_worldPosition[3]; double m_worldOrientation[4]; double m_localInertialPosition[3]; double m_localInertialOrientation[4]; + + ///world position and orientation of the (URDF) link frame + double m_worldLinkFramePosition[3]; + double m_worldLinkFrameOrientation[4]; }; //todo: discuss and decide about control mode and combinations diff --git a/examples/pybullet/hello_pybullet.py b/examples/pybullet/hello_pybullet.py new file mode 100644 index 000000000..28cd0a049 --- /dev/null +++ b/examples/pybullet/hello_pybullet.py @@ -0,0 +1,23 @@ +import pybullet as p +from time import sleep + +physicsClient = p.connect(p.GUI) + +p.setGravity(0,0,-10) +planeId = p.loadURDF("plane.urdf") +cubeStartPos = [0,0,1] +cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) +boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) +cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) + +useRealTimeSimulation = 0 + +if (useRealTimeSimulation): + p.setRealTimeSimulation(1) + +while 1: + if (useRealTimeSimulation): + p.setGravity(0,0,-10) + sleep(0.01) # Time in seconds. + else: + p.stepSimulation() \ No newline at end of file diff --git a/examples/pybullet/inverse_kinematics.py b/examples/pybullet/inverse_kinematics.py new file mode 100644 index 000000000..8707507fb --- /dev/null +++ b/examples/pybullet/inverse_kinematics.py @@ -0,0 +1,85 @@ +import pybullet as p +import time +import math +from datetime import datetime + + +p.connect(p.GUI) +p.loadURDF("plane.urdf",[0,0,-0.3]) +kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) +p.resetBasePositionAndOrientation(kukaId,[0,0,0],[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] + +for i in range (numJoints): + p.resetJointState(kukaId,i,rp[i]) + +p.setGravity(0,0,0) +t=0. +prevPose=[0,0,0] +prevPose1=[0,0,0] +hasPrevPose = 0 +useNullSpace = 0 + +useOrientation = 1 +useSimulation = 1 +useRealTimeSimulation = 1 +p.setRealTimeSimulation(useRealTimeSimulation) +#trailDuration is duration (in seconds) after debug lines will be removed automatically +#use 0 for no-removal +trailDuration = 15 + + +while 1: + if (useRealTimeSimulation): + dt = datetime.now() + t = (dt.second/60.)*2.*math.pi + else: + t=t+0.1 + + if (useSimulation and useRealTimeSimulation==0): + p.stepSimulation() + + for i in range (25): + pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)] + #end effector points down, not up (in case useOrientation==1) + orn = p.getQuaternionFromEuler([0,-math.pi,0]) + + if (useNullSpace==1): + if (useOrientation==1): + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp) + else: + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) + else: + if (useOrientation==1): + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn) + else: + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos) + + if (useSimulation): + for i in range (numJoints): + p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=300,positionGain=0.3,velocityGain=1) + else: + #reset the joint state (ignoring all dynamics, not recommended to use during simulation) + for i in range (numJoints): + p.resetJointState(kukaId,i,jointPoses[i]) + + ls = p.getLinkState(kukaId,kukaEndEffectorIndex) + if (hasPrevPose): + p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) + p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) + prevPose=pos + prevPose1=ls[4] + hasPrevPose = 1 \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5f23c6b73..def84f666 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2020,6 +2020,8 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args,PyObject* PyObject* pyLinkStateWorldOrientation; PyObject* pyLinkStateLocalInertialPosition; PyObject* pyLinkStateLocalInertialOrientation; + PyObject* pyLinkStateWorldLinkFramePosition; + PyObject* pyLinkStateWorldLinkFrameOrientation; struct b3LinkState linkState; @@ -2094,11 +2096,26 @@ b3PhysicsClientHandle sm = 0; PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); } - pyLinkState = PyTuple_New(4); + pyLinkStateWorldLinkFramePosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) { + PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i, + PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); + } + + pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) { + PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i, + PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i])); + } + + + pyLinkState = PyTuple_New(6); PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); + PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition ); + PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); return pyLinkState; } @@ -4048,13 +4065,18 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int bodyIndex; int endEffectorLinkIndex; - PyObject* targetPosObj; - PyObject* targetOrnObj; + PyObject* targetPosObj=0; + PyObject* targetOrnObj=0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOO|i", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&physicsClientId)) + PyObject* lowerLimitsObj = 0; + PyObject* upperLimitsObj = 0; + PyObject* jointRangesObj = 0; + PyObject* restPosesObj = 0; + + static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId)) { return NULL; } @@ -4067,8 +4089,49 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, { double pos[3]; double ori[4]={0,1.0,0,0}; - - if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori)) + int hasPos =pybullet_internalSetVectord(targetPosObj,pos); + int hasOrn = pybullet_internalSetVector4d(targetOrnObj,ori); + + int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0; + int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0; + int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0; + int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0; + + int numJoints = b3GetNumJoints(sm, bodyIndex); + + + int hasNullSpace = 0; + double* lowerLimits = 0; + double* upperLimits = 0; + double* jointRanges = 0; + double* restPoses = 0; + + if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && + (szJointRanges == numJoints) && (szRestPoses == numJoints)) + { + int szInBytes = sizeof(double) * numJoints; + int i; + PyObject* pylist = 0; + lowerLimits = (double*)malloc(szInBytes); + upperLimits = (double*)malloc(szInBytes); + jointRanges = (double*)malloc(szInBytes); + restPoses = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) + { + lowerLimits[i] = + pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); + upperLimits[i] = + pybullet_internalGetFloatFromSequence(upperLimitsObj, i); + jointRanges[i] = + pybullet_internalGetFloatFromSequence(jointRangesObj, i); + restPoses[i] = + pybullet_internalGetFloatFromSequence(restPosesObj, i); + } + hasNullSpace = 1; + } + + if (hasPos) { b3SharedMemoryStatusHandle statusHandle; int numPos=0; @@ -4076,7 +4139,27 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int result; b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex); - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori); + + if (hasNullSpace) + { + if (hasOrn) + { + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); + } else + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command,numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + } + } else + { + if (hasOrn) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,endEffectorLinkIndex,pos,ori); + } else + { + b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos); + } + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); result = b3GetStatusInverseKinematicsJointPositions(statusHandle,