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

Adjust jacobian.py to show mismatch between joint state and link state.

Browse Source
This commit is contained in:
Jeffrey Bingham
2017-09-25 23:11:35 -07:00
parent 0b239e8bc0
commit 6da931d0bd
2 changed files with 38 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -25,6 +25,18 @@ def setJointPosition(robot, position, kp=1.0, kv=0.3):
"Expected torque vector of " "Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque))) "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)))
clid = p.connect(p.SHARED_MEMORY) clid = p.connect(p.SHARED_MEMORY)
if (clid<0): if (clid<0):
@@ -45,12 +57,30 @@ if (numJoints!=7):
setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId)) setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId))
p.stepSimulation() p.stepSimulation()
result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1) # Get the value of the link and joint state after a simulation step.
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result result0 = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1)
pos, vel, torq = getJointStates(kukaId) pos, vel, torq = getJointStates(kukaId)
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, pos, vel, vel)
print "Link velocity of CoM from getLinkState:" # 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.
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_vt
print "Link velocity of CoM from linearJacobian * q_dot:" print "Link linear velocity of CoM from linearJacobian * q_dot:"
print np.dot(np.array(jac_t), np.array(vel)) 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))

1
examples/pybullet/pybullet.c
View File

@@ -7416,6 +7416,7 @@ static PyMethodDef SpamMethods[] = {
" frame."}, " frame."},
{"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS, {"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS,
"resetJointState(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n"
"Reset the state (position, velocity etc) for a joint on a body " "Reset the state (position, velocity etc) for a joint on a body "
"instantaneously, not through physics simulation."}, "instantaneously, not through physics simulation."},