def yawToQuat(angle): quatList = quaternion_from_euler(0.0,0.0,angle) quat = QuaternionMsg() quat.x = quatList[0] quat.y = quatList[1] quat.z = quatList[2] quat.w = quatList[3] return quat
def yawToQuat(angle): quatList = quaternion_from_euler(0.0, 0.0, angle) quat = QuaternionMsg() quat.x = quatList[0] quat.y = quatList[1] quat.z = quatList[2] quat.w = quatList[3] return quat
def createQuat(x,y,z): quatList = quaternion_from_euler(x,y,z) quat = QuaternionMsg() quat.x = quatList[0] quat.y = quatList[1] quat.z = quatList[2] quat.w = quatList[3] return quat
def createQuat(x, y, z): quatList = quaternion_from_euler(x, y, z) quat = QuaternionMsg() quat.x = quatList[0] quat.y = quatList[1] quat.z = quatList[2] quat.w = quatList[3] return quat
def get_message(self): object_id = self.object_name.split("_")[-1] msg = ModelDescription() # generate InstanceId msg.instance_id = InstanceId() msg.instance_id.class_name = self.object_type msg.instance_id.id = object_id # HACK this information should be in some ontology if 'ProductWithAN' in self.object_type: msg.instance_id.ns = '/IAISupermarket/Catalog' elif 'ShelfLabel' in self.object_type: msg.instance_id.ns = '/IAISupermarket/ShelfLabels' else: msg.instance_id.ns = '/IAISupermarket/Shelves' # generate MeshDescription # NOTE: default is to use the class name msg.mesh_description = MeshDescription() msg.mesh_description.path_to_mesh = '' msg.mesh_description.path_to_material = '' # generate Tag's msg.tags.append(self.get_tag('SemLog', 'LogType', 'Static')) msg.tags.append(self.get_tag('SemLog', 'Id', object_id)) msg.tags.append(self.get_tag('SemLog', 'Class', self.object_type)) # set the pose msg.pose = Pose() msg.pose.position = Point() msg.pose.position.x = self.transform[2][0] msg.pose.position.y = self.transform[2][1] msg.pose.position.z = self.transform[2][2] msg.pose.orientation = Quaternion() msg.pose.orientation.x = self.transform[3][0] msg.pose.orientation.y = self.transform[3][1] msg.pose.orientation.z = self.transform[3][2] msg.pose.orientation.w = self.transform[3][3] return msg
def __init__(self): """Class printing the marker of the wheelchair in rviz """ """ MEMBERS """ self.markers = UI_Markers() self.user_dir = Quaternion( ) # The user commanded direction, it can come from keyboard = key_cmd, voice = voice_cmd, or head = head_cmd self.user_vel = Twist( ) # The user commanded direction, it can come from keyboard = key_cmd, voice = voice_cmd, or head = head_cmd """ROS PARAMETERS""" self.tf_prefix = rospy.get_param( 'tf_prefix', '') # # Reads the tf_prefix from the ROS namespace if self.tf_prefix is not '': self.tf_prefix = '/' + self.tf_prefix self.discrete_interface = rospy.get_param( '~discrete_interface', False ) ## To enable/disable the discrete mode Set true when using the keyboard """ ROS SUBSCRIBERS """ self.user_dir_sub = rospy.Subscriber("user_dir", Quaternion, self.__user_dir_callback__, None, 1) self.user_vel_sub = rospy.Subscriber("user_vel", Twist, self.__user_vel_callback__, None, 1) self.vocal_command_sub = rospy.Subscriber("recognizer/output", String, self.__voice_callback__, None, 1) """ROS PUBLISHERS""" self.wheelchair_marker_pub = rospy.Publisher("wheelchair_marker", Marker, queue_size=1) self.humans_marker_array_pub = rospy.Publisher("humans_marker_array", MarkerArray, queue_size=1) self.dir_marker_pub = rospy.Publisher("dir_marker", Marker, queue_size=1) self.vel_marker_pub = rospy.Publisher("vel_marker", Marker, queue_size=1) self.text_marker_pub = rospy.Publisher('text_marker', Marker, queue_size=1) """Start """ r = rospy.Rate(10.0) while not rospy.is_shutdown(): wheelchair_marker = self.fill_wheelchair_marker() humans = self.fill_humans_marker_array() self.wheelchair_marker_pub.publish(wheelchair_marker) self.wheelchair_marker_pub.publish(self.fill_sit_human_marker( )) # I send it in the same topic it works well. self.humans_marker_array_pub.publish(humans) self.print_vel() r.sleep()
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 parse_pose(pose): rospy.loginfo('parse_pose') f_pose = Pose() f_pose.position.x = pose[0] f_pose.position.y = pose[1] f_pose.position.z = pose[2] f_pose.orientation = Quaternion(*quaternion_from_euler(pose[3], pose[4], pose[5])) return f_pose
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 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 rotate_quaternion_by_quaternion(q1, q2): """ Rotates a quaternion by another quaternion :param q1: first Quaternion :type: Quaternion :param q2: second Quaternion :type: Quaternion :return: rotated Quaternion :type: Quaternion """ r = quaternion_multiply([q1.x, q1.y, q1.z, q1.w], [q2.x, q2.y, q2.z, q2.w]) return Quaternion(*r)
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 euler_to_quaternion(roll, pitch, yaw): """ Creates a quaternion out of roll, pitch, yaw :param roll: :type: float :param pitch: :type: float :param yaw: :type: float :return: Quaternion from roll pitch yaw :type: Quaternion """ return Quaternion(*quaternion_from_euler(roll, pitch, yaw))
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)
def prolog_to_transform(frame_id, child_frame_id, translation, rotation): """ :type frame_id: str :type child_frame_id: str :type translation: list :type rotation: list :rtype: TransformStamped """ if child_frame_id==None or translation==None or rotation==None: return None t = TransformStamped() t.header.frame_id = frame_id.encode('utf-8') t.child_frame_id = child_frame_id.encode('utf-8') t.transform.translation = Point(*translation) t.transform.rotation = Quaternion(*rotation) return t
def get_marker(self): marker = Marker() marker.header.frame_id = self.ref_frame marker.type = marker.MESH_RESOURCE marker.action = Marker.ADD marker.id = 1337 marker.ns = self.get_short_name() marker.color = self.color marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.frame_locked = True marker.pose.position = Point(*self.transform[-2]) marker.pose.orientation = Quaternion(*self.transform[-1]) marker.mesh_resource = self.mesh_path[:-4] + '.dae' return marker
def rotate_quaternion(q, roll, pitch, yaw): ''' Rotates a quaternion by roll, pitch, yaw. :param q: Quaternion :type: Quaternion :param roll: roll :type: float :param pitch: pitch :type: float :param yaw: yaw :type: float :return: rotated quaternion :type: Quaternion ''' angle = quaternion_from_euler(roll, pitch, yaw) no = quaternion_multiply([q.x, q.y, q.z, q.w], angle) return Quaternion(*no)
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 get_marker(self): marker = Marker() marker.header.frame_id = self.ref_frame marker.type = marker.MESH_RESOURCE marker.action = Marker.ADD marker.id = 1337 marker.ns = self.object_name marker.color = self.color marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.frame_locked = True marker.pose.position = Point(*self.transform[-2]) marker.pose.orientation = Quaternion(*self.transform[-1]) if len(self.mesh_path) > 2 and self.mesh_path[0] == "'": marker.mesh_resource = self.mesh_path[1:-1] else: marker.mesh_resource = self.mesh_path return marker
def three_points_to_quaternion(origin, to, roll=None): """ Calculates a quaternion that points from "origin" to "to" and lies in the plane defined by "origin", "to" and "roll". :param origin: form this point :type: Point :param to: to this point :type: Point :param roll: in a plane defined by this third point :type: Point or None :return: orientation :type: Quaternion """ muh = False if roll is None: #TODO buggy roll = Point(0, 0, origin.z + 1.000001) muh = True n_1 = subtract_point(to, origin) n_1 = normalize(n_1) n = subtract_point(roll, origin) n = normalize(n) n_2 = subtract_point(n, multiply_point(dot_product(n, n_1), n_1)) n_2 = normalize(n_2) n_3 = cross_product(n_1, n_2) n_3 = normalize(n_3) rm = np.array(((n_1.x, n_2.x, n_3.x, 0), (n_1.y, n_2.y, n_3.y, 0), (n_1.z, n_2.z, n_3.z, 0), (0, 0, 0, 1)), dtype=np.float64) q = quaternion_from_matrix(rm) q = Quaternion(*q) if muh: q = rotate_quaternion(q, -pi / 2, 0, 0) return q
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
def make_6dof_marker(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() int_marker.header.frame_id = tip_link int_marker.pose.orientation.w = 1 int_marker.scale = MARKER_SCALE int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link) # insert a box self.make_sphere_control(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D: u'MOVE_3D', InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D', InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D' } int_marker.name += u'_' + control_modes_dict[interaction_mode] control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = u'rotate_x' control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = u'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 = u'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 = u'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 = u'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 = u'move_y' control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) self.markers[int_marker.name] = int_marker return int_marker
def pub_goal_marker(self, header, pose): """ :param header: :type header: std_msgs.msg._Header.Header :param pose: :type pose: Pose """ ma = MarkerArray() m = Marker() m.action = Marker.ADD m.type = Marker.CYLINDER m.header = header old_q = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] # x m.pose = deepcopy(pose) m.scale.x = 0.05 * MARKER_SCALE m.scale.y = 0.05 * MARKER_SCALE m.scale.z = MARKER_SCALE muh = qv_mult(old_q, [m.scale.z / 2, 0, 0]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.pose.orientation = Quaternion(*quaternion_multiply( old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0]))) m.color.r = 1 m.color.g = 0 m.color.b = 0 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 0 ma.markers.append(m) # y m = deepcopy(m) m.pose = deepcopy(pose) muh = qv_mult(old_q, [0, m.scale.z / 2, 0]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.pose.orientation = Quaternion(*quaternion_multiply( old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0]))) m.color.r = 0 m.color.g = 1 m.color.b = 0 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 1 ma.markers.append(m) # z m = deepcopy(m) m.pose = deepcopy(pose) muh = qv_mult(old_q, [0, 0, m.scale.z / 2]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.color.r = 0 m.color.g = 0 m.color.b = 1 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 2 ma.markers.append(m) self.marker_pub.publish(ma)
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) goal.pose.orientation = Quaternion(-0.35355339, -0.35355339, -0.14644661, 0.85355339) test.send_cart_goal(goal)
def matrixToPose(matrix): t = tuple(translation_from_matrix(matrix)) q = tuple(quaternion_from_matrix(matrix)) return Pose(Point(t[0], t[1], t[2]), Quaternion(q[0], q[1], q[2], q[3]))
def wiggle_cb(self, goal): # check for invalid goals if goal.arm != WiggleGoal.LEFT_ARM and goal.arm != WiggleGoal.RIGHT_ARM: rospy.logerr('arm goal should be {} or {}'.format( WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM)) return self.wiggle_action_server.set_aborted( text='arm goal should be {} or {}'.format( WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM)) if np.linalg.norm( np.array([ goal.goal_pose.pose.orientation.x, goal.goal_pose.pose.orientation.y, goal.goal_pose.pose.orientation.z, goal.goal_pose.pose.orientation.w ])) < 0.99: rospy.logerr('invalid orientation in goal position') return self.wiggle_action_server.set_aborted( text='invalid orientation in goal position') rospy.loginfo('wiggle wiggle wiggle') movement_duration = goal.timeout.data.to_sec() wiggle_per_sec = goal.cycle_time number_of_wiggles = int(movement_duration / wiggle_per_sec) number_of_wiggle_points = 10 radius_x = goal.upperbound_x radius_y = goal.upperbound_y hz = number_of_wiggle_points / wiggle_per_sec r = rospy.Rate(hz) tcp_goal_pose = self.transformPose('left_gripper_tool_frame', goal.goal_pose) spiral = self.create_spiral(goal_height=tcp_goal_pose.pose.position.z, radius_x=radius_x, radius_y=radius_y, number_of_points=number_of_wiggle_points, number_of_wiggles=number_of_wiggles) header = Header() header.frame_id = 'left_gripper_tool_frame' pose_list = self.np_array_to_pose_stampeds(spiral, Quaternion(0, 0, 0, 1), header) pose_list = [ deepcopy(self.transformPose('base_footprint', pose)) for pose in pose_list ] end_pose = self.transformPose('base_footprint', goal.goal_pose) for i, pose in enumerate(pose_list): if self.check_for_stop(): return self.pub_wiggle(pose, goal.arm) wiggle_feedback = WiggleFeedback() wiggle_feedback.progress = .9 * (i + 1) / float(len(pose_list)) self.wiggle_action_server.publish_feedback(wiggle_feedback) r.sleep() # move back to real goal time_to_get_to_endpose = rospy.Duration(1.0) deadline = rospy.get_rostime() + time_to_get_to_endpose while rospy.get_rostime() < deadline: if self.check_for_stop(): return self.pub_wiggle(end_pose, goal.arm) result = WiggleFeedback() result.progress = 1. self.wiggle_action_server.publish_feedback(result) self.wiggle_action_server.set_succeeded(result)