def add_yaml(self, yaml): """ Adds some fixed collision objects that are defined in the yaml description :param yaml: yaml description of the task :type: Task """ self.add_ground() if yaml is None: self.add_cam_mast(0.92, 0.92) else: x = yaml.mast_of_cam.base_pose.linear.x y = yaml.mast_of_cam.base_pose.linear.y self.add_cam_mast(x, y) if len(yaml.relative_puzzle_part_target_poses) == 0: return pose = PoseStamped() pose.header.frame_id = "/odom_combined" pose.pose = deepcopy(yaml.puzzle_fixture) #TODO: WTF? #if you change the Point shit here, also change it in map.py! # pose.pose.position = add_point(pose.pose.position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0)) pose.pose.position = get_puzzle_fixture_center(yaml.puzzle_fixture) box = self.make_box("puzzle", pose, [0.32, 0.32, 0.1]) print("puzzle_fixture collision object = " + str(box)) self.safe_objects.append(box.id) self.add_object(box)
def add_yaml(self, yaml): """ Adds some fixed collision objects that are defined in the yaml description :param yaml: yaml description of the task :type: Task """ self.add_ground() if yaml is None: self.add_cam_mast(0.92, 0.92) else: x = yaml.mast_of_cam.base_pose.linear.x y = yaml.mast_of_cam.base_pose.linear.y self.add_cam_mast(x, y) if len(yaml.relative_puzzle_part_target_poses) == 0: return pose = PoseStamped() pose.header.frame_id = "/odom_combined" pose.pose = deepcopy(yaml.puzzle_fixture) #TODO: WTF? #if you change the Point shit here, also change it in map.py! # pose.pose.position = add_point(pose.pose.position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0)) pose.pose.position = get_puzzle_fixture_center(yaml.puzzle_fixture) box = self.make_box("puzzle", pose, [0.32, 0.32, 0.1]) print("puzzle_fixture collision object = "+str(box)) self.safe_objects.append(box.id) self.add_object(box)
def goto_shelf2(self): shelf = PoseStamped() header = Header() header.frame_id = 'map' shelf.header = header shelf.pose.position = Point(-1.0, self.dist_to_shelfs, 0.0) shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) self(shelf)
def relative_pose(self, position, orientation): shelf = PoseStamped() header = Header() header.frame_id = 'base_footprint' shelf.header = header shelf.pose.position = Point(*position) shelf.pose.orientation = Quaternion(*orientation) self(shelf)
def goto_shelf4(self): shelf = PoseStamped() header = Header() header.frame_id = 'map' shelf.header = header shelf.pose.position = Point(-2.9, self.dist_to_shelfs, 0.000) shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) # shelf.pose.orientation = Quaternion(*quaternion_from_euler(0,0,pi*.99)) self(shelf)
def goto_shelf1(self): # self.client.cancel_all_goals() shelf = PoseStamped() header = Header() header.frame_id = 'map' shelf.header = header shelf.pose.position = Point(-0.0, self.dist_to_shelfs, 0.0) shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) self(shelf)
def np_array_to_pose_stampeds(self, nparray, orientation, header): pose_list = [] for p in nparray: pose = PoseStamped() pose.pose.position = Point(*p) pose.pose.orientation = orientation pose.header = header pose_list.append(pose) return pose_list
def odomCallback(odomData): global lastOdom,tfl,lastMapPose lastOdom = odomData temp = PoseStampedMsg() temp.pose = lastOdom.pose.pose temp.header = lastOdom.header try: #now lastmappose has map coords tfl.transformPose("map",temp,lastMapPose) except tf.TransformException: rospy.roserror("Transform Error")
def transform_to(self, pose_target, target_frame="/odom_combined"): ''' Transforms the pose_target into the target_frame. :param pose_target: object to transform as PoseStamped/PointStamped/Vector3Stamped/CollisionObject/PointCloud2 :param target_frame: goal frame id :return: transformed object ''' if pose_target.header.frame_id == target_frame: return pose_target odom_pose = None i = 0 while odom_pose is None and i < 10: try: #now = rospy.Time.now() #self.__listener.waitForTransform(target_frame, pose_target.header.frame_id, now, rospy.Duration(4)) if type(pose_target) is CollisionObject: i = 0 new_co = deepcopy(pose_target) for cop in pose_target.primitive_poses: p = PoseStamped() p.header = pose_target.header p.pose.orientation = cop.orientation p.pose.position = cop.position p = self.transform_to(p, target_frame) if p is None: return None new_co.primitive_poses[i].position = p.pose.position new_co.primitive_poses[i].orientation = p.pose.orientation i += 1 new_co.header.frame_id = target_frame return new_co if type(pose_target) is PoseStamped: odom_pose = self.__listener.transformPose(target_frame, pose_target) break if type(pose_target) is Vector3Stamped: odom_pose = self.__listener.transformVector3(target_frame, pose_target) break if type(pose_target) is PointStamped: odom_pose = self.__listener.transformPoint(target_frame, pose_target) break if type(pose_target) is PointCloud2: odom_pose = self.__listener.transformPointCloud(target_frame, pose_target) break except Exception, e: print "tf error:::", e rospy.sleep(0.5) i += 1 rospy.logdebug("tf fail nr. " + str(i))
def goal_pose_to_cmd(self, goal_pose, arm): moving_arm_cmd = ArmCommand() moving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL moving_arm_cmd.goal_pose = goal_pose asdf = SemanticFloat64() asdf.semantics = 'l_rot_error' asdf.value = 0.0174 moving_arm_cmd.convergence_thresholds.append(asdf) asdf = SemanticFloat64() asdf.semantics = 'l_trans_error' asdf.value = 0.001 moving_arm_cmd.convergence_thresholds.append(asdf) nonmoving_arm_cmd = ArmCommand() right_goal = PoseStamped() right_goal.header.frame_id = 'right_gripper_tool_frame' right_goal.pose.orientation.w = 1 nonmoving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL nonmoving_arm_cmd.goal_pose = self.transformPose( 'base_footprint', right_goal) cmd = WholeBodyCommand() cmd.type = WholeBodyCommand.STANDARD_CONTROLLER if arm == WiggleGoal.LEFT_ARM: cmd.left_ee = moving_arm_cmd cmd.right_ee = nonmoving_arm_cmd else: cmd.left_ee = nonmoving_arm_cmd cmd.right_ee = moving_arm_cmd return cmd
def get_place_position_for_puzzle(destination, orientation): rospy.logdebug("get_place_position_for_puzzle") place_poses = [] pitch = pi / 2 if orientation_to_vector( rotate_quaternion(deepcopy(orientation), 0, pitch, 0)).z > 0: pitch = -pitch place_pose_1 = PoseStamped() place_pose_1.header.frame_id = "/odom_combined" place_pose_1.pose.position = deepcopy(destination.point) place_pose_1.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, 0) place_poses.append(place_pose_1) #roll = add_point(deepcopy(destination.point),Point(-1, 0, 0)) place_pose_2 = deepcopy(place_pose_1) place_pose_2.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, pi / 2) place_poses.append(place_pose_2) #roll = add_point(deepcopy(destination.point),Point(0, 1, 0)) place_pose_3 = deepcopy(place_pose_1) place_pose_3.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, pi) place_poses.append(place_pose_3) #roll = add_point(deepcopy(destination.point),Point(0, -1, 0)) place_pose_4 = deepcopy(place_pose_1) place_pose_4.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, pi * 1.5) place_poses.append(place_pose_4) return place_poses
def marker_callback(self, data): if data.event_type == InteractiveMarkerFeedback.MOUSE_UP: rospy.loginfo("Current Marker Pose for %s:", self.mesh_resource) self.tool_marker.header = data.header self.tool_marker.pose = data.pose self.marker_pub.publish(self.tool_marker) rospy.loginfo("\n%s", PoseStamped(data.header, data.pose)) self.server.applyChanges()
def test1_1(self): p = PoseStamped() p.header.frame_id = "/odom_combined" p.pose.position = Point(1,0,0) p.pose.orientation = euler_to_quaternion(0,pi,0) p2 = Point(0.7815,0,0) p1 = get_fingertip(p) self.assertTrue(abs(p1.point.x - p2.x) < 0.0001) self.assertTrue(abs(p1.point.y - p2.y) < 0.0001) self.assertTrue(abs(p1.point.z - p2.z) < 0.0001)
def add_ground(self, height=-0.01): """ Adds a big flat box to the planning scene. :param height: z value :type: float """ pose = PoseStamped() pose.header.frame_id = "/odom_combined" pose.pose.position = Point(0, 0, height) pose.pose.orientation = Quaternion(0, 0, 0, 1) box = self.make_box("ground" + str(height), pose, [3, 3, 0.01]) self.safe_objects.append(box.id) self.add_object(box)
def __init__(self): self.n = int self.n = 0 """ GETTING PARAMS """ self.pose = rospy.get_param('~pose', 'amcl_pose') self.path = rospy.get_param('~path', 'path') self.bag_file_name = rospy.get_param( '~bag_file_name', '/home/arturo/rosbag_files/test_interfaces/arturo_voice_1.bag') """MEMBERS""" self.path = Path() self.stamped_pose = PoseStamped() """ SUBSCRIBERS """ self.amcl_pose_subscriber = rospy.Subscriber( self.pose, PoseWithCovarianceStamped, self.amcl_pose_callback) """ PUBLISHERS """ self.path_publisher = rospy.Publisher(self.path, Path, queue_size=1)
def make_scan_pose(point, distance, angle, frame="/odom_combined", n=8): """ Calculates "n" positions around and pointing to "point" with "distance" in an "angle" :param point: Point the positions will be pointing to :type: Point :param distance: distance from the point to tcp origin :type: float :param angle: pitch of the gripper, 0 = horizontally, pi/2 = downwards :type: float :param frame: the frame that the positions will have :type: str :param n: number of positions :type: float :return: list of scan poses :type: [PoseStamped """ #TODO: assumes odom_combined look_positions = [] alpha = pi/2 - angle r = sin(alpha) * distance h = cos(alpha) * distance h_vector = Point() h_vector.z = h muh = add_point(point, h_vector) for i in range(0, n): a = 2 * pi * ((i + 0.0) / (n + 0.0)) b = a - (pi / 2) look_point = Point(cos(a), sin(a), 0) look_point = set_vector_length(r, look_point) look_point = add_point(look_point, muh) roll_point = Point(cos(b), sin(b), 0) roll_point = add_point(roll_point, point) look_pose = PoseStamped() look_pose.header.frame_id = frame look_pose.pose.orientation = three_points_to_quaternion(look_point, point, roll_point) look_pose.pose.position = look_point look_positions.append(look_pose) look_positions.sort(key=lambda x: magnitude(x.pose.position)) return look_positions
def addConveyorBelt(self, msg): print "addConveyorBelt callback" # print msg # zu 1 drop_point = msg.conveyor_belt.drop_center_point print drop_point # zu 2 conveyor_belt_width = msg.conveyor_belt.drop_deviation.y * 2 print conveyor_belt_width # zu 3 conveyor_belt_height = drop_point.z print conveyor_belt_height # zu 4 conveyor_belt_length = msg.conveyor_belt.move_direction_and_length.y print conveyor_belt_length pose = PoseStamped() pose.header.frame_id = "/odom_combined" # h w d # pose.pose.position = Point(drop_point.x, # (drop_point.y - abs(conveyor_belt_length / 2)), # drop_point.z - abs(conveyor_belt_height / 2)) # pose.pose.orientation = Quaternion(0, 0, 0, 1) l = math.sqrt(msg.conveyor_belt.move_direction_and_length.x * msg.conveyor_belt.move_direction_and_length.x + conveyor_belt_length * conveyor_belt_length) alpha = math.pi / 2 - math.asin(conveyor_belt_length / l) wx = conveyor_belt_width * math.cos(alpha) wy = conveyor_belt_width * math.sin(alpha) w = wx + wy pose.pose.position = Point( drop_point.x, (drop_point.y - abs(conveyor_belt_length / 2)), drop_point.z - abs(conveyor_belt_height / 2)) pose.pose.orientation = Quaternion(0, 0, 0, 1) print pose cb_size = [w * 2, l, conveyor_belt_height] co = utils.manipulation.get_planning_scene().make_box( "conveyor_belt", pose, cb_size) utils.manipulation.get_planning_scene().add_object(co)
def make_grasp_pose(depth, gripper_origin, roll, frame_id): """ Calculates a Pose pointing from gripper_origin to direction. :param depth: desired distance between gripper_origin and direction :type: float :param gripper_origin: origin of the gripper :type: float :param roll: point for roll :type: Point :param frame_id: frame_id :type: str :return: grasp pose :type: PoseStamped """ grasp = PoseStamped() grasp.header.frame_id = frame_id grasp.pose.orientation = three_points_to_quaternion(gripper_origin, geometry_msgs.msg.Point(0, 0, 0), roll) grasp.pose.position = multiply_point(depth, gripper_origin) return grasp
def add_cam_mast(self, x, y): """ Adds a Cylinder where the cam mast is. :param x: x coordinate :type: float :param y: y coordinate :type: float """ pose = PoseStamped() pose.header.frame_id = "/odom_combined" pose.pose.position = Point(x, y, 0.55) pose.pose.orientation = Quaternion(0, 0, 0, 1) c1 = self.make_cylinder("cm1", pose, [1.1, 0.05]) self.safe_objects.append(c1.id) self.add_object(c1) pose.pose.position = Point(x, y, 1.0) pose.pose.orientation = Quaternion(0, 0, 0, 1) c1 = self.make_box("mast_cams", pose, [0.3, 0.3, 0.3]) self.safe_objects.append(c1.id) self.add_object(c1)
def test2_1(self): co = CollisionObject() co.operation = CollisionObject.ADD co.id = "muh" co.header.frame_id = "/odom_combined" cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions.append(0.3) cylinder.dimensions.append(0.03) co.primitives = [cylinder] co.primitive_poses = [Pose()] co.primitive_poses[0].position = Point(1.2185, 0,0) co.primitive_poses[0].orientation = Quaternion(0,0,0,1) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions.append(0.1) box.dimensions.append(0.1) box.dimensions.append(0.1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[1].position = Point(1.1185, 0,0) co.primitive_poses[1].orientation = Quaternion(0,0,0,1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[2].position = Point(0, 0,0) co.primitive_poses[2].orientation = Quaternion(0,0,0,1) p = PoseStamped() p.header.frame_id = "/odom_combined" p.pose.position = Point(1,0,0) p.pose.orientation = euler_to_quaternion(0,0,0) self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
if __name__ == '__main__': rospy.init_node('brain') move_base = MoveBase(True) move_base.client.cancel_all_goals() tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) goals = [] while not rospy.is_shutdown(): cmd = raw_input('[add|clear|replay|q] - ') if cmd == 'add': transform = tfBuffer.lookup_transform('map', 'base_footprint', rospy.Time()) goal_pose = PoseStamped() goal_pose.header.frame_id = transform.header.frame_id goal_pose.pose.position.x = transform.transform.translation.x goal_pose.pose.position.y = transform.transform.translation.y goal_pose.pose.position.z = transform.transform.translation.z goal_pose.pose.orientation = transform.transform.rotation goals.append(goal_pose) elif cmd == 'clear': goals = [] elif cmd == 'replay': for goal in goals: rospy.loginfo('moving to: \n{}'.format(goal)) move_base.move_to(goal) transform = tfBuffer.lookup_transform('map', 'base_footprint', rospy.Time()) goal_diff(goal, transform)
def make6DofMarker(self, interaction_mode, root_link, tip_link): def normed_q(x, y, z, w): return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w]) int_marker = InteractiveMarker() p = PoseStamped() p.header.frame_id = tip_link p.pose.orientation.w = 1 int_marker.header.frame_id = tip_link int_marker.pose.orientation.w = 1 int_marker.pose = self.transformPose(root_link, p).pose int_marker.header.frame_id = root_link int_marker.scale = .2 int_marker.name = "eef_{}_to_{}".format(root_link, tip_link) # insert a box self.makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D: "MOVE_3D", InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D", InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D" } int_marker.name += "_" + control_modes_dict[interaction_mode] control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) return int_marker
'ur5_wrist_1_joint', 'ur5_wrist_2_joint', 'ur5_wrist_3_joint', ] joint_state.position = [ 0.0174419, -1.33713, 1.47922, -1.05844, -0.00214559, -0.660072 ] # 0.0138173009806, # -0.948155685642, # 1.64415424887, # 0.833014565285, # -0.00345735390984, # -1.52754142152, ] test.send_joint_goal(joint_state) goal = PoseStamped() goal.header.frame_id = 'gripper_tool_frame' goal.pose.position.x = 0.1 goal.pose.position.z = -0.2 goal.pose.orientation.w = 1.0 test.send_cart_goal(goal) goal = PoseStamped() goal.header.frame_id = 'gripper_tool_frame' goal.pose.position = Point(0, 0, 0) goal.pose.orientation = Quaternion(0.0, 0.0, 0.70710678, 0.70710678) test.send_cart_goal(goal) goal = PoseStamped() goal.header.frame_id = 'gripper_tool_frame' goal.pose.position = Point(0, -0.1, 0)
def getPoseStamped(self, header=None): if header is None: header = Header(0, rospy.rostime.get_rostime(), "world") return PoseStamped(header, self.getPose())