class MakePlanServer:
def __init__(self, name):
self._action_name = name
self.dummy_object_ = u'dummy_plan_object'
self.gripper_frame_ = u'hand_palm_link'
self.root_frame_ = u'odom'
self._as = actionlib.SimpleActionServer(self._action_name, MakePlanAction, execute_cb=self.execute_cb,
auto_start=False)
self._giskard_wrapper = GiskardWrapper()
self.mode_rotation_ = self.get_mode_rotation()
self._as.start()
rospy.loginfo("{} is ready and waiting for orders.".format(self._action_name))
def get_mode_rotation(self):
mode_rotation = {}
front_rotation = rospy.get_param(u'/manipulation/base_gripper_rotation/front', default=None)
top_rotation = rospy.get_param(u'/manipulation/base_gripper_rotation/top', default=None)
if front_rotation:
mode_rotation[MakePlanGoal.FRONT] = front_rotation
if top_rotation:
mode_rotation[MakePlanGoal.TOP] = top_rotation
return mode_rotation
def execute_cb(self, goal):
rospy.loginfo("Making plan: {}".format(goal))
self._result = MakePlanResult()
self._result.error_code = self._result.FAILED
# TODO move robot in start pose currently replaced with actual robot pose
robot_pose = tfwrapper.lookup_pose('map', 'base_footprint')
# Prepare object
if goal.action_mode == goal.PLACE:
if goal.object_frame_id not in self._giskard_wrapper.get_attached_objects().object_names:
rospy.loginfo("object not attached to gripper: {}. Creating dummy object".format(goal.object_frame_id))
if self.dummy_object_ in self._giskard_wrapper.get_object_names().object_names:
self._giskard_wrapper.remove_object(self.dummy_object_)
self._giskard_wrapper.add_box(self.dummy_object_,
[goal.object_size.x, goal.object_size.y, goal.object_size.z],
self.gripper_frame_)
self._giskard_wrapper.attach_object(self.dummy_object_, self.gripper_frame_)
elif goal.action_mode == goal.GRASP:
if goal.object_frame_id in self._giskard_wrapper.get_object_names().object_names:
self._giskard_wrapper.allow_collision(body_b=goal.object_frame_id)
else:
rospy.logerr("Unknown action_mode: {}".format(goal.action_mode))
self._as.set_succeeded(self._result)
return
if goal.gripper_mode in [goal.FRONT, goal.TOP]:
goal.goal_pose.pose.orientation = Quaternion(
*quaternion_multiply(to_tf_quaternion(robot_pose.pose.orientation),
self.mode_rotation_[goal.gripper_mode]))
self._giskard_wrapper.set_cart_goal(self.root_frame_, self.gripper_frame_, goal.goal_pose)
self._giskard_wrapper.plan()
result = self._giskard_wrapper.get_result()
# Clean up dummy object
if self.dummy_object_ in self._giskard_wrapper.get_attached_objects().object_names:
self._giskard_wrapper.detach_object(self.dummy_object_)
self._giskard_wrapper.remove_object(self.dummy_object_)
# Clean up collission excemption for goal_object
self._giskard_wrapper.avoid_all_collisions()
if result.SUCCESS in result.error_codes:
self._result.error_code = self._result.SUCCESS
self._as.set_succeeded(self._result)
class MoveArm(object):
def __init__(self, enabled=True, knowrob=None, avoid_self_collisinon=True, tip='camera_link',
root='ur5_shoulder_link'):
self.move_time_limit = 25
self.enabled = enabled
self.knowrob = knowrob
self.giskard = GiskardWrapper()
self.tip = tip
self.root = root
self.trans_p_gain = 0.5
self.rot_p_gain = 0.5
self.trans_max_speed = 0.1
self.rot_max_speed = 0.3
self.self_collision_min_dist = 0.03
self.avoid_self_collision = avoid_self_collisinon
# TODO get this from param server of topic
self.joint_names = ['ur5_shoulder_pan_joint',
'ur5_shoulder_lift_joint',
'ur5_elbow_joint',
'ur5_wrist_1_joint',
'ur5_wrist_2_joint',
'ur5_wrist_3_joint', ]
def set_translation_goal(self, translation):
goal_pose = PoseStamped()
if isinstance(translation, PointStamped):
goal_pose.header = translation.header
goal_pose.pose.position = translation.point
else:
goal_pose = translation
self.giskard.set_translation_goal(self.root, self.tip, goal_pose, self.trans_p_gain, self.trans_max_speed)
def set_orientation_goal(self, orientation):
goal_pose = PoseStamped()
if isinstance(orientation, QuaternionStamped):
goal_pose.header = orientation.header
goal_pose.pose.orientation = orientation.quaternion
else:
goal_pose = orientation
self.giskard.set_rotation_goal(self.root, self.tip, goal_pose, self.rot_p_gain, self.rot_max_speed)
def set_default_self_collision_avoidance(self):
if not self.avoid_self_collision:
self.giskard.allow_self_collision()
else:
self.giskard.set_self_collision_distance(self.self_collision_min_dist)
self.giskard.allow_collision(['ur5_wrist_3_link'], self.giskard.get_robot_name(), ['ur5_forearm_link'])
def set_and_send_cartesian_goal(self, goal_pose):
self.set_translation_goal(goal_pose)
self.set_orientation_goal(goal_pose)
self.set_default_self_collision_avoidance()
return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS
def send_cartesian_goal(self):
if self.enabled:
self.set_default_self_collision_avoidance()
return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS
def set_and_send_joint_goal(self, joint_state):
if self.enabled:
self.giskard.set_joint_goal(joint_state)
self.set_default_self_collision_avoidance()
return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS
def move_absolute(self, target_pose, retry=True):
if self.enabled:
self.giskard.set_cart_goal('odom', 'base_footprint', target_pose)
return self.giskard.plan_and_execute()
# self.goal_pub.publish(target_pose)
# goal = MoveBaseGoal()
# goal.target_pose = target_pose
# if self.knowrob is not None:
# self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose))
# while True:
# self.client.send_goal(goal)
# wait_result = self.client.wait_for_result(rospy.Duration(self.timeout))
# result = self.client.get_result()
# state = self.client.get_state()
# if wait_result and state == GoalStatus.SUCCEEDED:
# break
# if retry:
# cmd = raw_input('base movement did not finish in time, retry? [y/n]')
# retry = cmd == 'y'
# if not retry:
# print('movement did not finish in time')
# if self.knowrob is not None:
# self.knowrob.finish_action()
# raise TimeoutError()
# if self.knowrob is not None:
# self.knowrob.finish_action()
# return result
def move_other_frame(self, target_pose, frame='camera_link', retry=True):
if self.enabled:
target_pose = self.cam_pose_to_base_pose(target_pose, frame)
target_pose = transform_pose('map', target_pose)
target_pose.pose.position.z = 0
self.giskard.set_cart_goal('odom', 'base_footprint', target_pose)
return self.giskard.plan_and_execute()
# self.goal_pub.publish(target_pose)
# goal = MoveBaseGoal()
# goal.target_pose = target_pose
# if self.knowrob is not None:
# self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose))
# while True:
# self.client.send_goal(goal)
# wait_result = self.client.wait_for_result(rospy.Duration(self.timeout))
# result = self.client.get_result()
# state = self.client.get_state()
# if wait_result and state == GoalStatus.SUCCEEDED:
# break
# if retry:
# cmd = raw_input('base movement did not finish in time, retry? [y/n]')
# retry = cmd == 'y'
# if not retry:
# print('movement did not finish in time')
# if self.knowrob is not None:
# self.knowrob.finish_action()
# raise TimeoutError()
# if self.knowrob is not None:
# self.knowrob.finish_action()
# return result
def cam_pose_to_base_pose(self, pose, frame):
"""
:type pose: PoseStamped
:rtype: PoseStamped
"""
T_shelf___cam_joint_g = msg_to_kdl(pose)
T_map___bfg = T_shelf___cam_joint_g * self.get_frame_in_base_footprint_kdl(frame).Inverse()
base_pose = kdl_to_posestamped(T_map___bfg, pose.header.frame_id)
return base_pose
def get_frame_in_base_footprint_kdl(self, frame):
"""
:rtype: PyKDL.Frame
"""
# if self.T_bf___cam_joint is None:
T_bf___cam_joint = msg_to_kdl(lookup_transform('base_footprint', frame))
T_bf___cam_joint.p[2] = 0
T_bf___cam_joint.M = PyKDL.Rotation()
return T_bf___cam_joint
def place_pose_left(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
-1.57,
-1.39,
-2.4,
0.46,
1.57,
-1.57,
]
return self.set_and_send_joint_goal(joint_state)
def place_pose_right(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
-np.pi / 2,
-2.44177755311,
2.15026930371,
0.291547812391,
np.pi / 2,
np.pi / 2,
]
return self.set_and_send_joint_goal(joint_state)
def floor_detection_pose_right(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
np.pi,
-1.37,
0.51,
-0.72,
-0.22,
0,
]
return self.set_and_send_joint_goal(joint_state)
def floor_detection_pose_left(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
0.0,
-1.768,
-0.51,
-2.396,
0.243438,
-np.pi,
]
return self.set_and_send_joint_goal(joint_state)
def drive_pose(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
np.pi * 1.5,
-np.pi / 2,
-2.3,
-np.pi / 2,
0,
-np.pi / 2,
]
return self.set_and_send_joint_goal(joint_state)
def pre_baseboard_pose(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
0.0,
-1.6460,
-2.171,
-0.85549,
0.2181,
-3.19172,
]
return self.set_and_send_joint_goal(joint_state)
def see_pose(self):
joint_state = JointState()
joint_state.name = self.joint_names
joint_state.position = [
# -1.57114202181,
# -1.44927913347,
# -1.25000602404,
# -4.01274043718,
# -1.56251222292,
# 1.62433183193,
-np.pi/2,
-2.18,
1.47,
1.03,
np.pi/2,
np.pi/2,
]
return self.set_and_send_joint_goal(joint_state)
class Manipulator:
def __init__(self, collision_distance=0.05, mode_rotation=None):
"""
Proxy class working with giskard for manipulating object with a gripper
:param collision_distance: distance to keep from objects None or < 0 allow all collisions
:type collision_distance:float
:param mode_rotation: Rotation of tip depending on robot_pose
:type mode_rotation: dict(mode: [x, y, z, w])
"""
self.mode_rotation_ = mode_rotation
self.collision_distance_ = collision_distance
self.giskard_wrapper_ = GiskardWrapper()
def set_collision(self, distance, collision_whitelist=None):
"""
Sets the collision avoidance for the Manipulator and next giskard_goal.
To disable collision avoidance set to None or <= 0.
:param distance: min distance to objects
:type distance: float
:param collision_whitelist: list of body names allowed for collision
:type collision_whitelist: list[string]
:return:
"""
self.collision_distance_ = distance
if 0 >= self.collision_distance_:
self.giskard_wrapper_.allow_all_collisions()
else:
self.giskard_wrapper_.avoid_all_collisions(
distance=self.collision_distance_)
if collision_whitelist:
for body in collision_whitelist:
self.giskard_wrapper_.allow_collision(body_b=body)
def move_to_goal(self,
root_link,
tip_link,
goal_pose,
robot_pose=None,
mode=None,
step=None,
collision_whitelist=None):
"""
Moves the tip to the given goal. Look at parameters for additional information
:param root_link: name of the root link of the kinematic chain
:type root_link: string
:param tip_link: name of the tip link of the kinematic chain
:type tip_link: string
:param robot_pose: current pose of the robot (optional required for step and mode to work)
:type robot_pose: PoseStamped
:param goal_pose: goal pose for the tip
:type goal_pose: PoseStamped
:param mode: mode for rotation of tip (optional if None orientation of goal pose is used)
:type
:param step: distance for step from goal (m) (optional)
:type step: float
:param collision_whitelist: list of body names allowed for collision (optional if None avoid all collisions)
:type collision_whitelist: list[string]
:return: True on success
"""
self.giskard_wrapper_.interrupt()
if robot_pose and mode and self.mode_rotation_ and mode in self.mode_rotation_:
goal_pose.pose.orientation = Quaternion(*quaternion_multiply(
to_tf_quaternion(robot_pose.pose.orientation),
self.mode_rotation_[mode]))
if robot_pose and step:
step_pose = PoseStamped()
step_pose.header.frame_id = goal_pose.header.frame_id
step_pose.header.stamp = rospy.Time.now()
step_pose.pose.position = calculate_waypoint2D(
goal_pose.pose.position, robot_pose.pose.position, step)
step_pose.pose.orientation = goal_pose.pose.orientation
rospy.loginfo("step_pose: {}".format(step_pose))
# Move to the defined step
self.set_collision(self.collision_distance_, collision_whitelist)
self.giskard_wrapper_.set_cart_goal(root_link, tip_link, step_pose)
self.giskard_wrapper_.plan_and_execute(wait=True)
rospy.loginfo("goal_pose: {}".format(goal_pose))
goal_pose.header.stamp = rospy.Time.now()
self.set_collision(self.collision_distance_, collision_whitelist)
self.giskard_wrapper_.set_cart_goal(root_link, tip_link, goal_pose)
self.giskard_wrapper_.plan_and_execute(wait=True)
result = self.giskard_wrapper_.get_result()
rospy.loginfo("Giskard result: {}".format(result.error_codes))
return result and result.SUCCESS in result.error_codes
def take_robot_pose(self, robot_pose):
"""
Lets the robot take the given pose
:param robot_pose: Dictionary (joint_name: position)
:type robot_pose: dict
:return: True if success
"""
self.giskard_wrapper_.interrupt()
self.set_collision(self.collision_distance_)
self.giskard_wrapper_.set_joint_goal(robot_pose)
self.giskard_wrapper_.plan_and_execute(wait=True)
result = self.giskard_wrapper_.get_result()
return result and result.SUCCESS in result.error_codes
def grasp_bar(self, root_link, tip_link, goal_pose):
"""
Lets the robot grasp the bar of an object (in this case the handles)
:type root_link str
:param root_link The base/root link of the robot
:type tip_link str
:param tip_link The tip link of the robot(gripper)
:type goal_pose PoseStamped
:param goal_pose The goal of the grasp bar action
"""
self.set_collision(-1)
#self.set_collision(self.collision_distance_)
self.giskard_wrapper_.set_cart_goal(root_link, tip_link, goal_pose)
self.giskard_wrapper_.plan_and_execute(wait=True)
result = self.giskard_wrapper_.get_result()
return result and result.SUCCESS in result.error_codes
def open(self, tip_link, object_link_name, angle_goal, use_limitation):
"""
Lets the robot open the given object
:type tip_link str
:param tip_link the name of the gripper
:type object_link_name str
:param object_link_name handle to grasp
:type angle_goal float
:param angle_goal the angle goal in relation to the current status
:type use_limitation bool
:param use_limitation indicator, if the limitation should be used
"""
self.change_base_scan_limitation(use_limitation)
self.set_collision(-1)
updates = {
u'rosparam': {
u'general_options': {
u'joint_weights': {
u'arm_lift_joint': 1000,
u'arm_roll_joint': 1000
}
}
}
}
self.giskard_wrapper_.update_god_map(updates)
self.giskard_wrapper_.set_open_goal(tip_link,
object_link_name.split('/')[1],
angle_goal)
self.giskard_wrapper_.plan_and_execute(wait=True)
result = self.giskard_wrapper_.get_result()
if use_limitation:
self.change_base_scan_limitation(False)
return result and result.SUCCESS in result.error_codes
def change_base_scan_limitation(self, indicator):
rospy.wait_for_service('/base_scan_limitation')
try:
base_scan_limitation = rospy.ServiceProxy('/base_scan_limitation',
SetBool)
response = base_scan_limitation(indicator)
return response.success
except rospy.ServiceProxy as e:
print("base scan limitation failed: %e" % e)