class SrOpenSuitcase(object):
"""
"""
# The xyz distance we want the palm link to be from the grasping point
DISTANCE_TO_GRASP = [-0.1, -0.01, 0.04]
# The distance used when computing the approach
APPROACH_DISTANCE = 0.03
def __init__(self,):
"""
"""
self.markers_pub_ = rospy.Publisher("~markers", Marker)
self.execution = Execution(use_database=False)
# direct control of the elbow swing, WRJ1 and THJ5 for throwing the lid open
self.publishers = {}
self.publishers["ElbowJSwing"] = rospy.Publisher("/sa_es_position_controller/command", Float64, latch=True)
self.publishers["WRJ1"] = rospy.Publisher(
"/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
)
self.publishers["THJ5"] = rospy.Publisher(
"/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
)
self.suitcase_src_ = rospy.Service("~open_suitcase", OpenSuitcase, self.open_lid)
def open_lid(self, suitcase_req):
suitcase = suitcase_req.suitcase
self.display_suitcase_(suitcase)
semi_circle = self.go_to_mechanism_and_grasp_(suitcase)
if semi_circle == None:
rospy.logerr("Failed to approach.")
sys.exit(1)
time.sleep(1.0)
self.lift_lid_(suitcase, semi_circle)
# throw the lid open
self.throw_lid_open()
def go_to_mechanism_and_grasp_(self, suitcase):
# compute the full trajectory
semi_circle = self.compute_semi_circle_traj_(suitcase)
# compute the pregrasp and grasp
grasp = Grasp()
grasp_pose_ = PoseStamped()
# the grasp is the first item of the semi circle
grasp_pose_ = semi_circle[0]
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# TODO: use the suitcase axis to approach from the perpendicular
pre_grasp_pose_.pose.position.x = pre_grasp_pose_.pose.position.x - self.APPROACH_DISTANCE
# TODO: find better postures
grasp.pre_grasp_posture.name = [
"FFJ0",
"FFJ3",
"FFJ4",
"LFJ0",
"LFJ3",
"LFJ4",
"LFJ5",
"MFJ0",
"MFJ3",
"MFJ4",
"RFJ0",
"RFJ3",
"RFJ4",
"THJ1",
"THJ2",
"THJ3",
"THJ4",
"THJ5",
"WRJ1",
"WRJ2",
]
grasp.pre_grasp_posture.position = [0.0] * 18
grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ4")] = 58.0
grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ5")] = -50.0
grasp.grasp_posture.name = [
"FFJ0",
"FFJ3",
"FFJ4",
"LFJ0",
"LFJ3",
"LFJ4",
"LFJ5",
"MFJ0",
"MFJ3",
"MFJ4",
"RFJ0",
"RFJ3",
"RFJ4",
"THJ1",
"THJ2",
"THJ3",
"THJ4",
"THJ5",
"WRJ1",
"WRJ2",
]
grasp.grasp_posture.position = [0.0] * 18
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ1")] = 57.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ2")] = 30.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ3")] = -15.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ4")] = 58.0
grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ5")] = 40.0
grasp.grasp_pose = grasp_pose_.pose
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
motion_plan_res = GetMotionPlanResponse()
interpolated_motion_plan_res = self.execution.plan.get_interpolated_ik_motion_plan(
pre_grasp_pose_, grasp_pose_, False
)
# check the result (depending on number of steps etc...)
if interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS:
number_of_interpolated_steps = 0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val != 1:
rospy.logerr(
"One unfeasible approach-phase step found at "
+ str(interpolation_index)
+ "with val "
+ str(traj_error_code.val)
)
break
else:
number_of_interpolated_steps = interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps + 1 == len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number approach is possible, checking motion plan to pre-grasp")
# print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.execution.plan.plan_arm_motion("right_arm", "jointspace", pre_grasp_pose_)
# execution part
if motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS:
# put hand in pre-grasp posture
if self.execution.pre_grasp_exec(grasp) < 0:
rospy.logerr("Failed to go in pregrasp.")
sys.exit()
# go there
# filter the trajectory
filtered_traj = self.execution.filter_traj_(motion_plan_res)
self.execution.display_traj_(filtered_traj)
# reach pregrasp pose
if self.execution.send_traj_(filtered_traj) < 0:
time.sleep(20) # TODO use actionlib here
# approach
if self.execution.send_traj_(interpolated_motion_plan_res.trajectory.joint_trajectory) < 0:
rospy.logerr("Failed to approach.")
sys.exit()
time.sleep(20) # TODO use actionlib here
# grasp
if self.execution.grasp_exec(grasp) < 0:
rospy.logerr("Failed to grasp.")
sys.exit()
time.sleep(20) # TODO use actionlib here
else:
# Failed, don't return the computed traj
return None
# return the full traj
return semi_circle
def lift_lid_(self, suitcase, semi_circle):
while len(semi_circle) > 0:
self.execution.plan_and_execute_step_(semi_circle)
time.sleep(0.5)
return OpenSuitcaseResponse(OpenSuitcaseResponse.SUCCESS)
def throw_lid_open(self):
# get the current joint states
res = self.execution.plan.get_joint_state_.call()
current_joint_states = res.joint_state
# flex the elbow, flip the wrist
elbow_target = current_joint_states.position[current_joint_states.name.index("ElbowJSwing")] + 0.5
wrist_target = current_joint_states.position[current_joint_states.name.index("WRJ1")] - 0.5
thj5_target = -0.80
self.publishers["ElbowJSwing"].publish(elbow_target)
self.publishers["WRJ1"].publish(wrist_target)
self.publishers["THJ5"].publish(thj5_target)
def compute_semi_circle_traj_(self, suitcase, nb_steps=50):
poses = []
# compute a semi-circular trajectory, starting
# from the suitcase mechanism, rotating
# around the suitcase axis
target = PoseStamped()
target.header.frame_id = suitcase.header.frame_id
target.pose = suitcase.opening_mechanism.pose_stamped.pose
# axis_x and axis_z are the projection of the mechanism model
# onto the suitcase axis (point around which we want to rotate)
mechanism = [
suitcase.opening_mechanism.pose_stamped.pose.position.x,
suitcase.opening_mechanism.pose_stamped.pose.position.y,
suitcase.opening_mechanism.pose_stamped.pose.position.z,
]
axis = [
suitcase.lid_axis_a.x + (suitcase.lid_axis_b.x - suitcase.lid_axis_a.x) / 2.0,
suitcase.lid_axis_a.y + (suitcase.lid_axis_b.y - suitcase.lid_axis_a.y) / 2.0,
suitcase.lid_axis_a.z + (suitcase.lid_axis_b.z - suitcase.lid_axis_a.z) / 2.0,
]
# We're always starting with the palm straight along the x axis
# TODO: use real suitcase axis instead?
suitcase_axis = (1, 0, 0)
for i in range(0, nb_steps + 1):
# we're rotating from this angle around the suitcase axis to point towards the suitcase
rotation_angle = float(i) * math.pi / 2.0 / float(nb_steps)
####
# POSITION
target.pose.position.x = axis[0] - (
(axis[0] - mechanism[0] - self.DISTANCE_TO_GRASP[0]) * math.cos(rotation_angle)
)
target.pose.position.y = suitcase.opening_mechanism.pose_stamped.pose.position.y + self.DISTANCE_TO_GRASP[1]
target.pose.position.z = (
axis[2]
+ ((axis[0] - mechanism[0] - self.DISTANCE_TO_GRASP[0]) * math.sin(rotation_angle))
+ self.DISTANCE_TO_GRASP[2]
)
####
# ORIENTATION
# limit the wrj1 axis
rotation_angle = min(rotation_angle, math.radians(15.0))
# add 90 degrees to point the axis toward the suitcase
rotation_angle += math.pi / 2.0
# orientation, palm z axis pointing towards the suitcase axes
# z toward suitcase
toward_z = transformations.quaternion_about_axis(math.pi / 2.0, (0, 0, 1))
# then rotate as we go up to continue pointing at the axis
step_rotation = transformations.quaternion_about_axis(rotation_angle, suitcase_axis)
# combine those transform:
orientation = transformations.quaternion_multiply(toward_z, step_rotation)
target.pose.orientation.x = orientation[0]
target.pose.orientation.y = orientation[1]
target.pose.orientation.z = orientation[2]
target.pose.orientation.w = orientation[3]
poses.append(copy.deepcopy(target))
return poses
def display_suitcase_(self, suitcase):
# display axes
axes_marker = Marker()
axes_marker.header.frame_id = suitcase.header.frame_id
axes_marker.ns = "suitcase"
axes_marker.id = 0
axes_marker.type = Marker.LINE_STRIP
axes_marker.action = Marker.ADD
axes_marker.points.append(suitcase.lid_axis_a)
axes_marker.points.append(suitcase.lid_axis_b)
axes_marker.scale.x = 0.02
axes_marker.color.a = 0.4
axes_marker.color.r = 0.1
axes_marker.color.g = 0.47
axes_marker.color.b = 0.88
self.markers_pub_.publish(axes_marker)
# display mechanism
mechanism_marker = Marker()
mechanism_marker.header.frame_id = suitcase.header.frame_id
mechanism_marker.ns = "suitcase"
mechanism_marker.id = 1
mechanism_marker.type = Marker.CUBE
mechanism_marker.action = Marker.ADD
mechanism_marker.pose = suitcase.opening_mechanism.pose_stamped.pose
mechanism_marker.scale = suitcase.opening_mechanism.dimensions
mechanism_marker.color.a = 0.4
mechanism_marker.color.r = 0.1
mechanism_marker.color.g = 0.47
mechanism_marker.color.b = 0.88
self.markers_pub_.publish(mechanism_marker)
