Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fix the translation jacobian.

Browse Source
This commit is contained in:
Jeffrey Bingham
2017-09-27 22:23:22 -07:00
parent 6da931d0bd
commit d8b80bce40
3 changed files with 35 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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);

61
examples/pybullet/examples/jacobian.py
View File

@@ -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))

8
examples/pybullet/pybullet.c
View File

@@ -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"