def update_basis_orientation(self): """ This function update the orientation of the basis and get the next Gripper in short distance from goal :return: """ x, y, r = self.get_current_base_position() rospy.logout("The basis is at position :") rospy.logout(x) rospy.logout(y) rospy.logout(r) # get pose torso pose_torso = tf_wrapper.lookup_pose(self._origin, self._torso) # get pose goal pose_goal = tf_wrapper.lookup_pose(self._origin, self._goal) # get list of pose of gripper list_pose_gripper = [tf_wrapper.lookup_pose(self._origin, p) for p in self._list_grippers] # get angle to G angle = [float(w.get_angle_casadi(self.get_x_y(pose_torso), self.get_x_y(pose_goal), self.get_x_y(p))) for p in list_pose_gripper] # max angle G to goal max_angle = np.amax(angle) rospy.logout("Max angle is :") rospy.logout(max_angle) # update old and new orientation max_angle = r + max_angle rospy.logout("Max angle + r is :") rospy.logout(max_angle) # get selected Gripper gripper = np.where(np.array(angle) == np.amax(angle)) rospy.logout("Farther away Gripper:") rospy.logout(gripper[0][0]) # convert angle in axis q = w.quaternion_from_axis_angle([0, 0, 1], max_angle)
def attach_cylinder(self, name=u'cylinder', height=1, radius=1, frame_id=None, position=None, orientation=None, expected_response=UpdateWorldResponse.SUCCESS): scm = self.get_robot().get_self_collision_matrix() expected_pose = PoseStamped() expected_pose.header.frame_id = frame_id expected_pose.pose.position = Point(*position) if orientation: expected_pose.pose.orientation = Quaternion(*orientation) else: expected_pose.pose.orientation = Quaternion(0, 0, 0, 1) r = self.wrapper.attach_cylinder(name, height, radius, frame_id, position, orientation) assert r.error_codes == expected_response, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_response)) if expected_response == UpdateWorldResponse.SUCCESS: self.wait_for_synced() assert name in self.get_controllable_links() assert not self.get_world().has_object(name) assert not name in self.wrapper.get_object_names().object_names assert name in self.wrapper.get_attached_objects().object_names assert scm.difference( self.get_robot().get_self_collision_matrix()) == set() assert len(scm) < len(self.get_robot().get_self_collision_matrix()) compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose) self.loop_once()
def __init__(self): """ this class controls and performs mvt of gripper """ self.giskard = GiskardWrapper() rospy.logout("--> Set kitchen/world in Giskard") rospy.logout("- Set pose kitchen") kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world") kitchen_pose.header.frame_id = "map" # setup kitchen rospy.logout("- Get urdf") self.urdf = rospy.get_param("kitchen_description") self.parsed_urdf = URDF.from_xml_string(self.urdf) self.config_file = None rospy.logout("- clear urdf and add kitchen urdf") self.giskard.clear_world() self.giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states") # setup gripper as service rospy.logout("- Set right and left Gripper service proxy") self.l_gripper_service = rospy.ServiceProxy('/l_gripper_simulator/set_joint_states', SetJointState) self.r_gripper_service = rospy.ServiceProxy('/r_gripper_simulator/set_joint_states', SetJointState) # setup gripper as actionlib # self.rgripper = actionlib.SimpleActionClient('r_gripper/gripper_command', GripperCommandAction) # self.lgripper = actionlib.SimpleActionClient('l_gripper/gripper_command', GripperCommandAction) # self.rgripper.wait_for_server() # self.lgripper.wait_for_server() rospy.logout("--> Gripper are ready for every task.")
def set_cart_goal_for_basis(self, x, y, z, q): """ this function send goal to the basis of robot :param x: position on x axis :type: float :param y: position on y axis :type: float :param z: position on z axis :type: float :param q: quaternion orientation :type: array float :return: """ # take base_footprint poseStamp = lookup_pose(self._origin, "base_footprint") poseStamp.header.frame_id = self._origin # set goal pose poseStamp.pose.position.x = x poseStamp.pose.position.y = y poseStamp.pose.position.z = z poseStamp.pose.orientation.x = q[0] poseStamp.pose.orientation.y = q[1] poseStamp.pose.orientation.z = q[2] poseStamp.pose.orientation.w = q[3] self._giskard.set_cart_goal(self._origin, "base_footprint", poseStamp) self._giskard.avoid_all_collisions() self._giskard.plan_and_execute()
def kitchen_setup(zero_pose): """ :type resetted_giskard: GiskardTestWrapper :return: """ object_name = u'kitchen' zero_pose.add_urdf(object_name, rospy.get_param(u'kitchen_description'), tf.lookup_pose(u'map', u'iai_kitchen/world')) return zero_pose
def get_current_base_position(self): """ the method check the current pose of base_footprint to odom and get it :return: x, y, rotation """ basis = tf_wrapper.lookup_pose(self._origin, "base_footprint") t, r = [basis.pose.position.x, basis.pose.position.y, basis.pose.position.z], \ [basis.pose.orientation.x, basis.pose.orientation.y, basis.pose.orientation.z, basis.pose.orientation.w] # self.get_msg_translation_and_rotation(self._basis, "map") return self.base_position(t, r)
def set_cart_goal_for_basis(x, y, z): giskard = GiskardWrapper() # take base_footprint poseStamp = lookup_pose("odom_combined", "base_footprint") # set goal pose poseStamp.pose.position.x = x poseStamp.pose.position.y = y poseStamp.pose.position.z = z giskard.set_cart_goal("odom_combined", "base_footprint", poseStamp) giskard.avoid_all_collisions() giskard.plan_and_execute()
def kitchen_setup(resetted_giskard): """ :type resetted_giskard: GiskardTestWrapper :return: """ resetted_giskard.allow_all_collisions() resetted_giskard.send_and_check_joint_goal(gaya_pose) object_name = u'kitchen' resetted_giskard.add_urdf(object_name, rospy.get_param(u'kitchen_description'), lookup_pose(u'map', u'iai_kitchen/world'), u'/kitchen/joint_states') js = {k: 0.0 for k in resetted_giskard.get_world().get_object(object_name).get_movable_joints()} resetted_giskard.set_kitchen_js(js) return resetted_giskard
def cb(self, traj): """ :type traj: FollowJointTrajectoryActionGoal :return: """ map_T_odom_original = msg_to_kdl(tf.lookup_pose( 'map', self.odom_frame)) traj = traj.goal start_time = traj.trajectory.header.stamp js = JointState() js.name = traj.trajectory.joint_names odom_x_id = js.name.index(self.odom_x) odom_y_id = js.name.index(self.odom_y) odom_z_id = js.name.index(self.odom_z) rospy.sleep(start_time - rospy.get_rostime()) dt = traj.trajectory.points[1].time_from_start.to_sec( ) - traj.trajectory.points[0].time_from_start.to_sec() r = rospy.Rate(1 / dt) for traj_point in traj.trajectory.points: odom_T_map = msg_to_kdl(tf.lookup_pose(self.odom_frame, 'map')) js.header.stamp = start_time + traj_point.time_from_start js.position = list(traj_point.positions) js.velocity = traj_point.velocities odom_x = js.position[odom_x_id] odom_y = js.position[odom_y_id] odom_z = js.position[odom_z_id] odom_original_T_goal = kdl.Frame(kdl.Rotation().RotZ(odom_z), kdl.Vector(odom_x, odom_y, 0)) map_T_goal = map_T_odom_original * odom_original_T_goal odom_T_goal = odom_T_map * map_T_goal js.position[odom_x_id] = odom_T_goal.p[0] js.position[odom_y_id] = odom_T_goal.p[1] js.position[odom_z_id] = kdl.Rotation().RotZ(odom_z).GetRot()[2] self.joint_state_pub.publish(js) r.sleep() js.velocity = [0 for _ in js.velocity] self.joint_state_pub.publish(js)
def execute_cb(self, goal): # uncomment to disable collision avoidance # self._manipulator.set_collision(None) rospy.loginfo("Grasping: {}".format(goal)) # Set initial result value. success = True self._result.error_code = self._result.FAILED collision_whitelist = [] if goal.object_frame_id in self._giskard_wrapper.get_object_names( ).object_names: collision_whitelist.append(goal.object_frame_id) else: rospy.logwarn("unknown object: {}".format(goal.object_frame_id)) # get current robot_pose robot_pose = tfwrapper.lookup_pose('map', 'base_footprint') # open gripper self._gripper.set_gripper_joint_position(1.2) success &= self._manipulator.move_to_goal( root_link=self._root, tip_link=u'hand_gripper_tool_frame', goal_pose=goal.goal_pose, robot_pose=robot_pose, mode=goal.grasp_mode, step=0.1, collision_whitelist=collision_whitelist) if success: self._gripper.close_gripper_force(0.8) success &= self._gripper.object_in_gripper() if success: # Attach object if goal.object_frame_id in self._giskard_wrapper.get_object_names( ).object_names: self._giskard_wrapper.attach_object( goal.object_frame_id, u'hand_palm_link') self._gripper.publish_object_in_gripper( goal.object_frame_id, goal.goal_pose, ObjectInGripper.GRASPED) robot_pose.header.stamp = rospy.Time.now( ) # Might not be needed but is cleaner this way success &= self._manipulator.move_to_goal(root_link=self._root, tip_link=u'base_footprint', goal_pose=robot_pose) success &= self._manipulator.take_robot_pose( rospy.get_param(u'/manipulation/robot_poses/transport')) if success: self._result.error_code = self._result.SUCCESS self._as.set_succeeded(self._result)
def set_cart_goal_test(): giskard = GiskardWrapper() poseStamp = lookup_pose("odom_combined", "iai_kitchen/kitchen_island_left_lower_drawer_handle") h_g = np_to_kdl(np.array([[-1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])) pose = pose_to_kdl(poseStamp.pose) pose = pose * h_g poseStamp.pose = kdl_to_pose(pose) giskard.set_cart_goal("odom_combined", "r_gripper_tool_frame", poseStamp) giskard.allow_all_collisions() giskard.plan_and_execute()
def update(self): markers = [] time_stamp = rospy.Time() objects_dict = self.get_world().get_objects() # If objects were added, update the namespace and objects if self.has_environment_changed(): self.set_full_link_names_for_objects() # Creating of marker for every link in an object for object_name, object in objects_dict.items(): for link_name in object.get_link_names(): if object.has_link_visuals(link_name): # Simple objects (containing only one link) are skipped, since they are already managed # in plugin_pybullet.py and as marker encoded with the function as_marker_msg from urdf_object.py if link_name == object_name and len( object.get_link_names()) == 1: continue # More complex objects will be published here: else: marker = object.link_as_marker(link_name) if marker is None: continue marker.header.frame_id = self.map_frame id_str = self.get_id_str(object_name, link_name) marker.id = int( hashlib.md5(id_str).hexdigest()[:6], 16 ) # FIXME find a better way to give the same link the same id self.ids.add(marker.id) marker.ns = self.marker_namespace marker.header.stamp = time_stamp try: full_link_name = self.links_full_frame_name[ link_name] except KeyError: continue pose = lookup_pose(self.map_frame, full_link_name).pose if object.has_non_identity_visual_offset(link_name): marker.pose = kdl_to_pose( pose_to_kdl(pose) * pose_to_kdl(marker.pose)) else: marker.pose = pose markers.append(marker) self.publisher.publish(markers) if self.ensure_publish: rospy.sleep(0.1) return py_trees.common.Status.SUCCESS
def kitchen_setup(better_pose): # better_pose.allow_all_collisions() # better_pose.send_and_check_joint_goal(gaya_pose) object_name = u'kitchen' better_pose.add_urdf(object_name, rospy.get_param(u'kitchen_description'), tf.lookup_pose(u'map', u'iai_kitchen/world'), u'/kitchen/joint_states', set_js_topic=u'/kitchen/cram_joint_states') js = { k: 0.0 for k in better_pose.get_world().get_object( object_name).get_movable_joints() } better_pose.set_kitchen_js(js) return better_pose
def update(self): try: if self.mjs is None: js = self.lock.get() else: js = self.lock.get_nowait() self.mjs = to_joint_state_dict(js) except Empty: pass robot_frame = self.get_robot().get_root() base_pose = lookup_pose(self.map_frame, robot_frame) self.get_robot().base_pose = base_pose.pose self.god_map.safe_set_data(identifier.joint_states, self.mjs) return Status.SUCCESS
def __init__(self): print("MotionTaskWithConstraint is initialized !") self._giskard = GiskardWrapper() rospy.logout("- Set pose kitchen") kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world") kitchen_pose.header.frame_id = "map" # setup kitchen rospy.logout("- Get urdf") self.urdf = rospy.get_param("kitchen_description") self.parsed_urdf = URDF.from_xml_string(self.urdf) self.config_file = None rospy.logout("- clear urdf and add kitchen urdf") self._giskard.clear_world() self._giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")
def cb(dt): pose = lookup_pose(odom, base_footprint) js = JointState() js.header.stamp = pose.header.stamp js.name = [x, y, z] js.position = [ pose.pose.position.x, pose.pose.position.y, rotation_from_matrix( quaternion_matrix([ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ]))[0] ] js.velocity = [0, 0, 0] js.effort = [0, 0, 0] pose_pub.publish(js)
def check_cart_goal(self, tip_link, goal_pose): goal_in_base = transform_pose(u'map', goal_pose) current_pose = lookup_pose(u'map', tip_link) np.testing.assert_array_almost_equal( msg_to_list(goal_in_base.pose.position), msg_to_list(current_pose.pose.position), decimal=2) try: np.testing.assert_array_almost_equal( msg_to_list(goal_in_base.pose.orientation), msg_to_list(current_pose.pose.orientation), decimal=2) except AssertionError: np.testing.assert_array_almost_equal( msg_to_list(goal_in_base.pose.orientation), -np.array(msg_to_list(current_pose.pose.orientation)), decimal=2)
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)
def execute_cb(self, goal): # uncomment to disable collision avoidance # self._manipulator.set_collision(None) rospy.loginfo("Placing: {}".format(goal)) # Set initial result value. success = True self._result.error_code = self._result.FAILED if goal.object_frame_id not in self._giskard_wrapper.get_attached_objects().object_names: rospy.logwarn("object not attached to gripper: {}".format(goal.object_frame_id)) # get current robot_pose robot_pose = tfwrapper.lookup_pose('map', 'base_footprint') success &= self._manipulator.move_to_goal(root_link=self._root, tip_link=u'hand_gripper_tool_frame', goal_pose=goal.goal_pose, robot_pose=robot_pose, mode=goal.place_mode, step=0.1) if success: self._gripper.set_gripper_joint_position(1.2) if goal.object_frame_id in self._giskard_wrapper.get_attached_objects().object_names: self._giskard_wrapper.detach_object(goal.object_frame_id) self._gripper.publish_object_in_gripper(goal.object_frame_id, goal.goal_pose, ObjectInGripper.PLACED) collision_whitelist = [] if goal.object_frame_id in self._giskard_wrapper.get_object_names().object_names: collision_whitelist.append(goal.object_frame_id) else: rospy.logwarn("unknown object: {}".format(goal.object_frame_id)) robot_pose.header.stamp = rospy.Time.now() # Might not be needed but is cleaner this way success &= self._manipulator.move_to_goal(root_link=self._root, tip_link=u'base_footprint', goal_pose=robot_pose, collision_whitelist=collision_whitelist) success &= self._manipulator.take_robot_pose(rospy.get_param(u'/manipulation/robot_poses/neutral')) if success: self._result.error_code = self._result.SUCCESS self._as.set_succeeded(self._result)
#! /usr/bin/env python import rospy from giskardpy.python_interface import GiskardWrapper from giskardpy import tfwrapper rospy.init_node('add_static_giskard') giskard_wrapper = GiskardWrapper() _urdf = rospy.get_param('hsrb_lab', False) kitchen_frame = rospy.get_param('~environment_frame', 'iai_kitchen/world') while not _urdf: rospy.sleep(5) _urdf = rospy.get_param('hsrb_lab', False) giskard_wrapper.detach_object('gripper_dummy') giskard_wrapper.remove_object('gripper_dummy') giskard_wrapper.remove_object('lab') p = tfwrapper.lookup_pose('map', kitchen_frame) #giskard_wrapper.attach_box(name='gripper_dummy', size=[0.05, 0.1, 0.1], frame_id="hand_palm_link", # position=[0.05, 0, 0.05], orientation=[0, 0, 0, 1]) giskard_wrapper.add_urdf(name='lab', urdf=_urdf, js_topic='/kitchen/joint_states', pose=p)
def test_avoid_self_collision2(self, self_collision_pose): self_collision_pose.send_and_check_goal() map_T_root = lookup_pose(u'map', u'base_footprint') expected_pose = Pose() expected_pose.orientation.w = 1 compare_poses(map_T_root.pose, expected_pose)
from giskardpy.tfwrapper import lookup_transform, lookup_pose from giskardpy import logging if __name__ == '__main__': rospy.init_node('add_urdf') giskard = GiskardWrapper() try: name = rospy.get_param('~name') path = rospy.get_param('~path', None) param_name = rospy.get_param('~param', None) position = rospy.get_param('~position', None) orientation = rospy.get_param('~rpy', None) root_frame = rospy.get_param('~root_frame', None) map_frame = rospy.get_param('~frame_id', 'map') if root_frame is not None: pose = lookup_pose(map_frame, root_frame) else: pose = PoseStamped() pose.header.frame_id = map_frame if position is not None: pose.pose.position = Point(*position) if orientation is not None: pose.pose.orientation = Quaternion(*quaternion_from_euler( *orientation)) else: pose.pose.orientation.w = 1 if path is None: if param_name is None: logging.logwarn('neither _param nor _path specified') sys.exit() else: