diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7f3223b59..790991d37 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6667,6 +6667,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } // Only calculate if the localPosition is non-zero. if (btInverseDynamics::maxAbs(localPosition) > 0.0) { + // Write the localPosition into world coordinates. + btInverseDynamics::mat33 world_rotation_body; + tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body); + localPosition = world_rotation_body * localPosition; + // Correct the translational jacobian. btInverseDynamics::mat33 skewCrossProduct; btInverseDynamics::skew(localPosition, &skewCrossProduct); btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs); diff --git a/examples/pybullet/examples/jacobian.py b/examples/pybullet/examples/jacobian.py index d0b4f68ce..e52caccdf 100644 --- a/examples/pybullet/examples/jacobian.py +++ b/examples/pybullet/examples/jacobian.py @@ -1,7 +1,7 @@ import sys sys.path.append('/Users/bingjeff/Documents/projects/walkman/bullet3/bin/') import pybullet as p -import numpy as np + def getJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) @@ -25,22 +25,17 @@ def setJointPosition(robot, position, kp=1.0, kv=0.3): "Expected torque vector of " "length {}, got {}".format(num_joints, len(torque))) -def setJointState(robot, position, velocity=None): - num_joints = pb.getNumJoints(robot) - if velocity is None: - velocity = [0.0] * num_joints - if len(position) == num_joints and len(velocity) == num_joints: - for c in range(num_joints): - pb.resetJointState(robot, c, position[c], velocity[c]) - else: - print("Not setting state. " - "Expected vectors of length {}, " - "got pos={}, vel={}".format(num_joints, len(position), len(velocity))) +def multiplyJacobian(jacobian, vector): + result = [0.0, 0.0, 0.0] + for c in range(len(vector)): + for r in range(3): + result[r] += jacobian[r][c] * vector[c] + return result clid = p.connect(p.SHARED_MEMORY) if (clid<0): - p.connect(p.GUI) + p.connect(p.DIRECT) time_step = 0.001 gravity_constant = -9.81 p.resetSimulation() @@ -53,34 +48,24 @@ kukaEndEffectorIndex = 6 numJoints = p.getNumJoints(kukaId) if (numJoints!=7): exit() - +# Set a joint target for the position control and step the sim. setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId)) p.stepSimulation() - -# Get the value of the link and joint state after a simulation step. -result0 = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1) +# Get the joint and link state directly from Bullet. pos, vel, torq = getJointStates(kukaId) - -# Now "reset" the simulation to have the same joint state and read the link state. -setJointState(kukaId, pos, vel) -result1 = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1) -link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result1 - -# Show that the link origin is different for supposedly the same joint state. -print "Link origin - stepSimulation:" -print result0[0] -print "Link origin - resetJointState:" -print result1[0] - -# Using the "reset" system the body velocities (should) match. +result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) +link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result +# Get the Jacobians for the CoM of the end-effector link. +# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn. +# The localPosition is always defined in terms of the link frame coordinates. zero_vec = [0.0] * numJoints jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, pos, zero_vec, zero_vec) -print "Link linear velocity of CoM from getLinkState:" -print link_vt -print "Link linear velocity of CoM from linearJacobian * q_dot:" -print np.dot(np.array(jac_t), np.array(vel)) -print "Link angular velocity of CoM from getLinkState:" -print link_vr -print "Link angular velocity of CoM from angularJacobian * q_dot:" -print np.dot(np.array(jac_r), np.array(vel)) +print ("Link linear velocity of CoM from getLinkState:") +print (link_vt) +print ("Link linear velocity of CoM from linearJacobian * q_dot:") +print (multiplyJacobian(jac_t, vel)) +print ("Link angular velocity of CoM from getLinkState:") +print (link_vr) +print ("Link angular velocity of CoM from angularJacobian * q_dot:") +print (multiplyJacobian(jac_r, vel)) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ea4917a88..f21d294c3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -7411,6 +7411,12 @@ static PyMethodDef SpamMethods[] = { "Get the state (position, velocity etc) for multiple joints on a body."}, {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS, + "position_linkcom_world, world_rotation_linkcom, " + "position_linkcom_frame, frame_rotation_linkcom, " + "position_frame_world, world_rotation_frame, " + "linearVelocity_linkcom_world, angularVelocity_linkcom_world = " + "getLinkState(objectUniqueId, linkIndex, computeLinkVelocity=0, " + "computeForwardKinematics=0, physicsClientId=0)\n" "Provides extra information such as the Cartesian world coordinates" " center of mass (COM) of the link, relative to the world reference" " frame."}, @@ -7564,7 +7570,7 @@ static PyMethodDef SpamMethods[] = { "Args:\n" " bodyIndex - a scalar defining the unique object id.\n" " linkIndex - a scalar identifying the link containing the local point.\n" - " localPosition - a list of [x, y, z] of the coordinates of the local point.\n" + " localPosition - a list of [x, y, z] of the coordinates defined in the link frame.\n" " objPositions - a list of the joint positions.\n" " objVelocities - a list of the joint velocities.\n" " objAccelerations - a list of the joint accelerations.\n"