def get_jacobian(robot,
tool,
loc_pos=[0.0] * 3,
perturb=False,
mu=0.1,
sigma=0.1):
"""
returns two 3 by 20 full jacobian, translational, and rotational velocity respectively
for val, 20 actuated joints are ordered as follows:
2 Torso joints: [joint 56, joint 57]
7 left arm joints: [joint 41 - joint 47]
2 left grippers : [leftgripper & leftgripper2]
7 right arm joints: [joint 1 - joint 7]
2 right grippers : [rightgripper & rightgripper2]
robot: body unique id of robot
tool: linkID. The Jacobian is calculated w.r.t the CoM of linkID by default
loc_pos: the point on the specified link to compute the jacobian for, in
link local coordinates around its center of mass.
"""
pos, vel, torq = get_motor_joint_states(robot)
zero_vec = [0.0] * len(pos)
if perturb:
err = np.random.normal(mu, sigma, len(pos))
jac_t, jac_r = p.calculateJacobian(robot, tool, loc_pos,
list(pos + err), zero_vec, zero_vec)
return np.array(jac_t), np.array(jac_r)
jac_t, jac_r = p.calculateJacobian(robot, tool, loc_pos, pos, zero_vec,
zero_vec)
return np.array(jac_t), np.array(jac_r)
def jacobian(self, joint_angles=None):
"""
:return: Jacobian matrix for provided joint configuration
:rtype: ndarray (shape: 6x7)
:param joint_angles: Optional parameter. If different than None,
then returned jacobian will be evaluated at
given joint_angles. Otherwise the returned
jacobian will be evaluated at current robot
joint angles.
:type joint_angles: [float] * len(self.get_movable_joints)
"""
if joint_angles is None:
joint_angles = self.angles()
linear_jacobian, angular_jacobian = pb.calculateJacobian(
bodyUniqueId=self._id,
linkIndex=self._ee_link_idx,
localPosition=[0.0, 0.0, 0.0],
objPositions=joint_angles.tolist(),
objVelocities=np.zeros(self.n_joints()).tolist(),
objAccelerations=np.zeros(self.n_joints()).tolist(),
physicsClientId=self._uid)
jacobian = np.vstack(
[np.array(linear_jacobian),
np.array(angular_jacobian)])
return jacobian
def jacobian(self):
""" calculate the jacobian for the end effector position at current
joint state.
Returns:
jacobian tuple(mat3x, mat3x): translational jacobian ((dof), (dof), (dof))
and angular jacobian ((dof), (dof), (dof))
Notes:
localPosition: point on the specified link to compute the jacobian for, in
link coordinates around its center of mass. by default we assume we want it
around ee center of mass.
TODO: Verify this jacobian cdis what we want or if ee position is further from
com.
"""
motor_pos, motor_vel, motor_accel = self.getMotorJointStates()
linear, angular = pybullet.calculateJacobian(
bodyUniqueId=self._arm_id,
linkIndex=self._ee_index,
localPosition=[0.0, 0, 0],
objPositions=motor_pos,
objVelocities=[0] * len(motor_pos),
objAccelerations=[0] * len(motor_pos),
physicsClientId=self._physics_id)
jacobian = np.vstack((linear, angular))
jacobian = jacobian[:7, :7]
return jacobian
def test_ee_jacobian(self, request, setup_dict, test_info):
robot_model, sim, num_dofs, test_case = extract_setup_dict(setup_dict)
for ee_link_idx, ee_link_name in test_info.link_list:
# Bullet sim
set_pybullet_state(sim, robot_model, num_dofs, test_case.joint_pos,
test_case.joint_vel)
bullet_jac_lin, bullet_jac_ang = p.calculateJacobian(
bodyUniqueId=sim.robot_id,
linkIndex=ee_link_idx,
localPosition=[0, 0, 0],
objPositions=test_case.joint_pos,
objVelocities=test_case.joint_vel,
objAccelerations=[0] * num_dofs,
physicsClientId=sim.pc_id,
)
# Differentiable model
model_jac_lin, model_jac_ang = robot_model.compute_endeffector_jacobian(
torch.Tensor(test_case.joint_pos).reshape(1, num_dofs),
ee_link_name)
# Compare
assert np.allclose(model_jac_lin.detach().numpy(),
np.asarray(bullet_jac_lin),
atol=1e-7)
assert np.allclose(model_jac_ang.detach().numpy(),
np.asarray(bullet_jac_ang),
atol=1e-7)
def test_ee_jacobian(self, request, setup_dict):
robot_model = setup_dict["robot_model"]
test_case = setup_dict["test_case"]
ee_id = dof
test_angles, test_velocities = (
test_case["joint_angles"],
test_case["joint_velocities"],
)
model_jac_lin, model_jac_ang = robot_model.compute_endeffector_jacobian(
torch.Tensor(test_angles).reshape(1, dof), "endEffector")
bullet_jac_lin, bullet_jac_ang = p.calculateJacobian(
bodyUniqueId=robot_id,
linkIndex=ee_id,
localPosition=[0, 0, 0],
objPositions=test_angles,
objVelocities=test_velocities,
objAccelerations=[0] * dof,
physicsClientId=pc_id)
assert np.allclose(model_jac_lin.detach().numpy(),
np.asarray(bullet_jac_lin),
atol=1e-7)
assert np.allclose(model_jac_ang.detach().numpy(),
np.asarray(bullet_jac_ang),
atol=1e-7)
def get_state(self, t):
"""
Retrieves current system state
"""
joint_pos = [
p.getJointState(self.o.kukaobject.kukaId, i)[0]
for i in range(self._numJoints)
]
joint_vel = [
p.getJointState(self.o.kukaobject.kukaId, i)[1]
for i in range(self._numJoints)
]
eepos = p.getLinkState(
self.o.kukaobject.kukaId, self.o.kukaobject.kukaEndEffectorIndex)[
0] # is that correct?? or is it another index??
eeorn = p.getLinkState(self.o.kukaobject.kukaId,
self.o.kukaobject.kukaEndEffectorIndex)[1]
result = p.getLinkState(self.o.kukaobject.kukaId,
self.o.kukaobject.kukaEndEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
zero_vec = [0.0] * len(joint_pos)
jac_t, jac_r = p.calculateJacobian(
self.o.kukaobject.kukaId, self.o.kukaobject.kukaEndEffectorIndex,
com_trn, joint_pos, zero_vec, zero_vec) # what to use???
anchor_object_p3d = np.zeros(7, )
rgb_img, depth_img = self.get_images(1)
rgb_crop, depth_crop = self.get_images(0)
# Compute visual features if desired
if self.ptconf.COMPUTE_FEATURES:
a_pred = self.feature_fn(self.tcn, rgb_crop)[0]
image_feature = np.squeeze(a_pred)
else:
image_feature = np.zeros(self.emb_dim)
tgt = self.hyperparams['debug_cost_tgt'][t + 1]
# print("centroid diff: {}".format(centroid_diff[:3]))
# print("tgt: {}".format(tgt[:3]))
print("cost current step {}: {}".format(t + 1,
np.linalg.norm(a_pred - tgt)))
object_pose = np.array(self.getObjectPose(self.h.duckid)[:3])
centroid_diff = np.zeros(7, )
object_pose = duck_centroid # Cube - Bowl (# Look at Mask RCNN General config for ID order)
# Collect all states
stateX = np.concatenate([
np.array(joint_pos),
np.array(joint_vel),
np.array(eepos + eeorn), image_feature, centroid_diff,
anchor_object_p3d
]).flatten()
image_data = [rgb_crop]
return stateX, jac_t, image_data
def update(self, action):
joint_positions = [
state[0] for state in p.getJointStates(self.uid, self.joint_ids)
]
joint_velocities = [
state[1] for state in p.getJointStates(self.uid, self.joint_ids)
]
n_joints = len(joint_positions)
J = p.calculateJacobian(self.uid, self.end_frame,
self.offset_admittance_point, joint_positions,
[0.] * n_joints, [0.] * n_joints)
T_g = p.calculateInverseDynamics(self.uid, joint_positions,
[0.] * n_joints, [0.] * n_joints)
T_cmd = action['force'].dot(np.array(J[0])) + action['torque'].dot(
np.array(J[1]))
T_pos = (self.target_pose - np.array(joint_positions)) * self.kp
T_vel = (np.array(joint_velocities)) * -self.kd
p.setJointMotorControlArray(self.uid,
self.joint_ids,
p.TORQUE_CONTROL,
forces=T_cmd + T_g + T_pos + T_vel)
def run_ee_link(kukaId,eevel):
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] * len(mpos)
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
import numpy as np
J = jac_t + jac_r
Ja = np.linalg.inv(np.array(J))
Q = np.matmul(Ja,eevel)
maxForce = 500
for i in range(1,7):
p.setJointMotorControl2(bodyUniqueId=kukaId,
jointIndex=i,
controlMode=p.VELOCITY_CONTROL,
targetVelocity = Q[i-1], #check if mapping is proper
force = maxForce)
def traj_tracking_vel(self, targetPos, targetQuat, posGain=20, velGain=5):
eePos, eeQuat = self._panda.get_ee()
eePosError = targetPos - eePos
eeOrnError = log_rot(quat2rot(targetQuat).dot(
(quat2rot(eeQuat).T))) # in spatial frame
jointPoses = self._panda.get_arm_joints() + [0, 0, 0] # add fingers
eeState = p.getLinkState(self._pandaId,
self._panda.pandaEndEffectorLinkIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
# 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] * len(jointPoses)
jac_t, jac_r = p.calculateJacobian(
self._pandaId, self._panda.pandaEndEffectorLinkIndex, eeState[2],
jointPoses, zero_vec,
zero_vec) # use localInertialFrameOrientation
jac_sp = full_jacob_pb(
jac_t, jac_r)[:, :7] # 6x10 -> 6x7, ignore last three columns
try:
jointDot = np.linalg.pinv(jac_sp).dot((np.hstack(
(posGain * eePosError,
velGain * eeOrnError)).reshape(6, 1))) # pseudo-inverse
except np.linalg.LinAlgError:
jointDot = np.zeros((7, 1))
return jointDot
def _calc_jacobian(self, q=None, dq=None, localPos=None):
if q is None:
q = self.q
if dq is None:
dq = self.dq
if localPos is None:
localPos = [0, 0, 0]
# append zeros to q to fit pybullet
q = self._pad_zeros_to_joint_values(q)
dq = self._pad_zeros_to_joint_values(dq)
linear_jacobian, angular_jacobian = pb.calculateJacobian(
bodyUniqueId=self._pb_sawyer,
linkIndex=self._link_id_dict[self.ee_name],
localPosition=localPos,
objPositions=q,
objVelocities=dq,
objAccelerations=[0] * self.num_free_joints,
physicsClientId=self._clid)
# Save full linear jacoban as 3 x 10
self._linear_jacobian = np.reshape(linear_jacobian,
(3, self.num_free_joints))
# save full angular jacobian as 3x10
self._angular_jacobian = np.reshape(angular_jacobian,
(3, self.num_free_joints))
# Save full jacobian as 6x10
self._jacobian = np.vstack(
(self._linear_jacobian, self._angular_jacobian))
return linear_jacobian, angular_jacobian
def get_jacobians(self):
# compute joint states and joint info
joint_states = p.getJointStates(self.ik_robot,
range(p.getNumJoints(self.ik_robot)))
joint_infos = [
p.getJointInfo(self.ik_robot, i)
for i in range(p.getNumJoints(self.ik_robot))
]
# get joint states for relevant joints
joint_states = [
j for j, i in zip(joint_states, joint_infos) if i[3] > -1
]
if self.debug:
relevant_joints = [(i[0], i[1]) for i in joint_infos if i[3] > -1]
print(relevant_joints)
# get joint positions from joint states
joint_positions = [state[0] for state in joint_states]
# set zero vector (for joint velocities and accelerations)
zero_vec = [0.0] * len(joint_positions)
# set local position on relevant joint
local_pos = [0.0, 0.0, 0.0]
# get appropriate end-effector
if self.control_arm == "left":
ee = self.effector_left
else: # self.control_arm == "right"
ee = self.effector_right
# compute linear and angular Jacobians for all relevant joints
J_lin_reljoints, J_ang_reljoints = p.calculateJacobian(
self.ik_robot, ee, local_pos, joint_positions, zero_vec, zero_vec)
if self.debug:
print("linear Jacobian row size: ", len(J_lin_reljoints),
"col size: ", len(J_lin_reljoints[0]))
print("angular Jacobian size: ", len(J_ang_reljoints),
"col size: ", len(J_ang_reljoints[0]))
# first column corresponds to head_pan, which we do not care about for manipulation
# initialize tuples for linear and angular Jacobians
J_lin = ()
J_ang = ()
# remove first item in each row, which corresponds to head_pan
for r in J_lin_reljoints:
J_lin = J_lin + (r[1:], )
for r in J_ang_reljoints:
J_ang = J_ang + (r[1:], )
if self.debug:
print("linear Jacobian row size: ", len(J_lin), "col size: ",
len(J_lin[0]))
print("angular Jacobian size: ", len(J_ang), "col size: ",
len(J_ang[0]))
return J_lin, J_ang
def _get_link_jacobian(self, link_index):
"""
Get the Jacobian of a link frame in the form 6xN [J_trans; J_rot]
"""
zero_vec = [0.0] * len(self.jnt_pos)
jac_t, jac_r = p.calculateJacobian(self.arm[0], link_index, [0, 0, 0], self.jnt_pos, zero_vec, zero_vec)
J = np.concatenate((jac_t, jac_r), axis=0)
return J
def J_toe(self):
J = [p.calculateJacobian(GP.robot,i,
[0,0,0], # the point on the specified link
list(self.state[:7]),
[0 for a in range(7)], # joint velocities
[0 for a in range(7)]) # desired joint accelerations
for i in [5,8]] # calculates the linear jacobian and the angular jacobian
# Each element of J is tuple (J_x, J_r), the jacobian of transition and rotation
return np.concatenate([np.array(j[0])[GP.PRISMA_AXIS] for j in J], axis = 0)
def get_end_effector_jacobian(self):
"""
Grab the rotational and translational jacobian of the end effector
"""
zero_vec = [0.0] * len(self.jnt_pos)
self.get_end_effector_state()
jac_t, jac_r = p.calculateJacobian(self.arm[0], self._ee_link_index, self.ee_com_trn, self.jnt_pos, zero_vec,
zero_vec)
J = np.concatenate((jac_t, jac_r), axis=0)
return J
def get_position_jacobian(self):
#position jacobian w.r.t the joint position at end-effector link
#note there would be an offset to the real contact point
zero_vec = [0.0] * self.nJointsPerArm
com_pos = [0.0] * 3
com_pos[2] = 0.02
#warning: we cannot directly feed numpy array to calculateJacobian. it dumps a segment fault...
obs_lst = list(self.observation)
jac_t_1, jac_r_1 = p.calculateJacobian(self.arm1Uid,
self.nJointsPerArm - 1, com_pos,
obs_lst[:7], zero_vec, zero_vec)
jac_t_2, jac_r_2 = p.calculateJacobian(self.arm2Uid,
self.nJointsPerArm - 1, com_pos,
obs_lst[7:14], zero_vec,
zero_vec)
jac_t_3, jac_r_3 = p.calculateJacobian(self.arm3Uid,
self.nJointsPerArm - 1, com_pos,
obs_lst[14:21], zero_vec,
zero_vec)
return jac_t_1, jac_t_2, jac_t_3
def compute_jacobian(robot, link, positions=None):
joints = robot.get_movable_joints()
if positions is None:
positions = robot.get_joint_positions(joints)
assert len(joints) == len(positions)
velocities = [0.0] * len(positions)
accelerations = [0.0] * len(positions)
translate, rotate = p.calculateJacobian(robot.id, link.linkID, geometry.unit_point(), positions,
velocities, accelerations, physicsClientId=CLIENT)
#movable_from_joints(robot, joints)
return list(zip(*translate)), list(zip(*rotate)) # len(joints) x 3
def get_bullet_jacobian(joint_pos, joint_vel):
set_pybullet_state(sim, robot_model, num_dofs, joint_pos, joint_vel)
return p.calculateJacobian(
bodyUniqueId=sim.robot_id,
linkIndex=ee_link_idx,
localPosition=[0, 0, 0],
objPositions=joint_pos,
objVelocities=joint_vel,
objAccelerations=[0] * num_dofs,
physicsClientId=sim.pc_id,
)
def getJacobian(self, joint_pos):
eeState = p.getLinkState(self.robot_id, self.end_eff_index)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot = eeState
zero_vec = [0.0] * len(joint_pos)
jac_t, jac_r = p.calculateJacobian(self.robot_id,
self.end_eff_index, com_trn,
list(joint_pos), zero_vec, zero_vec)
J_t = np.asarray(jac_t)
J_r = np.asarray(jac_r)
J = np.concatenate((J_t, J_r), axis=0)
print('Jacobian:', J)
return J
def _get_jacobian(self, observation):
ret = []
for tip in self.env.platform.simfinger.pybullet_tip_link_indices:
J, _ = p.calculateJacobian(
self.env.platform.simfinger.finger_id, tip,
np.zeros(3).tolist(), observation["robot_position"].tolist(),
observation["robot_velocity"].tolist(),
np.zeros(len(observation["robot_position"])).tolist(),
self.env.platform.simfinger._pybullet_client_id)
ret.append(J)
ret = np.vstack(ret)
return ret
def _read_jacobian(self):
linear_jacobian, angular_jacobian = p.calculateJacobian(
self.panda, 11, [0, 0, 0], list(self.state['joint_position']),
[0] * 9, [0] * 9)
linear_jacobian = np.asarray(linear_jacobian)[:, :7]
angular_jacobian = np.asarray(angular_jacobian)[:, :7]
full_jacobian = np.zeros((6, 7))
full_jacobian[0:3, :] = linear_jacobian
full_jacobian[3:6, :] = angular_jacobian
self.jacobian['full_jacobian'] = full_jacobian
self.jacobian['linear_jacobian'] = linear_jacobian
self.jacobian['angular_jacobian'] = angular_jacobian
def _get_jacobians(self, observation):
# Returns: numpy(3*num_fingers X num_joints_per_finger)
ret = []
for tip in self.finger.pybullet_tip_link_indices:
J, _ = pybullet.calculateJacobian(
self.finger.finger_id, tip,
np.zeros(3).tolist(),
observation["observation"]["position"].tolist(),
observation["observation"]["velocity"].tolist(),
np.zeros(len(observation["observation"]["position"])).tolist())
ret.append(J)
ret = np.vstack(ret)
return ret
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf",
"TwoJointRobot_w_fixedJoints.urdf", "kuka_iiwa/model.urdf",
"kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId,
[0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
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] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex,
com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def angular_jacobian(self):
""" The angular jacobian rdot= J_r*qdot
"""
motor_pos = self.motor_joint_positions
_, angular = pybullet.calculateJacobian(
bodyUniqueId=self._arm_id,
linkIndex=self._ee_index,
localPosition=[0, 0, 0],
objPositions=motor_pos,
objVelocities=[0] * len(motor_pos),
objAccelerations=[0] * len(motor_pos),
physicsClientId=self._physics_id)
return np.asarray(angular)[:, :7]
def update_jacobians(self):
"""Permet de récolter la matrice jacobienne de translation par rapport à chacun des points
Cette fonction doit être vérifiée !"""
velVec = [0 for i in range(0, 3)]
accVec = velVec
self.update_joint_pos(
self.world.ppsId
) #On update la position des joints histoire d'être sur
self.update_joint_frame()
for index in range(1, p.getNumJoints(
self.world.ppsId)): #On parcourt 3 éléments
localPoint = self.CoM_to_point(index,
self.arrayRepLeoni[index - 1][0])
localCoM = self.frame_to_CoM(index)
tempJac = p.calculateJacobian(self.world.ppsId, index, localPoint,
self.jointPos, velVec, accVec)[0]
self.arrayJacRep[index - 1] = tempJac
tempJac = p.calculateJacobian(self.world.ppsId, index, localCoM,
self.jointPos, velVec,
accVec)[0] #Always at CoM
self.arrayJacAtt[index - 1] = tempJac
#logging.info("Test localInertialFramePosition: " + str(self.frame_to_CoM(index+1)))
return
def calculateJacobian(self, q):
pos, vel, torq = self.getJointStates(self.kinova)
mpos, mvel, mtorq = self.getMotorJointStates(self.kinova)
# get the end effector link state
result = p.getLinkState(self.kinova,
self.kinovaEndEffector,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(self.kinova, self.kinovaEndEffector, com_trn, mpos, zero_vec, zero_vec)
self.jacobian = jac_t
# calculate the linear velocity of the endeffector
print(self.multiplyJacobian(self.kinova, jac_t, vel))
return (jac_t, jac_r)
def stabilization_control(time_horizon = 2.0):
print('dynamics: ' + str(p.getDynamicsInfo(car_id, -1)))
joints = len(p.getJointStates(car_id, wheel_indices))
print('joints: ' + str(joints))
print('numJoints: ' + str(p.getNumJoints(car_id)))
# import ipdb; ipdb.set_trace()
# for dof in range(0, 16):
# target_angle = [0.0] * dof
# target_velocity = [0.0] * dof
# target_accel = [0.0] * dof
# try:
# torque = p.calculateInverseDynamics(car_id, target_angle, target_velocity, target_accel)
# print("worked with {}".format(dof))
# except SystemError as e:
# pass # print(e)
# setJointPosition(car_id, [0.1])
p.stepSimulation()
pos, vel, torq = getJointStates(car_id)
mpos, mvel, mtorq = getMotorJointStates(car_id)
result = p.getLinkState(car_id, 0, computeLinkVelocity=1, computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
print('numJoints: {}'.format(p.getNumJoints(car_id)))
vehicle_com_relative_to_first_wheel = [-0.1015, -0.09255, 0]
zero_vec = [0.0] * len(mpos)
for i in range(p.getNumJoints(car_id)):
wheel_jacobian = np.array(p.calculateJacobian(car_id, i, com_trn, mpos, zero_vec, zero_vec))
wheel_j_trans_I = np.linalg.pinv(wheel_jacobian[0])
wheel_j_rot_I = np.linalg.pinv(wheel_jacobian[1])
print(wheel_j_rot_I)
# import ipdb; ipdb.set_trace()
# print("link angular velocity of () from ")
# print(multiplyJacobian(car_id, wheel_jacobian[1], vel))
torque = [100]
return torque
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
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] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def formulate_jacobian_matrix(self, robot_id, link_index, robot_state, link_state, cp, trajectory, group,
time_step_count, zero_vec):
link_position_in_world_frame = link_state[4]
link_orentation_in_world_frame = link_state[5]
closest_point_on_link_in_link_frame, _ = self.get_point_in_local_frame(
link_position_in_world_frame, link_orentation_in_world_frame,
cp)
position_jacobian, _ = sim.calculateJacobian( robot_id, link_index,
closest_point_on_link_in_link_frame,
robot_state, zero_vec, zero_vec)
position_jacobian = [jac[-len(robot_state):] for jac in position_jacobian]
group_jacobian = [[x[self.joint_name_to_jac_id[g]] for g in group] for x in position_jacobian]
jacobian_matrix = self.get_jacobian_matrix(group_jacobian, len(trajectory), len(group), time_step_count)
return jacobian_matrix
def test_jecobian(kukaId):
# 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] * len(mpos)
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
J = jac_t + jac_r
print(np.shape(J))
def updateState(self, updateJ = False):
"""Retrieves/Measures the end-effector position
and derives the configuration position and Jacobian matrix from it.
(Ideally, configuration position can also be measured directly)
Parameters
----------
updateJ : bool
If True, Jacobian matrix is updated based on measurements
(Default: False)
Returns
-------
None
"""
# Get link state
linkState = p.getLinkState(
self.robot,
self.robotEndEffectorIndex,
computeLinkVelocity = 1,
computeForwardKinematics = 1
)
# Save x value and find q
self.x = linkState[4]
self.q = self.ik(self.x)
# Calculate Jacobian
if updateJ:
J, _ = p.calculateJacobian(
bodyUniqueId = self.robot,
linkIndex = self.robotEndEffectorIndex,
localPosition = list(linkState[2]),
objPositions = list(self.q),
objVelocities = [0.] * len(list(self.q)),
objAccelerations = [0.] * len(list(self.q))
)
self.J = np.array(J)
def jacobian(side):
if SIMULATION == True:
joint_positions = joint_states(
Side.LEFT)[:, 0].tolist() + joint_states(Side.RIGHT)[:,
0].tolist()
if side == Side.LEFT:
link_index = 4
if side == Side.RIGHT:
link_index = 9
j_linear, j_angular = p.calculateJacobian(bipedId, link_index,
[0, 0, 0], joint_positions,
10 * [0], 10 * [0])
if side == Side.LEFT:
j_linear = np.array(j_linear)[:, 6:11]
j_angular = np.array(j_angular)[:, 6:11]
if side == Side.RIGHT:
j_linear = np.array(j_linear)[:, 11:16]
j_angular = np.array(j_angular)[:, 11:16]
return np.array([*j_linear, *j_angular])
else:
joint_positions = joint_states(side)[:, 0].tolist()
return np.array(J(*joint_positions)).astype(np.float64)
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
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] * 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(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(kukaId, jac_r, vel))