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