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