我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用geometry_msgs.msg.Vector3()。
def drive(gest): if gest.data == 1: #FIST turtlesimPub.publish("go back") tsPub.publish(Twist(Vector3(-1.0, 0, 0), Vector3(0, 0, 0))) elif gest.data == 2 and armState == 1: #WAVE_IN, RIGHT arm turtlesimPub.publish("go left") tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0))) elif gest.data == 2 and armState == 2: #WAVE_IN, LEFT arm turtlesimPub.publish("go right") tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0))) elif gest.data == 3 and armState == 1: #WAVE_OUT, RIGHT arm turtlesimPub.publish("go right") tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0))) elif gest.data == 3 and armState == 2: #WAVE_OUT, LEFT arm turtlesimPub.publish("go left") tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0))) elif gest.data == 4: #FINGERS_SPREAD turtlesimPub.publish("go forward") tsPub.publish(Twist(Vector3(1.0, 0, 0), Vector3(0, 0, 0)))
def ControlListener(): rospy.init_node('robot_motion_control', anonymous=True) rospy.Subscriber("robot1Com", controldata, callback1) P1.pub = rospy.Publisher('/home1/command', Vector3, queue_size=10) rospy.Subscriber("robot2Com", controldata, callback2) P2.pub = rospy.Publisher('/home2/command', Vector3, queue_size=10) while not rospy.is_shutdown(): rospy.spin() return # spin() simply keeps python from exiting until this node is stopped #rospy.spin()
def fmt(obj, nest_level=0): """ Format any common object """ if nest_level > 10: return "" if isinstance(obj, float): return "{0:.3f}".format(obj) if isinstance(obj, list): return "(" + ",".join(map(lambda x: fmt(x, nest_level + 1), obj)) + ")" if isinstance(obj, (numpy.ndarray, numpy.generic)): return fmt(obj.tolist(), nest_level + 1) if isinstance(obj, dict): pairs = map(lambda x, y: "(" + fmt(x) + "," + fmt(y, nest_level + 1) + ")", obj.items()) return fmt(pairs) if isinstance(obj, Vector3): return "({},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z)) if isinstance(obj, Quaternion): return "({},{},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z), fmt(obj.w)) # print " obj " + str(obj) + " is of type " + str(type(obj)) return str(obj)
def determine_hand_move_time(self, sidename, position, orientation, cur_transform): move_time = self.HAND_TRAJECTORY_TIME desired_q = msg_q_to_q(orientation) cur_q = msg_q_to_q(cur_transform.rotation) rotation_amount = quaternion_multiply(desired_q, conjugate_q(cur_q)) rot_w = rotation_amount[3] if rot_w < 0: rot_w = -rot_w add_time = 0 if rot_w < 0.85: add_time = self.HAND_ROTATION_TIME # if rot_w < 0.92 # add_time = self.HAND_ROTATION_TIME # if rot_w < 0.75: # add_time += self.HAND_ROTATION_TIME if add_time > 0: rospy.loginfo('Doing significant {} hand rotation, ' 'adding rotation wait of {}.'.format(sidename, add_time)) move_time += add_time return move_time # Moves the hand center to an absolute world position (Vector3) and orientation (Quaternion).
def figure_target(self, zarj, previous): """ Set the origin """ self.origin = previous.target if abs(self.distance) > 0.001: pelvis = zarj.transform.lookup_transform('world', 'pelvis', rospy.Time()).transform quaternion = [pelvis.rotation.w, pelvis.rotation.x, pelvis.rotation.y, pelvis.rotation.z] matrix = quaternion_matrix(quaternion) point = np.matrix([0, self.distance, 0, 1], dtype='float32') point.resize((4, 1)) rotated = matrix*point self.target = Vector3(self.origin.x - rotated.item(1), self.origin.y + rotated.item(2), self.origin.z) else: self.target = self.origin if self.turn is not None: self.target_orientation = self.turn + \ previous.target_orientation
def walk_to(self, point): """ Shuffle to a given point, point is given for the left foot """ msg = FootstepDataListRosMessage() msg.default_transfer_time = self.DEFAULT_TRANSFER_TIME msg.default_swing_time = self.DEFAULT_SWING_TIME msg.execution_mode = msg.OVERRIDE # Override means replace others msg.unique_id = self.zarj.uid self.lookup_feet() delta = Vector3(point.x - self.lf_start_position.x, point.y - self.lf_start_position.y, 0) start_foot = self._find_first_xy_foot(point) msg.footstep_data_list = self.create_xy_steps(delta, start_foot, 0.15) self.steps = 0 self.planned_steps = len(msg.footstep_data_list) self.walk_failure = None self.start_walk = rospy.get_time() self.footstep_list_publisher.publish(msg) log('FootstepDataList: uid ' + str(msg.unique_id))
def turn_body_to_pose(self, pose): """ Turn the torso to a given orientation Angles smaller than 110 or larger than 250 are unstable """ log("Turn body to match pose {}".format(pose)) msg = ChestTrajectoryRosMessage() msg.unique_id = self.zarj.uid msg.execution_mode = msg.OVERRIDE result = self.zarj.transform.tf_buffer.transform(pose, 'world') point = SO3TrajectoryPointRosMessage() point.time = 1.0 point.orientation = result.pose.orientation point.angular_velocity = Vector3(0.0, 0.0, 0.0) msg.taskspace_trajectory_points = [point] self.chest_publisher.publish(msg)
def drive(gest): global movingState global zero global speed if gest.data == 1: #FIST movingState -= 1 elif gest.data == 4 or gest.data == 2: #FINGERS_SPREAD movingState += 1 elif gest.data == 3 : zero = y if movingState > 0 : movingState = 1 turtlesimPub.publish("go forward") speed = 1 # tsPub.publish(Twist(Vector3(1.0, 0, 0), Vector3(0, 0, 0))) elif movingState < 0 : movingState = -1 turtlesimPub.publish("go back") speed = -1 # tsPub.publish(Twist(Vector3(-1.0, 0, 0), Vector3(0, 0, 0))) else : speed = 0 print (speed)
def strength(emgArr1): emgArr=emgArr1.data # Define proportional control constant: K = 0.005 # Get the average muscle activation of the left, right, and all sides aveRight=(emgArr[0]+emgArr[1]+emgArr[2]+emgArr[3])/4 aveLeft=(emgArr[4]+emgArr[5]+emgArr[6]+emgArr[7])/4 ave=(aveLeft+aveRight)/2 # If all muscles activated, drive forward exponentially if ave > 500: tsPub.publish(Twist(Vector3(0.1*math.exp(K*ave),0,0),Vector3(0,0,0))) # If only left muscles activated, rotate proportionally elif aveLeft > (aveRight + 200): tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,K*ave))) # If only right muscles activated, rotate proportionally elif aveRight > (aveLeft + 200): tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,-K*ave))) # If not very activated, don't move (high-pass filter) else: tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,0)))
def marker_from_circle(circle, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0): marker = Marker() marker.header = make_header("/map") marker.ns = "Markers_NS" marker.id = index marker.type = Marker.CYLINDER marker.action = 0 # action=0 add/modify object # marker.color.r = 1.0 # marker.color.g = 0.0 # marker.color.b = 0.0 # marker.color.a = 0.4 marker.color = color marker.lifetime = rospy.Duration.from_sec(lifetime) marker.pose = Pose() marker.pose.position.z = z marker.pose.position.x = circle.center[0] marker.pose.position.y = circle.center[1] marker.scale = Vector3(circle.radius*2.0, circle.radius*2.0, 0) return marker
def marker_from_point_radius(point, radius, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0): marker = Marker() marker.header = make_header("/map") marker.ns = "Speed_NS" marker.id = index marker.type = Marker.CYLINDER marker.action = 0 # action=0 add/modify object # marker.color.r = 1.0 # marker.color.g = 0.0 # marker.color.b = 0.0 # marker.color.a = 0.4 marker.color = color marker.lifetime = rospy.Duration.from_sec(lifetime) marker.pose = Pose() marker.pose.position.z = z marker.pose.position.x = point[0] marker.pose.position.y = point[1] marker.scale = Vector3(radius*2.0, radius*2.0, 0.001) return marker
def test_transform(self): t = Transform( translation=Vector3(1, 2, 3), rotation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0)) ) t_mat = ros_numpy.numpify(t) np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0]) msg = ros_numpy.msgify(Transform, t_mat) np.testing.assert_allclose(msg.translation.x, t.translation.x) np.testing.assert_allclose(msg.translation.y, t.translation.y) np.testing.assert_allclose(msg.translation.z, t.translation.z) np.testing.assert_allclose(msg.rotation.x, t.rotation.x) np.testing.assert_allclose(msg.rotation.y, t.rotation.y) np.testing.assert_allclose(msg.rotation.z, t.rotation.z) np.testing.assert_allclose(msg.rotation.w, t.rotation.w)
def numpy_to_transform(arr): from tf import transformations shape, rest = arr.shape[:-2], arr.shape[-2:] assert rest == (4,4) if len(shape) == 0: trans = transformations.translation_from_matrix(arr) quat = transformations.quaternion_from_matrix(arr) return Transform( translation=Vector3(*trans), rotation=Quaternion(*quat) ) else: res = np.empty(shape, dtype=np.object_) for idx in np.ndindex(shape): res[idx] = Transform( translation=Vector3(*transformations.translation_from_matrix(arr[idx])), rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx])) )
def numpy_to_pose(arr): from tf import transformations shape, rest = arr.shape[:-2], arr.shape[-2:] assert rest == (4,4) if len(shape) == 0: trans = transformations.translation_from_matrix(arr) quat = transformations.quaternion_from_matrix(arr) return Pose( position=Vector3(*trans), orientation=Quaternion(*quat) ) else: res = np.empty(shape, dtype=np.object_) for idx in np.ndindex(shape): res[idx] = Pose( position=Vector3(*transformations.translation_from_matrix(arr[idx])), orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx])) )
def _tuple_to_msg_transform(tf_t): """ Converts a tf tuple into a geometry_msgs/Transform message :type tf_t: ((float, float, float), (float, float, float, float)) :rtype: geometry_msgs.msg.Transform """ transl = Vector3(*tf_t[0]) rot = Quaternion(*tf_t[1]) return Transform(transl, rot)
def drive_arc(self, omega, travelTime, sign): '''given the omega, travel time and direction, drive in the corresponding arc''' # The third parameter (sign) represents whether the forward velocity of the twist will be positive or negative now = rospy.Time.now() while rospy.Time.now() - now <= travelTime: self.twist = Twist(linear=Vector3(sign*SPEED,0,0), angular=Vector3(0,0,omega))
def __init__(self, name): self.tf_buffer = None self.tf_listener = None self.robot_name = name self.rate = rospy.Rate(10.0) self.base_pose = None self.pose_subscriber = None self.world_transform = Vector3(0, 0, 0) self.gazebo_subscriber = None self.gazebo_model = None
def received_pose_calibration(self, data): """ Receive a pose message for calibration """ if self.pose_subscriber is None: return self.base_pose = data self.pose_subscriber.unregister() self.pose_subscriber = None self.world_transform = Vector3( data.pose.pose.position.x - self.gazebo_model.x, data.pose.pose.position.y - self.gazebo_model.y, data.pose.pose.position.z - self.gazebo_model.z)
def v_to_msg_v(v): return Vector3(v[0], v[1], v[2])
def add_msg_v(msg_v1, msg_v2): return Vector3(msg_v1.x + msg_v2.x, msg_v1.y + msg_v2.y, msg_v1.z + msg_v2.z)
def move_hand_center_abs(self, sidename, position, orientation, move_time = None): desired_q = msg_q_to_q(orientation) # Use the hand trajectory message to move the hand if move_time is None: cur_transform = self.get_current_hand_center_transform(sidename) move_time = self.determine_hand_move_time(sidename, position, orientation, cur_transform) rospy.loginfo('Desired {} hand center orientation in world reference frame is {}.'.format( sidename, fmt_q_as_ypr(desired_q))) # Rotate from orientation based on x-axis being along hand to one where x-axis # is perpendicular to palm. if sidename == 'left': to_palm_q = [0, 0, -self.SQRT_TWO / 2, self.SQRT_TWO/2] elif sidename == 'right': to_palm_q = [0, 0, self.SQRT_TWO / 2, self.SQRT_TWO/2] else: rospy.logerr("Unknown side {}".format(sidename)) return perpendicular_to_palm_q = quaternion_multiply(desired_q, to_palm_q) palm_orientation = q_to_msg_q(perpendicular_to_palm_q) # rospy.loginfo('Palm quaternion desired: ' + str(perpendicular_to_palm_q)) msg = self.make_hand_trajectory(sidename, position, palm_orientation, self.HAND_TRAJECTORY_TIME) self.hand_trajectory_publisher.publish(msg) rospy.sleep(move_time + 0.5) # Moves the hand by a relative displacement and to a named orientation. If no orientation # is provided, then the orientation is not changed. The relative displacement is done # within the reference frame of the torso so Vector3(1,0,0) will move the hand forward # away from the front of the robot.
def create_xy_steps(self, movement, start_foot, stride=DEFAULT_STRIDE): """ Create a series of xy direction footsteps. """ footsteps = [] current_step = Vector3(0, 0, 0) foot = start_foot while current_step.x != movement.x or current_step.y != movement.y: if foot == start_foot: full_step = False d_x = movement.x - current_step.x d_y = movement.y - current_step.y if abs(d_x) > stride: d_x = sign(d_x) * stride full_step = True if abs(d_y) > stride: d_y = sign(d_y) * stride full_step = True current_step.x += d_x current_step.y += d_y footsteps.append( self.create_one_footstep(foot, [current_step.x, current_step.y, 0.0], world=True)) if not full_step: break else: footsteps.append( self.create_one_footstep(foot, [current_step.x, current_step.y, 0.0], world=True)) foot = invert_foot(foot) foot = invert_foot(foot) footsteps.append(self.create_one_footstep(foot, [current_step.x, current_step.y, 0.0], world=True)) return footsteps
def get_lean_points(self, angle): """ Return a set of trajectory points to accomplish a lean """ point = SO3TrajectoryPointRosMessage() point.time = 1.0 # Just give it a second to get there rotate = quaternion_about_axis(radians(angle), [0, 0, -1]) point.orientation = Quaternion(rotate[1], rotate[2], rotate[3], rotate[0]) point.angular_velocity = Vector3(0, 0, 0) log('Lean to: {}'.format(point)) points = [] points.append(point) return points
def lean_body(self, angle): """ Lean forward or back a given angle """ chest_position = self.zarj.transform.lookup_transform( 'world', 'torso', rospy.Time()).transform log("Lean body {} degrees".format(angle)) msg = ChestTrajectoryRosMessage() msg.unique_id = self.zarj.uid msg.execution_mode = msg.OVERRIDE euler = euler_from_quaternion((chest_position.rotation.w, chest_position.rotation.x, chest_position.rotation.y, chest_position.rotation.z)) point = SO3TrajectoryPointRosMessage() point.time = 1.0 point.orientation = chest_position.rotation qua = quaternion_from_euler(euler[0], euler[1] + radians(angle), euler[2]) point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0]) point.angular_velocity = Vector3(0.0, 0.0, 0.0) msg.taskspace_trajectory_points = [point] self.chest_publisher.publish(msg)
def turn_body(self, angle): """ Turn the torso by a given angle Angles smaller than 110 or larger than 250 are unstable """ log("Turn body {} degrees".format(angle)) chest_position = self.zarj.transform.lookup_transform( 'world', 'torso', rospy.Time()).transform msg = ChestTrajectoryRosMessage() msg.unique_id = self.zarj.uid msg.execution_mode = msg.OVERRIDE euler = euler_from_quaternion((chest_position.rotation.w, chest_position.rotation.x, chest_position.rotation.y, chest_position.rotation.z)) point = SO3TrajectoryPointRosMessage() point.time = 1.0 qua = quaternion_from_euler(euler[0] + radians(angle), euler[1], euler[2]) point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0]) point.angular_velocity = Vector3(0.0, 0.0, 0.0) msg.taskspace_trajectory_points = [point] self.chest_publisher.publish(msg)
def turn_body_to(self, angle, wait=True): """ Turn the torso to a given angle as related to the pelvis Angles smaller than 110 or larger than 250 are unstable """ log("Turn body to {} degrees".format(angle)) chest_position = self.zarj.transform.lookup_transform( 'pelvis', 'torso', rospy.Time()).transform msg = ChestTrajectoryRosMessage() msg.unique_id = self.zarj.uid msg.execution_mode = msg.OVERRIDE euler = euler_from_quaternion((chest_position.rotation.w, chest_position.rotation.x, chest_position.rotation.y, chest_position.rotation.z)) qua = quaternion_from_euler(radians(-angle-180), euler[1], euler[2]) pose = PoseStamped() pose.pose.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0]) pose.header.frame_id = 'pelvis' pose.header.stamp = rospy.Time() result = self.zarj.transform.tf_buffer.transform(pose, 'world') point = SO3TrajectoryPointRosMessage() point.time = 1.0 point.orientation = result.pose.orientation point.angular_velocity = Vector3(0.0, 0.0, 0.0) msg.taskspace_trajectory_points = [point] self.chest_publisher.publish(msg) if wait: rospy.sleep(point.time + 0.1)
def eyes_found_red(name, center): global found_button # Button pressed position gathered from Gazebo analysis buttonPos = Vector3(3.35, -0.73, 1.25) offset = Vector3(center.point.x - buttonPos.x, center.point.y - buttonPos.y, center.point.z - buttonPos.z) zarj_eyes.remove_detection_request(name) zarj_tf.set_world_transform(offset) found_button = True
def _do_repair(self, a, b): """ do repair """ if LEAK_SIDE: point_vector = Vector3(a, 0, b) else: point_vector = Vector3(0, a, b) print 'Move by', point_vector self.fc.zarj.hands.move_hand_center_rel('right', point_vector, move_time=0.5) # TODO: What are we going to do to ensure we are touching the wall? rospy.sleep(2.0) return False
def _vector_to(self, vector, to='base'): h = Header() h.stamp = rospy.Time(0) h.frame_id = '{}_gripper'.format(self.limb_name) v = Vector3(*vector) v_base = self.tl.transformVector3(to, Vector3Stamped(h, v)).vector v_cartesian = [v_base.x, v_base.y, v_base.z, 0, 0, 0] return v_cartesian
def vector3_from_point(p): return Vector3(p.x, p.y, p.z)
def turn(imuRead): global zero global y global K global speed y = imuRead.orientation.y if (imuRead.orientation.y>zero): tsPub.publish(Twist(Vector3(speed,0,0),Vector3(0,0,K*(imuRead.orientation.y-zero)))) if (imuRead.orientation.y<zero): tsPub.publish(Twist(Vector3(speed,0,0),Vector3(0,0,-K*(zero-imuRead.orientation.y)))) #print (y)
def callback1(data): # print data if data.cmdType == 'mov' and data.x == data.x: msg = Vector3() msg.x, msg.y, msg.z = pid.robot_ctrl(data) P1.pub.publish(msg)
def callback2(data): # print data if data.cmdType == 'mov' and data.x == data.x: msg = Vector3() msg.x, msg.y, msg.z = pid.robot_ctrl(data) P2.pub.publish(msg)
def _pub_acc_yaw_desired(self): a = Vector3() a.x = 0.0 a.y = 0.0 a.z = 0.2 self._acc_yaw_msg.acceleration_or_force = a #self._local_msg.yaw = 0.0 self._local_pub.publish(self._acc_yaw_msg)
def p_numpy_to_ros_vector(p_np): p = Vector3() p.x = p_np[0] p.y = p_np[1] p.z = p_np[2] return p
def __init__(self, ros_node): """ Make buttons to tell robot to look different directions """ super(tega_lookat_ui, self).__init__() # get reference to ros node so we can do callbacks to # publish messages self.ros_node = ros_node # put buttons in a box lookat_box = QtGui.QGroupBox(self) lookat_layout = QtGui.QGridLayout(lookat_box) lookat_box.setTitle("Lookat") # create lookat buttons and add to layout # TODO calibrate lookat! # look stage left self.lbutton = QtGui.QPushButton("left", lookat_box) self.lbutton.clicked.connect(lambda: self.ros_node.send_lookat_message(Vector3(-50,0,0))) lookat_layout.addWidget(self.lbutton, 1, 0) # look center self.cbutton = QtGui.QPushButton("center", lookat_box) self.cbutton.clicked.connect(lambda: self.ros_node.send_lookat_message(Vector3(0,1,0))) lookat_layout.addWidget(self.cbutton, 1, 1) # look stage right self.rbutton = QtGui.QPushButton("right", lookat_box) self.rbutton.clicked.connect(lambda: self.ros_node.send_lookat_message(Vector3(50,0,0))) lookat_layout.addWidget(self.rbutton, 1, 2) # look up self.ubutton = QtGui.QPushButton("up", lookat_box) self.ubutton.clicked.connect(lambda: self.ros_node.send_lookat_message(Vector3(0,50,0))) lookat_layout.addWidget(self.ubutton, 0, 1) # look down self.dbutton = QtGui.QPushButton("down", lookat_box) self.dbutton.clicked.connect(lambda: self.ros_node.send_lookat_message(Vector3(0,-50,0))) lookat_layout.addWidget(self.dbutton, 2, 1)
def __init__(self): self.STOP_ACTION = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) self.action = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) self.base_state = None rospy.Subscriber("/mobile_base/sensors/core", topic_format["/mobile_base/sensors/core"], self.update_base_state) self.termination_flag = False self.pause_flag = False self.stop_once = False
def test_vector3(self): v = Vector3(1, 2, 3) v_arr = ros_numpy.numpify(v) np.testing.assert_array_equal(v_arr, [1, 2, 3]) v_arrh = ros_numpy.numpify(v, hom=True) np.testing.assert_array_equal(v_arrh, [1, 2, 3, 0]) self.assertEqual(v, ros_numpy.msgify(Vector3, v_arr)) self.assertEqual(v, ros_numpy.msgify(Vector3, v_arrh)) with self.assertRaises(AssertionError): ros_numpy.msgify(Vector3, np.array([0, 0, 0, 1]))
def numpy_to_vector3(arr): if arr.shape[-1] == 4: assert np.all(arr[...,-1] == 0) arr = arr[...,:-1] if len(arr.shape) == 1: return Vector3(*arr) else: return np.apply_along_axis(lambda v: Vector3(*v), axis=-1, arr=arr)
def run(self): while self.is_looping(): navigation_tf_msg = TFMessage() try: motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True)) localization = self.navigation.getRobotPositionInMap() exploration_path = self.navigation.getExplorationPath() except Exception as e: navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D())) self.publishers["map_tf"].publish(navigation_tf_msg) self.rate.sleep() continue if len(localization) > 0 and len(localization[0]) == 3: navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2]) navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot) navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion)) self.publishers["map_tf"].publish(navigation_tf_msg) if len(localization) > 0: if self.publishers["uncertainty"].get_num_connections() > 0: uncertainty = Marker() uncertainty.header.frame_id = "/base_footprint" uncertainty.header.stamp = rospy.Time.now() uncertainty.type = Marker.CYLINDER uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2) uncertainty.pose.position = Point(0, 0, 0) uncertainty.pose.orientation = self.get_ros_quaternion( almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1)) uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5) self.publishers["uncertainty"].publish(uncertainty) if self.publishers["map"].get_num_connections() > 0: aggregated_map = None try: aggregated_map = self.navigation.getMetricalMap() except Exception as e: print("error " + str(e)) if aggregated_map is not None: map_marker = OccupancyGrid() map_marker.header.stamp = rospy.Time.now() map_marker.header.frame_id = "/map" map_marker.info.resolution = aggregated_map[0] map_marker.info.width = aggregated_map[1] map_marker.info.height = aggregated_map[2] map_marker.info.origin.orientation = self.get_ros_quaternion( almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1)) map_marker.info.origin.position.x = aggregated_map[3][0] map_marker.info.origin.position.y = aggregated_map[3][1] map_marker.data = aggregated_map[4] self.publishers["map"].publish(map_marker) if self.publishers["exploration_path"].get_num_connections() > 0: path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "/map" for node in exploration_path: current_node = PoseStamped() current_node.pose.position.x = node[0] current_node.pose.position.y = node[1] path.poses.append(current_node) self.publishers["exploration_path"].publish(path) self.rate.sleep()
def move_hand_center_rel(self, sidename, offset, orientation_name = "unchanged", move_time = None): current_center = self.get_current_hand_center_transform(sidename) cur_v = msg_v_to_v(current_center.translation) cur_q = msg_q_to_q(current_center.rotation) lf_frame = self.zarj.transform.lookup_transform('world', self.zarj.walk.lfname, rospy.Time()) lf_q = msg_q_to_q(lf_frame.transform.rotation) apply_lf_transform = True offset_v = msg_v_to_v(offset) abs_offset_v = rotate_v(offset_v, lf_q) if orientation_name == "none": # Same as handshake position. q = [0, 0, 0, 1] elif orientation_name == "faceup": # q = [0.5,-0.5,0.5,0.5] if doing direct in hand message. q = [self.SQRT_TWO/2, 0, 0, self.SQRT_TWO/2] elif orientation_name == "facedown": # q = [-0.5, 0.5, 0.5, 0.5] if doing direct in hand message. q = [-self.SQRT_TWO/2, 0, 0, self.SQRT_TWO/2] else: apply_lf_transform = False q = cur_q # print("----Torso_q: " + str(torso_q)) if apply_lf_transform: if sidename == 'left': q = conjugate_q(q) rospy.loginfo('Asked {} hand center to move, relative to left foot, by {} and ' 'to {}.'.format(sidename, offset_v, fmt_q_as_ypr(q))) # Apply torso rotation, and then our preferred rotation. # q = quaternion_multiply(torso_q, quaternion_multiply(q, conjugate_q(torso_q))) q = quaternion_multiply(lf_q, q) else: relative_q = quaternion_multiply(conjugate_q(lf_q), q) rospy.loginfo('Asked {} hand center to move, relative to left foot, by {} ' 'and to stay at {}.'.format(sidename, offset_v, fmt_q_as_ypr(relative_q))) position = v_to_msg_v(add_v(cur_v, abs_offset_v)) orientation = q_to_msg_q(q) # print("----Current q: " + str(cur_q)) # print("----Orientation q: " + str(q)) if move_time is None: move_time = self.determine_hand_move_time(sidename, position, orientation, current_center) self.move_hand_center_abs(sidename, position, orientation, move_time) # The value ypr is the array of desired changes in yaw, pitch and roll (in degrees) # Offset is a displacement in Vector3 representation and is relative to the frame of the torso. # Generally the hand trajectory message seems to do better with smaller rotations than large and # it appears to be quite good in giving highly precise orientations of the hand.
def mashButton(self): # Button pressed position gathered from Gazebo analysis #buttonPosition = Vector3(3.35, -0.73, 1.25) # we can subtract the length of the fingers... about 9 cm buttonPosition = Vector3(3.16, -0.73, 1.25) self.zarj.transform.transform_from_world(buttonPosition) handWorld = self.zarj.transform.lookup_transform('world', 'rightPalm', rospy.Time()) hp1 = SE3TrajectoryPointRosMessage() hp1.time = 0.2 hp1.position = deepcopy(handWorld.transform.translation) hp1.position.y = buttonPosition.y hp1.position.z = buttonPosition.z hp1.orientation = handWorld.transform.rotation hp2 = SE3TrajectoryPointRosMessage() hp2.time = 0.3 hp2.position.x = buttonPosition.x hp2.position.y = buttonPosition.y hp2.position.z = buttonPosition.z hp2.orientation = handWorld.transform.rotation hp3 = SE3TrajectoryPointRosMessage() hp3.time = 0.6 hp3.position = deepcopy(handWorld.transform.translation) hp3.position.y = buttonPosition.y hp3.position.z = buttonPosition.z hp3.orientation = handWorld.transform.rotation hp4 = SE3TrajectoryPointRosMessage() hp4.time = 0.8 hp4.position = deepcopy(handWorld.transform.translation) hp4.orientation = handWorld.transform.rotation msg = HandTrajectoryRosMessage() msg.robot_side = msg.RIGHT msg.base_for_control = msg.WORLD msg.execution_mode = msg.OVERRIDE msg.taskspace_trajectory_points = [ hp1, hp2, hp3, hp4 ] msg.unique_id = self.zarj.uid log('AS Mash Button: uid' + str(msg.unique_id)) self.hand_trajectory_publisher.publish(msg)
def __init__(self, zarj_os): log("Zarj walk initialization begin") self.zarj = zarj_os self.steps = 0 self.publishers = [] self.planned_steps = 0 self.stride = self.DEFAULT_STRIDE self.transfer_time = self.DEFAULT_TRANSFER_TIME self.swing_time = self.DEFAULT_SWING_TIME self.stance_width = self.DEFAULT_STANCE_WIDTH self.lf_start_position = Vector3() self.lf_start_orientation = Quaternion() self.rf_start_position = Vector3() self.rf_start_orientation = Quaternion() self.behavior = 0 self.start_walk = None self.walk_failure = None self.last_footstep = None lfp = "/ihmc_ros/{0}/left_foot_frame_name".format(self.zarj.robot_name) rfp = "/ihmc_ros/{0}/right_foot_frame_name".format(self.zarj.robot_name) if rospy.has_param(rfp) and rospy.has_param(lfp): self.lfname = rospy.get_param(lfp) self.rfname = rospy.get_param(rfp) if self.lfname == None or self.rfname == None: log("Zarj could not find left or right foot name") raise RuntimeError self.footstep_list_publisher = rospy.Publisher( "/ihmc_ros/{0}/control/footstep_list".format(self.zarj.robot_name), FootstepDataListRosMessage, queue_size=10) self.publishers.append(self.footstep_list_publisher) self.footstep_status_subscriber = rospy.Subscriber( "/ihmc_ros/{0}/output/footstep_status".format(self.zarj.robot_name), FootstepStatusRosMessage, self.receive_footstep_status) self.behavior_subscriber = rospy.Subscriber( "/ihmc_ros/{0}/output/behavior".format(self.zarj.robot_name), Int32, self.receive_behavior) self.abort_publisher = rospy.Publisher( "/ihmc_ros/{0}/control/abort_walking".format(self.zarj.robot_name), AbortWalkingRosMessage, queue_size=1) self.publishers.append(self.abort_publisher) self.lookup_feet() self.zarj.wait_for_publishers(self.publishers) log("Zarj walk initialization completed")
def mashButton(): # Button pressed position gathered from Gazebo analysis #buttonPosition = Vector3(3.35, -0.73, 1.25) # we can subtract the length of the fingers... about 9 cm buttonPosition = Vector3(3.16, -0.73, 1.25) zarj_tf.transform_from_world(buttonPosition) handWorld = zarj_tf.lookup_frame_in_world('rightPalm', rospy.Time()) hp1 = SE3TrajectoryPointRosMessage() hp1.time = 0.2 hp1.position = deepcopy(handWorld.transform.translation) hp1.position.y = buttonPosition.y hp1.position.z = buttonPosition.z hp1.orientation = handWorld.transform.rotation hp2 = SE3TrajectoryPointRosMessage() hp2.time = 0.3 hp2.position.x = buttonPosition.x hp2.position.y = buttonPosition.y hp2.position.z = buttonPosition.z hp2.orientation = handWorld.transform.rotation hp3 = SE3TrajectoryPointRosMessage() hp3.time = 0.6 hp3.position = deepcopy(handWorld.transform.translation) hp3.position.y = buttonPosition.y hp3.position.z = buttonPosition.z hp3.orientation = handWorld.transform.rotation hp4 = SE3TrajectoryPointRosMessage() hp4.time = 0.8 hp4.position = deepcopy(handWorld.transform.translation) hp4.orientation = handWorld.transform.rotation msg = HandTrajectoryRosMessage() msg.robot_side = msg.RIGHT msg.base_for_control = msg.WORLD msg.execution_mode = msg.OVERRIDE msg.taskspace_trajectory_points = [ hp1, hp2, hp3, hp4 ] msg.unique_id = uid() rospy.loginfo('AS Mash Button: uid' + str(msg.unique_id)) handTrajectoryPublisher.publish(msg)
def lift_left_foot(): global lf_start_position global lf_start_orientation wholemsg = WholeBodyTrajectoryRosMessage() wholemsg.unique_id = uid() wholemsg.left_hand_trajectory_message.robot_side = 0 wholemsg.right_hand_trajectory_message.robot_side = 1 wholemsg.left_arm_trajectory_message.robot_side = 0 wholemsg.right_arm_trajectory_message.robot_side = 1 wholemsg.left_foot_trajectory_message.robot_side = 0 wholemsg.right_foot_trajectory_message.robot_side = 1 wholemsg.left_foot_trajectory_message.unique_id = wholemsg.unique_id wholemsg.left_foot_trajectory_message.execution_mode = wholemsg.left_foot_trajectory_message.OVERRIDE zero = Vector3(0.0, 0.0, 0.0) p3 = SE3TrajectoryPointRosMessage() p3.time = 0.25 p3.position = deepcopy(lf_start_position) p3.position.z += 0.05 p3.position.x += 0.2 p3.orientation = deepcopy(lf_start_orientation) #p3.orientation.z += 0.2 p3.linear_velocity = zero p3.angular_velocity = zero p4 = SE3TrajectoryPointRosMessage() p4.time = 0.5 p4.position = deepcopy(lf_start_position) p4.position.z -= 0.01 p4.position.x += 0.4 p4.orientation = deepcopy(lf_start_orientation) #p4.orientation.z += 0.2 p4.linear_velocity = zero p4.angular_velocity = zero wholemsg.left_foot_trajectory_message.taskspace_trajectory_points = [ p3, p4 ] rospy.loginfo('JPW lifting left foot: uid' + str(wholemsg.unique_id)) BodyPublisher.publish(wholemsg)
def lift_right_foot(): global rf_start_position global rf_start_orientation wholemsg = WholeBodyTrajectoryRosMessage() wholemsg.unique_id = uid() wholemsg.left_hand_trajectory_message.robot_side = 0 wholemsg.right_hand_trajectory_message.robot_side = 1 wholemsg.left_arm_trajectory_message.robot_side = 0 wholemsg.right_arm_trajectory_message.robot_side = 1 wholemsg.left_foot_trajectory_message.robot_side = 0 wholemsg.right_foot_trajectory_message.robot_side = 1 wholemsg.right_foot_trajectory_message.unique_id = wholemsg.unique_id wholemsg.right_foot_trajectory_message.execution_mode = wholemsg.right_foot_trajectory_message.OVERRIDE zero = Vector3(0.0, 0.0, 0.0) p1 = SE3TrajectoryPointRosMessage() p1.time = .25 p1.position = deepcopy(rf_start_position) p1.position.z += 0.05 p1.position.x += 0.2 p1.orientation = deepcopy(rf_start_orientation) #p1.orientation.z += 0.2 p1.linear_velocity = zero p1.angular_velocity = zero p2 = SE3TrajectoryPointRosMessage() p2.time = .5 p2.position = deepcopy(rf_start_position) p2.position.z -= 0.01 p2.position.x += 0.4 p2.orientation = deepcopy(rf_start_orientation) #p2.orientation.z += 0.2 p2.linear_velocity = zero p2.angular_velocity = zero wholemsg.right_foot_trajectory_message.taskspace_trajectory_points = [ p1, p2 ] rospy.loginfo('JPW lifting right foot: uid' + str(wholemsg.unique_id)) BodyPublisher.publish(wholemsg)
