def computePoseReward(self): totalQuatDistance = 0 for i in [0]: rotation1 = self.targetPose[i:i+4] rotation2 = self.agentPose[i:i+4] diffQuat = p.getDifferenceQuaternion(rotation1,rotation2) _, quatMag = p.getAxisAngleFromQuaternion(diffQuat) # value is rounded because the calculations even for the same quaternions sometimes produce small errors totalQuatDistance += np.around(quatMag, decimals=2) return np.exp(-2*totalQuatDistance) # original value is -2*distance
def sync_brick(object_index, multibody): pos, orn = p.getBasePositionAndOrientation(multibody) rot_mat = np.reshape( p.getMatrixFromQuaternion( p.getDifferenceQuaternion(orn, brick_orns[object_index])), (3, 3)) vertices[object_index * sec: object_index * sec + sec] = \ (vertices[object_index * sec: object_index * sec + sec] - brick_centers[object_index]) @ rot_mat + pos brick_centers[object_index] = pos brick_orns[object_index] = orn
def sync_joints(actor_list, multibody): for joint in range(p.getNumJoints(multibody)): # `p.getLinkState` offers various information about the joints # as a list and the values in 4th and 5th index refer to the joint's # position and orientation respectively. pos, orn = p.getLinkState(multibody, joint)[4:6] rot_mat = np.reshape( p.getMatrixFromQuaternion( p.getDifferenceQuaternion(orn, linkOrientations[joint])), (3, 3)) vertices[joint * sec: joint * sec + sec] = \ (vertices[joint * sec: joint * sec + sec] - linkPositions[joint]) @ rot_mat + pos linkPositions[joint] = pos linkOrientations[joint] = orn
def calculate_ik(self, pos, rot=None, n_iters_outer=3, n_iters_inner=50, jd=0.005, init_arm_jpos=(0,-0.1,0.1,0,0) ): current_joint_states = self.get_joint_states() n_arm_joints = len(self.arm_joint_ids) self._teleport_arm(init_arm_jpos) for _ in range(n_iters_outer): jpos = pb.calculateInverseKinematics(self.robot_id, self.end_effector_link_index, pos, rot, maxNumIterations=n_iters_inner, jointDamping=self.n_joints*[jd], physicsClientId=self._client ) self._teleport_arm(jpos[:n_arm_joints]) solved_pos, solved_rot = self.pb_sim.get_hand_pose() info = { 'ik_pos' : solved_pos, 'ik_rot' : solved_rot, 'ik_pos_error' : np.linalg.norm(np.subtract(pos, solved_pos)), 'ik_rot_error' : 0, } if rot is not None: qd = pb.getDifferenceQuaternion(rot, solved_rot) info['ik_rot_error'] = 2 * np.arctan2(np.linalg.norm(qd[:3]), qd[3]) # reset arm to previous joint states self.set_joint_states(current_joint_states) return jpos[:n_arm_joints], info
def calculateAngularVelocity(self, ornStart, ornEnd, deltaTime): dorn = p.getDifferenceQuaternion(ornStart, ornEnd) axis, angle = p.getAxisAngleFromQuaternion(dorn) angVel = [(x*angle) / deltaTime for x in axis] return angVel
def move_arm_to_pose(self, desired_pos_gripper, desired_orient_gripper, desired_finger_angle, max_steps=1000): """ Move robot gripper to given position, orientation and opening. """ # x range (0.5, 0.7) # y range (-0.15, 0.25) # z range (0.15, 0.4) # grasp at 0.1 to 0.15 (0.125)? def set_joints(from_index, to_index): # set motor control for them for i in range(from_index, to_index + 1): info = p.getJointInfo(self.kuka.kukaUid, i) p.setJointMotorControl2(bodyUniqueId=self.kuka.kukaUid, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=joint_poses[i], targetVelocity=0, force=self.kuka.maxForce, maxVelocity=self.kuka.maxVelocity, positionGain=0.3, velocityGain=1) # TODO: for some reason tool TCP is not at tool tip #desired_pos_gripper[2] += 0.19 # TODO: left finger tip - base ## tool centering compensation #eul = p.getEulerFromQuaternion(desired_orient_gripper) #desired_pos_gripper[0] += -0.02 * np.sin(eul[2]) #desired_pos_gripper[1] += -0.02 * np.cos(eul[2]) desired_pos_gripper = np.asarray(desired_pos_gripper) #self.get_joint_info() # calculate TCP and shift desired position from TCP to base gripper_state = p.getLinkState(self.kuka.kukaUid, self.kuka.kukaGripperIndex) left_fingertip_state = p.getLinkState(self.kuka.kukaUid, self.left_fingertip_index) right_fingertip_state = p.getLinkState(self.kuka.kukaUid, self.right_fingertip_index) tcp = 1/2 * (np.asarray(left_fingertip_state[0]) + np.asarray(right_fingertip_state[0])) tcp_offs = tcp - np.asarray(gripper_state[0]) desired_pos_gripper -= tcp_offs # shift desired pos #desired_pos_gripper[2] -= 0.01 steps = 0 while True: steps += 1 if steps > max_steps: return False gripper_state = p.getLinkState(self.kuka.kukaUid, self.kuka.kukaGripperIndex) actual_pos_gripper = gripper_state[0] actual_orient_gripper = gripper_state[1] # calculate euclidean distance between desired and actual # gripper positions pos_diff = np.linalg.norm(np.array(desired_pos_gripper)-np.array(actual_pos_gripper)) # qaternions align when their w component is near 1.0 diff_quant = p.getDifferenceQuaternion(desired_orient_gripper, actual_orient_gripper) diff_quant_w = diff_quant[3] diff_eul = p.getEulerFromQuaternion(diff_quant) diff_eul_z = diff_eul[2] # finger? left_side_finger_angle = -p.getJointState(self.kuka.kukaUid, self.left_finger_index)[0] right_side_finger_angle = p.getJointState(self.kuka.kukaUid, self.right_finger_index)[0] left_side_diff = left_side_finger_angle - desired_finger_angle right_side_diff = right_side_finger_angle - desired_finger_angle # get closure force in x #forces = p.getJointState(self.kuka.kukaUid, self.left_finger_index)[2] #force_x = forces[0] # if pos, orient and fingers look good, we are done! # diff_quant_w > 0.9999 if (pos_diff < 0.001 and diff_eul_z < 0.0001 # and (force_x < 0 or force_x > 5)): and left_side_diff < 0.01 and right_side_diff < 0.01): return True # use IK to calculate target joint positions # (ignore null space) joint_poses = p.calculateInverseKinematics(self.kuka.kukaUid, self.kuka.kukaGripperIndex, desired_pos_gripper, desired_orient_gripper, self.kuka.ll, self.kuka.ul, self.kuka.jr, self.kuka.rp) set_joints(0, self.kuka.kukaGripperIndex) # gripper fingers p.setJointMotorControl2(self.kuka.kukaUid, 8, p.POSITION_CONTROL, targetPosition=-desired_finger_angle, force=self.kuka.fingerAForce) p.setJointMotorControl2(self.kuka.kukaUid, 11, p.POSITION_CONTROL, targetPosition=desired_finger_angle,force=self.kuka.fingerBForce) p.setJointMotorControl2(self.kuka.kukaUid, 10, p.POSITION_CONTROL, targetPosition=0, force=self.kuka.fingerTipForce) p.setJointMotorControl2(self.kuka.kukaUid, 13, p.POSITION_CONTROL, targetPosition=0, force=self.kuka.fingerTipForce) # step sim! p.stepSimulation() for _ in range(500): p.stepSimulation()
def quatdiff(q0, q1): return pb.getDifferenceQuaternion(q0, q1)
import numpy as np import pybullet as pb q1 = pb.getQuaternionFromEuler((0, 0, 0)) q2 = pb.getQuaternionFromEuler((0.001, 0.001, 0.001)) print(q1) print(q2) d = pb.getDifferenceQuaternion(q1, q2) theta = 2 * np.arccos(abs(sum([v1 * v2 for (v1, v2) in zip(q1, q2)]))) print(theta)
def GetLinkPosOrn(self, name): # check if given link name (link == joint) if "link" in name: name.replace("link", "joint", 1) idx = self.jointDict[name] # Check link exists if name in self.jointDict: state = p.getLinkState(self.linkage, idx, computeForwardKinematics=True) pos = list( np.array(state[4]) - np.array(self.initPosOrn[idx][self.POS_IDX])) #orn = np.array(state[5]) - self.initPosOrn[idx][self.ORN_IDX] orn = p.getDifferenceQuaternion(self.initPosOrn[idx][self.ORN_IDX], state[5]) logging.debug("%s pos=%s orn=%s", name, str(pos), str(p.getEulerFromQuaternion(orn))) else: logging.error("Unknown name input to GetLinkPosOrn") return [] return [pos, orn] # # TODO # # Find equivalent force-moment couple (through COM) # def TranslateForce(self, force) : # logging.debug("Force translations") # # Get contact forces # #TODO # def GetLinkContactForces(self, idx) : # # Compute collision forces # collisions = p.getContactPoints(bodyA = self.linkage, linkIndexA = idx) # for info in collisions : # orn = info[7] # force = info[9] # # Get force/torque in world coordinates # def CalcForceTorqueWorld(self, magn, idx) : # info = p.getJointInfo(self.linkage, idx) # #axis = info[] # # # Sum all forces/moments on link at last time step # # TODO # def GetLinkTotalDynamics(self, name) : # logging.debug("Getting total force") # info = np.zeros(6) # [Fx,Fy,Fz,Mx,My,Mz] # # Check if given link name (link == joint) # if "link" in name : # name.replace("link", "joint", 1) # # Make sure valid # if name not in self.jointDict : # logging.error("Unknown name input to GetLinkTotalForceMom") # return list(info) # # # Potential sources: joint/constraint rxn (when child & parent), contact, applied, gravity # # Go through linkJoint list # linkId = self.jointDict[name] # joints = self.linkJoints[linkId] # holds joint ids involving this link # # # Joints where link is parent # for num in range(len(joints)) : # # Need to switch direction of rxn force for parent # mult = 1 # if num == self.LINK_PARENT_IDX : # mult = -1 # for i in joints[num] : # if i >= 0: # # Reg joint # # Get applied/rxn force/torque # ret = [] # if i in self.actJointIds : # # Rxn forces # ret = p.getJointState(self.linkage, i) # dynamics = np.add(dynamics, np.array(ret[2])*mult) # # # Applied force/torque # # else : # # Rxn force/torque # if i in self.spherJointIds : # ret = p.getJointStateMultiDof(self.linkage, i) # dynamics = np.add(dynamics, np.array(ret[2])*mult) # elif i in self.dumbJointIds : # ret = p.getJointState(self.linkage, i) # dynamics = np.add(dynamics, np.array(ret[2])*mult) # # # Applied force/torque # applied = ret[3] # # else : # # Constraint # dynamics = np.add(dynamics, np.array(p.getConstraintState(i*-1))*-1) # # # Applied torques/forces from actuator # if self.actJointControl == p.TORQUE_CONTROL and linkId in self.actJointIds : # pass # # # Collisions # dynamics += self.GetLinkContactForces(linkId) # # # Gravity # dynamics[2] += self.GRAV_ACCELZ # # return dynamics # # # Alternate form of link dynamics # def GetLinkAccel2(self, name) : # # check if given link name (link == joint) # if "link" in name : # name.replace("link", "joint", 1) # # # check link exists # if "name" in self.jointDict : # # Get total forces/moments # info = self.GetLinkTotalDynamics(name) # # Mass and Inertial info # # # # Get joint and constraint dynamics # # returns [Fx,Fy,Fz,Mx,My,Mz] # # TODO: may need to rotate to match world frame orientation # def GetJointDynamics(self, name) : # # Determine if constraint or joint # state = [] # if "joint" in name : # ret = p.getJointState(self.jointDict[name]) # state = ret[2] # elif "constraint" in name : # state = p.getConstraintState(self.constraintDict[name]).tolist() # state.extend([0]*(self.DYN_STATE_SIZE-len(state))) # else: # logging.error("Unknown name input to GetJointDynamics") # # return state # # # Print joint states # def PrintJointDynamics(self) : # logging.info("Joint States") # id_list = list(range(p.getNumJoints(self.linkage))) # ret = p.getJointStates(self.linkage, id_list) # idx = 0 # for state in ret : # if(len(state) == 4) : # logging.debug("Id=%d velo=%s rxn=%s applied=%s", idx, str(state[1]), str(state[2]), str(state[3])) # elif(len(state) == 3) : # logging.debug("Id=%d velo=%s rxn=%s", idx, str(state[1]), str(state[2])) # else : # logging.error("Unexpected return list length %d", len(state)) # idx += 1 # print() # print()