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 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_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 set_object_pose(self, name, pose_matrix):
if name == self.object_name:
pos, quat = T.mat2pose(pose_matrix)
self.object_curr_pos_base = pos
self.object_curr_quat_base = quat
if self.debug:
print("BaxterObject6DPoseController:", self.object_name,
"pos in base: ", self.object_curr_pos_base)
print("BaxterObject6DPoseController:", self.object_name,
"rot in base: ", self.object_curr_quat_base)
else:
print(
"BaxterObject6DPoseController: object name %s not recognized; not setting pose"
% name)
return
def set_body_poses(self, body_pose_dict):
# initialize dictionary of body names to poses in world frame
self.body_pose_dict_world = {}
# convert poses to world frame
for body in body_pose_dict.keys():
# get pos and quat in base frame from matrix
body_pos, body_quat = T.mat2pose(body_pose_dict[body])
# convert to world frame
body_pos_world, body_quat_world = self.bullet_base_pose_to_world_pose(
(body_pos, body_quat))
# set pose in world frame in dictionary
self.body_pose_dict_world[body] = (body_pos_world, body_quat_world)
return
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)
