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

Fix jacobian computation

Browse Source
This commit is contained in:
Hervé Audren
2018-03-26 18:40:42 +09:00
parent 680ce39e96
commit a2e9ecd957
2 changed files with 78 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,6 +8,15 @@ def getJointStates(robot):
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
def getMotorJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
@@ -20,47 +29,60 @@ def setJointPosition(robot, position, kp=1.0, kv=0.3):
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
def multiplyJacobian(jacobian, vector):
def multiplyJacobian(robot, jacobian, vector):
result = [0.0, 0.0, 0.0]
i = 0
for c in range(len(vector)):
if p.getJointInfo(robot, c)[3] > -1:
for r in range(3):
result[r] += jacobian[r][c] * vector[c]
result[r] += jacobian[r][i] * vector[c]
i += 1
return result
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
exit()
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId))
setJointPosition(kukaId, [0.1] * numJoints)
p.stepSimulation()
# Get the joint and link state directly from Bullet.
pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId)
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)
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, 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 (multiplyJacobian(jac_t, vel))
print (multiplyJacobian(kukaId, 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))
print (multiplyJacobian(kukaId, jac_r, vel))

50
examples/pybullet/pybullet.c
View File

@@ -8326,10 +8326,48 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
int szObVel = PySequence_Size(objVelocities);
int szObAcc = PySequence_Size(objAccelerations);
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
if (numJoints && (szLoPos == 3) && (szObPos == numJoints) &&
(szObVel == numJoints) && (szObAcc == numJoints))
int j=0;
int dofCountOrg = 0;
for (j=0;j<numJoints;j++)
{
int byteSizeJoints = sizeof(double) * numJoints;
struct b3JointInfo info;
b3GetJointInfo(sm, bodyUniqueId, j, &info);
switch (info.m_jointType)
{
case eRevoluteType:
{
dofCountOrg+=1;
break;
}
case ePrismaticType:
{
dofCountOrg+=1;
break;
}
case eSphericalType:
{
PyErr_SetString(SpamError,
"Spherirical joints are not supported in the pybullet binding");
return NULL;
}
case ePlanarType:
{
PyErr_SetString(SpamError,
"Planar joints are not supported in the pybullet binding");
return NULL;
}
default:
{
//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
}
}
}
if (dofCountOrg && (szLoPos == 3) && (szObPos == dofCountOrg) &&
(szObVel == dofCountOrg) && (szObAcc == dofCountOrg))
{
int byteSizeJoints = sizeof(double) * dofCountOrg;
int byteSizeVec3 = sizeof(double) * 3;
int i;
PyObject* pyResultList = PyTuple_New(2);
@@ -8341,7 +8379,7 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
double* angularJacobian = NULL;
pybullet_internalSetVectord(localPosition, localPoint);
for (i = 0; i < numJoints; i++)
for (i = 0; i < dofCountOrg; i++)
{
jointPositions[i] =
pybullet_internalGetFloatFromSequence(objPositions, i);
@@ -8423,11 +8461,11 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
else
{
PyErr_SetString(SpamError,
"calculateJacobian [numJoints] needs to be "
"calculateJacobian [numDof] needs to be "
"positive, [local position] needs to be of "
"size 3 and [joint positions], "
"[joint velocities], [joint accelerations] "
"need to match the number of joints.");
"need to match the number of DoF.");
return NULL;
}
}