def joint_space_client():
angle = 0.0
giskard = GiskardWrapper()
rospy.sleep(0.5)
# Creates the SimpleActionClient, passing the type of the action
# (MoveAction) to the constructor.
# Creates a goal to send to the action server.
# Waits until the action server has started up and started
# listening for goals.
angle += 0.8
rospy.loginfo("%f", angle)
gaya_pose = {'r_shoulder_pan_joint': -1.7125,
'r_shoulder_lift_joint': -0.25672,
'r_upper_arm_roll_joint': -1.46335,
'r_elbow_flex_joint': -2.12216,
'r_forearm_roll_joint': 1.76632,
'r_wrist_flex_joint': -0.10001,
'r_wrist_roll_joint': 0.05106,
'l_shoulder_pan_joint': 1.9652,
'l_shoulder_lift_joint': - 0.26499,
'l_upper_arm_roll_joint': 1.3837,
'l_elbow_flex_joint': - 2.1224,
'l_forearm_roll_joint': 16.99,
'l_wrist_flex_joint': - 0.10001,
'l_wrist_roll_joint': 0}
giskard.allow_all_collisions()
giskard.set_joint_goal(gaya_pose)
giskard.plan_and_execute()
return giskard.get_result()
class MoveGripperServer:
_feedback = MoveGripperFeedback()
_result = MoveGripperResult()
_root = u'odom'
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
MoveGripperAction,
execute_cb=self.execute_cb,
auto_start=False)
self._giskard_wrapper = GiskardWrapper()
self._as.start()
def execute_cb(self, goal):
rospy.loginfo("Move gripper: {}".format(goal))
self._result.error_code = self._result.FAILED
self._giskard_wrapper.interrupt()
self._giskard_wrapper.set_cart_goal(self._root, u'hand_palm_link',
goal.goal_pose)
self._giskard_wrapper.plan_and_execute(wait=False)
result = self._giskard_wrapper.get_result(rospy.Duration(60))
if result and result.SUCCESS in result.error_code:
self._result.error_code = self._result.SUCCESS
self._as.set_succeeded(self._result)
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 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)
class TakePoseServer:
_feedback = TakePoseFeedback()
_result = TakePoseResult()
_goal = TakePoseAction()
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
TakePoseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
self._giskard_wrapper = GiskardWrapper()
self._robot = hsrb_interface.Robot()
self._whole_body = self._robot.get('whole_body')
rospy.loginfo("{} is ready and waiting for orders.".format(
self._action_name))
def execute_cb(self, goal):
rospy.loginfo("Take pose: {}".format(goal))
self._result.error_code = self._result.FAILED
self._giskard_wrapper.allow_self_collision()
if goal.pose_mode == goal.FREE:
self._giskard_wrapper.set_joint_goal({
u'head_pan_joint':
goal.head_pan_joint,
u'head_tilt_joint':
goal.head_tilt_joint,
u'arm_lift_joint':
goal.arm_lift_joint,
u'arm_flex_joint':
goal.arm_flex_joint,
u'arm_roll_joint':
goal.arm_roll_joint,
u'wrist_flex_joint':
goal.wrist_flex_joint,
u'wrist_roll_joint':
goal.wrist_roll_joint
})
self._giskard_wrapper.plan_and_execute(wait=True)
elif goal.pose_mode == goal.NEUTRAL:
self._giskard_wrapper.set_joint_goal(
rospy.get_param(u'/manipulation/robot_poses/neutral'))
self._giskard_wrapper.plan_and_execute(wait=True)
elif goal.pose_mode == goal.LOOK_HIGH:
self._giskard_wrapper.set_joint_goal(
rospy.get_param(u'/manipulation/robot_poses/look_high'))
self._giskard_wrapper.plan_and_execute(wait=True)
elif goal.pose_mode == goal.LOOK_LOW:
self._giskard_wrapper.set_joint_goal(
rospy.get_param(u'/manipulation/robot_poses/look_low'))
self._giskard_wrapper.plan_and_execute(wait=True)
elif goal.pose_mode == goal.LOOK_FLOOR:
self._giskard_wrapper.set_joint_goal(
rospy.get_param(u'/manipulation/robot_poses/look_floor'))
self._giskard_wrapper.plan_and_execute(wait=True)
elif goal.pose_mode == goal.GAZE:
camera_height = goal.gaze_point.z - 0.4
if camera_height < 0.0:
camera_height = 0.0
elif camera_height > 0.69:
camera_height = 0.69
self._giskard_wrapper.set_joint_goal({
u'head_pan_joint': -1.54,
u'head_tilt_joint': 0.0,
u'arm_lift_joint': camera_height, # must be between 0 and 0.69
u'arm_flex_joint': -0.5,
u'arm_roll_joint': -1.8,
u'wrist_flex_joint': -1.57,
u'wrist_roll_joint': 0.0
})
self._giskard_wrapper.plan_and_execute(wait=True)
v3 = hsrb_interface.geometry.Vector3(x=goal.gaze_point.x,
y=goal.gaze_point.y,
z=goal.gaze_point.z)
self._whole_body.gaze_point(point=v3, ref_frame_id='map')
elif goal.pose_mode == goal.GIVE_TAKE:
self._giskard_wrapper.set_joint_goal(
rospy.get_param(u'/manipulation/robot_poses/give_take'))
self._giskard_wrapper.plan_and_execute(wait=True)
result = self._giskard_wrapper.get_result(rospy.Duration(60))
if result and result.SUCCESS in result.error_codes:
self._result.error_code = self._result.SUCCESS
self._giskard_wrapper.avoid_self_collision()
self._as.set_succeeded(self._result)