def ik_robot_eef_joint_cartesian_pose(self):
"""
Returns the current cartesian pose of the last joint of the ik robot with respect
to the base frame as a (pos, orn) tuple where orn is a x-y-z-w quaternion.
"""
out = []
for eff in [self.effector_right, self.effector_left]:
eef_pos_in_world = np.array(p.getLinkState(self.ik_robot, eff)[0])
eef_orn_in_world = np.array(p.getLinkState(self.ik_robot, eff)[1])
eef_pose_in_world = T.pose2mat(
(eef_pos_in_world, eef_orn_in_world))
base_pos_in_world = np.array(
p.getBasePositionAndOrientation(self.ik_robot)[0])
base_orn_in_world = np.array(
p.getBasePositionAndOrientation(self.ik_robot)[1])
base_pose_in_world = T.pose2mat(
(base_pos_in_world, base_orn_in_world))
world_pose_in_base = T.pose_inv(base_pose_in_world)
eef_pose_in_base = T.pose_in_A_to_pose_in_B(
pose_A=eef_pose_in_world, pose_A_in_B=world_pose_in_base)
out.extend(T.mat2pose(eef_pose_in_base))
return out
def get_dx(self):
# compute gradient (angle to rotate by)
grad = -self.gradient()
# compute the axis to rotate around
r_axis = np.cross(self.ee_point_axis, self.align_axis)
r_axis = T.norm(r_axis)
if self.debug:
print("point axis: ", self.ee_point_axis, "align axis: ",
self.align_axis, "rotate axis: ", r_axis)
# compute transform based on angle and axis, transform w.r.t. end-effector
rot_wrt_ee = T.rotation_matrix(grad, r_axis)
# pose of end-effector w.r.t. world
ee_wrt_world = T.pose2mat((self.curr_pos, self.curr_quat))
# compute rotation w.r.t. world
# T(rot w.r.t. world) = T(end-effector w.r.t. world) * T(rot w.r.t. end-effector)
# (rotation in world frame) = (end-effector in world frame) * (rotation in end-effector frame)
rot_wrt_world = T.pose_in_A_to_pose_in_B(rot_wrt_ee, ee_wrt_world)
# get rotation w.r.t. world
_, dx_quat = T.mat2pose(rot_wrt_world)
# we do not want to change linear position
dx_pos = np.zeros(3)
# compute dx
dx = np.hstack([dx_pos, dx_quat])
return dx
def get_align_axis(self):
# align pose w.r.t. world
align_wrt_world = T.pose2mat(
(self.align_pos, T.euler_to_quat([0, 0, 0])))
# pose of end-effector w.r.t. world
ee_wrt_world = T.pose2mat((self.curr_pos, self.curr_quat))
if self.debug:
print("align pos: ", self.align_pos)
print("curr pos: ", self.curr_pos)
# pose of world w.r.t. end-effector
world_wrt_ee = T.pose_inv(ee_wrt_world)
# align pose w.r.t. end-effector
# T(align pose w.r.t end-effector) = T(world w.r.t. end-effector) * T(align pose w.r.t. world)
# (align pose in end-effector frame) = (world pose in end-effector frame) * (align pose in world frame)
align_wrt_ee = T.pose_in_A_to_pose_in_B(align_wrt_world, world_wrt_ee)
# get axis from pose
align_axis, _ = T.mat2pose(align_wrt_ee)
align_axis = T.norm(align_axis)
# set align axis
self.align_axis = align_axis
return
def pose_in_base_from_pose_in_unity(self, pose_in_unity):
# get base pose in unity
base_pos_in_unity = self.sim.data.get_body_xpos("base")
base_rot_in_unity = self.sim.data.get_body_xmat("base").reshape((3, 3))
base_pose_in_unity = T.make_pose(base_pos_in_unity, base_rot_in_unity)
# get pose of unity in base
unity_pose_in_base = T.pose_inv(base_pose_in_unity)
# pose in base = pose of unity in base * pose in unity
pose_in_base = T.pose_in_A_to_pose_in_B(pose_in_unity,
unity_pose_in_base)
return pose_in_base
def ik_robot_eef_joint_cartesian_pose(self):
"""
Returns the current cartesian pose of the last joint of the ik robot with respect to the base frame as
a (pos, orn) tuple where orn is a x-y-z-w quaternion
"""
eef_pos_in_world = np.array(p.getLinkState(self.ik_robot, 6)[0])
eef_orn_in_world = np.array(p.getLinkState(self.ik_robot, 6)[1])
eef_pose_in_world = T.pose2mat((eef_pos_in_world, eef_orn_in_world))
base_pos_in_world = np.array(p.getBasePositionAndOrientation(self.ik_robot)[0])
base_orn_in_world = np.array(p.getBasePositionAndOrientation(self.ik_robot)[1])
base_pose_in_world = T.pose2mat((base_pos_in_world, base_orn_in_world))
world_pose_in_base = T.pose_inv(base_pose_in_world)
eef_pose_in_base = T.pose_in_A_to_pose_in_B(
pose_A=eef_pose_in_world, pose_A_in_B=world_pose_in_base
)
return T.mat2pose(eef_pose_in_base)
def bullet_base_pose_to_world_pose(self, pose_in_base):
"""
Convert a pose in the base frame to a pose in the world frame.
Args:
pose_in_base: a (pos, orn) tuple.
Returns:
pose_in world: a (pos, orn) tuple.
"""
pose_in_base = T.pose2mat(pose_in_base)
base_pos_in_world = np.array(p.getBasePositionAndOrientation(self.ik_robot)[0])
base_orn_in_world = np.array(p.getBasePositionAndOrientation(self.ik_robot)[1])
base_pose_in_world = T.pose2mat((base_pos_in_world, base_orn_in_world))
pose_in_world = T.pose_in_A_to_pose_in_B(
pose_A=pose_in_base, pose_A_in_B=base_pose_in_world
)
return T.mat2pose(pose_in_world)