def get_hand_frame_pose(self, robot_state=None, frame_id=None):
'''
Returns the pose of the hand in the current or passed in robot state.
Note that this function uses the TransformState get_transform function rather than FK.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state in which you want to find
the pose of the hand frame. If nothing is passed in, returns the pose of the hand frame in the robot
state in the planning scene interface.
*frame_id (string):* The frame in which you want the pose of the hand frame. If nothing is passed in,
returns in the robot frame.
**Returns:**
A geometry_msgs.msg.PoseStamped that is the position of the hand frame.
'''
if frame_id == None:
frame_id = self._psi.robot_frame
if robot_state == None:
robot_state = self._psi.get_robot_state()
trans = self._transformer.get_transform(self.hand.hand_frame, frame_id,
robot_state)
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = conv.transform_to_pose(trans.transform)
return ps
def transform_quaternion(self, new_frame_id, old_frame_id, quat,
robot_state):
'''
Transforms a quaternion defined in old_frame_id into new_frame_id according to robot state.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**quat (geometry_msgs.msg.Quaternion):** quaternion defined in old_frame_id
**robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation
**Returns:**
A geometry_msgs.msg.Quaternion defined in new_frame_id
**Raises:**
**exceptions.TransformStateError:** if there is an error getting the transform
'''
trans = self.get_transform(old_frame_id, new_frame_id, robot_state)
return gt.transform_quaternion(quat,
conv.transform_to_pose(trans.transform))
def get_hand_frame_pose(self, robot_state=None, frame_id=None):
'''
Returns the pose of the hand in the current or passed in robot state.
Note that this function uses the TransformState get_transform function rather than FK.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* The robot state in which you want to find
the pose of the hand frame. If nothing is passed in, returns the pose of the hand frame in the robot
state in the planning scene interface.
*frame_id (string):* The frame in which you want the pose of the hand frame. If nothing is passed in,
returns in the robot frame.
**Returns:**
A geometry_msgs.msg.PoseStamped that is the position of the hand frame.
'''
if frame_id == None:
frame_id = self._psi.robot_frame
if robot_state == None:
robot_state = self._psi.get_robot_state()
trans = self._transformer.get_transform(self.hand.hand_frame, frame_id, robot_state)
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = conv.transform_to_pose(trans.transform)
return ps
def transform_point(self, new_frame_id, old_frame_id, point, robot_state):
"""
Transforms a point defined in old_frame_id into new_frame_id according to robot state.
**Args:**
**new_frame_id (string):** new frame id
**old_frame_id (string):** old frame id
**point (geometry_msgs.msg.Point):** point defined in old_frame_id
**robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation
**Returns:**
A geometry_msgs.msg.Point defined in new_frame_id
**Raises:**
**exceptions.TransformStateError:** if there is an error getting the transform
"""
trans = self.get_transform(old_frame_id, new_frame_id, robot_state)
return gt.transform_point(point, conv.transform_to_pose(trans.transform))
def add_spatula(self, arm):
spatula = CollisionObject()
spatula.id = "spatula"
spatula.header.frame_id = self.wi.world_frame
spatula.operation.operation = spatula.operation.ADD
paddle = Shape()
handle = Shape()
paddle.type = paddle.BOX
paddle.dimensions = [0.11, 0.12, 0.005]
handle.type = handle.CYLINDER
handle.dimensions = [0.02, 0.24]
paddle_pose = Pose()
handle_pose = Pose()
paddle_pose.position.y = paddle.dimensions[1] / 2.0
paddle_pose.orientation.w = 1.0
angle = np.pi / 5.0
handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
np.pi / 2.0 - angle)
handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
np.pi / 2.0 - angle)
handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)
spatula.shapes = [paddle, handle]
spatula.poses = [paddle_pose, handle_pose]
#this is the grasp transformation
pos_on_handle = handle.dimensions[1] - 0.1
inv_grasp = Transform()
grasp = RigidGrasp()
#really should be calculating this...
inv_grasp.translation.y = GRIPPER_LENGTH
inv_grasp.translation.z = pos_on_handle / 2.0
#flip 90 degrees
inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
origin = Pose()
origin.orientation.w = 1.0
grasp.transform = pose_to_transform(
gt.inverse_transform_pose(origin, g))
grasp.touch_links = [arm[0] + '_end_effector']
grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
grasp.min_approach_distance = 0
grasp.desired_approach_distance = 0.15
grasp.min_distance_from_surface = -1
spat_p = copy.deepcopy(spatula)
wtrans = Pose()
wtrans.orientation.x = np.sin(angle / 2.0)
wtrans.orientation.w = np.cos(angle / 2.0)
if self.world == -1:
wtrans.position.x = 3
wtrans.position.y = -2.8
wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['far_corner']
elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
wtrans.position.x = -1.7
wtrans.position.y = 2
wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['door_table']
else:
wtrans.position.x = 0.6
wtrans.position.y = -0.3
wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
0]
ss = ['center_table']
if self.world == -4 or self.world == -5:
wtrans.position.y = 0
rot = Quaternion()
rot.z = np.sin(np.pi / 2.0)
rot.w = np.cos(np.pi / 2.0)
wtrans.orientation = gt.multiply_quaternions(
rot, wtrans.orientation)
if self.world == -5:
wtrans.position.x = 0
for i in range(len(spat_p.poses)):
spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)
self.wi.add_object(spat_p)
return ObjectType(type="SpatulaObject",
collision_object=spatula,
parameters=ss,
numeric_parameters=paddle.dimensions +
handle.dimensions + [angle]), [grasp]
