def GetOdomFromState(state, spot_wrapper, use_vision=True): """Maps odometry data from robot state proto to ROS Odometry message Args: data: Robot State proto spot_wrapper: A SpotWrapper object Returns: Odometry message """ odom_msg = Odometry() local_time = spot_wrapper.robotToLocalTime( state.kinematic_state.acquisition_timestamp) odom_msg.header.stamp = rospy.Time(local_time.seconds, local_time.nanos) if use_vision == True: odom_msg.header.frame_id = 'vision' tform_body = get_vision_tform_body( state.kinematic_state.transforms_snapshot) else: odom_msg.header.frame_id = 'odom' tform_body = get_odom_tform_body( state.kinematic_state.transforms_snapshot) odom_msg.child_frame_id = 'body' pose_odom_msg = PoseWithCovariance() pose_odom_msg.pose.position.x = tform_body.position.x pose_odom_msg.pose.position.y = tform_body.position.y pose_odom_msg.pose.position.z = tform_body.position.z pose_odom_msg.pose.orientation.x = tform_body.rotation.x pose_odom_msg.pose.orientation.y = tform_body.rotation.y pose_odom_msg.pose.orientation.z = tform_body.rotation.z pose_odom_msg.pose.orientation.w = tform_body.rotation.w odom_msg.pose = pose_odom_msg twist_odom_msg = GetOdomTwistFromState(state, spot_wrapper).twist odom_msg.twist = twist_odom_msg return odom_msg
def __init__(self, image_response): self.image = self.extract_image(image_response) self.body_T_image_sensor = get_a_tform_b(image_response.shot.transforms_snapshot, \ BODY_FRAME_NAME, image_response.shot.frame_name_image_sensor) self.vision_T_body = get_vision_tform_body(image_response.shot.transforms_snapshot) if not self.body_T_image_sensor: raise Exception("Won't work.") if image_response.source.pinhole: resolution = numpy.asarray([ \ image_response.source.cols, \ image_response.source.rows]) focal_length = numpy.asarray([ \ image_response.source.pinhole.intrinsics.focal_length.x, \ image_response.source.pinhole.intrinsics.focal_length.y]) principal_point = numpy.asarray([ \ image_response.source.pinhole.intrinsics.principal_point.x, \ image_response.source.pinhole.intrinsics.principal_point.y]) else: raise Exception("Won't work.") sensor_T_vo = (self.vision_T_body * self.body_T_image_sensor).inverse() camera_projection_mat = numpy.eye(4) camera_projection_mat[0, 0] = (focal_length[0] / resolution[0]) camera_projection_mat[0, 2] = (principal_point[0] / resolution[0]) camera_projection_mat[1, 1] = (focal_length[1] / resolution[1]) camera_projection_mat[1, 2] = (principal_point[1] / resolution[1]) self.MVP = camera_projection_mat.dot(sensor_T_vo.to_matrix())
def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name='odom'): """Send a trajectory motion command to the robot. Args: goal_x: Position X coordinate in meters goal_y: Position Y coordinate in meters goal_heading: Pose heading in radians cmd_duration: Time-to-live for the command in seconds. frame_name: frame_name to be used to calc the target position. 'odom' or 'vision' """ self._at_goal = False self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration if frame_name == 'vision': vision_tform_body = frame_helpers.get_vision_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) vision_tform_goal = vision_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.trajectory_command( goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=frame_helpers.VISION_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) elif frame_name == 'odom': odom_tform_body = frame_helpers.get_odom_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) odom_tform_goal = odom_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.trajectory_command( goal_x=odom_tform_goal.x, goal_y=odom_tform_goal.y, goal_heading=odom_tform_goal.rot.to_yaw(), frame_name=frame_helpers.ODOM_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) else: raise ValueError('frame_name must be \'vision\' or \'odom\'') if response[0]: self._last_trajectory_command = response[2] return response[0], response[1]
def final_state(self): """Check if the current robot state is within range of the fiducial position.""" robot_state = get_vision_tform_body(self.robot_state.kinematic_state.transforms_snapshot) robot_angle = robot_state.rot.to_yaw() if self._current_tag_world_pose.size != 0: x_dist = abs(self._current_tag_world_pose[0] - robot_state.x) y_dist = abs(self._current_tag_world_pose[1] - robot_state.y) angle = abs(self._angle_desired - robot_angle) if ((x_dist < self._x_eps) and (y_dist < self._y_eps) and (angle < self._angle_eps)): return True return False
def offset_tag_pose(self, object_rt_world, dist_margin=1.0): """Offset the go-to location of the fiducial and compute the desired heading.""" robot_rt_world = get_vision_tform_body(self.robot_state.kinematic_state.transforms_snapshot) robot_to_object_ewrt_world = np.array( [object_rt_world.x - robot_rt_world.x, object_rt_world.y - robot_rt_world.y, 0]) robot_to_object_ewrt_world_norm = robot_to_object_ewrt_world / np.linalg.norm( robot_to_object_ewrt_world) heading = self.get_desired_angle(robot_to_object_ewrt_world_norm) goto_rt_world = np.array([ object_rt_world.x - robot_to_object_ewrt_world_norm[0] * dist_margin, object_rt_world.y - robot_to_object_ewrt_world_norm[1] * dist_margin ]) return goto_rt_world, heading
def get_pose(robot_state_client): if not robot_state_client: return None, np.zeros((3, ), np.float32) # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand # the robot's current position in the vision frame. vision_tform_body = get_vision_tform_body( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) # Position and heading # position = (vision_tform_body.x, vision_tform_body.y, vision_tform_body.z) # # We get heading by rotating the forward pointing vector (1., 0., 0.) # heading = np.array(vision_tform_body.rotation.transform_point(1., 0., 0.), np.float32) # x y yaw pos = np.array((vision_tform_body.x, vision_tform_body.y, vision_tform_body.rotation.to_yaw()), np.float32) print("pos %f %f %f" % (pos[0], pos[1], np.rad2deg(pos[2]))) # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw())) return vision_tform_body, pos
def main(): import argparse parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) options = parser.parse_args() # Create robot object. sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster') sdk.load_app_token(options.app_token) robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) # Check that an estop is connected with the robot so that the robot commands can be executed. verify_estop(robot) # Create the lease client. lease_client = robot.ensure_client(LeaseClient.default_service_name) lease = lease_client.acquire() robot.time_sync.wait_for_sync() lk = bosdyn.client.lease.LeaseKeepAlive(lease_client) # Setup clients for the robot state and robot command services. robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) # Power on the robot and stand it up. robot.power_on() blocking_stand(robot_command_client) # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand # the robot's current position in the vision frame. vision_tform_body = get_vision_tform_body( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) # We want to command a trajectory to go forward one meter in the x-direction of the body. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter forward in the x-axis of the body. # Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the # rotation of the body at the goal to match the rotation of the body currently, so we do not need # to transform the rotation. body_tform_goal = math_helpers.SE3Pose(x=1, y=0, z=0, rot=math_helpers.Quat()) # We can then transform this transform to get the goal position relative to the vision frame. vision_tform_goal = vision_tform_body * body_tform_goal # Command the robot to go to the goal point in the vision frame. The command will stop at the new # position in the vision frame. robot_cmd = RobotCommandBuilder.trajectory_command( goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=VISION_FRAME_NAME) end_time = 2.0 robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) # Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body, # which describes the robot body's position in the odom frame. odom_tform_body = get_odom_tform_body(robot_state_client.get_robot_state(). kinematic_state.transforms_snapshot) # We want to command a trajectory to go backwards one meter and to the left one meter. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value) # in the y-axis of the body. body_tform_goal = math_helpers.SE3Pose(x=-1, y=1, z=0, rot=math_helpers.Quat()) # We can then transform this transform to get the goal position relative to the odom frame. odom_tform_goal = odom_tform_body * body_tform_goal # Command the robot to go to the goal point in the odom frame. The command will stop at the new # position in the odom frame. robot_cmd = RobotCommandBuilder.trajectory_command( goal_x=odom_tform_goal.x, goal_y=odom_tform_goal.y, goal_heading=odom_tform_goal.rot.to_yaw(), frame_name=ODOM_FRAME_NAME) end_time = 5.0 robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) return True
def move_relative(robot_command_client, robot_state_client, x, y, yaw, start_frame=None, timeout=10.0, block=False, verbose=False): if start_frame is not None: vision_tform_body = start_frame else: # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand # the robot's current position in the vision frame. vision_tform_body = get_vision_tform_body( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) # We want to command a trajectory to go forward one meter in the x-direction of the body. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter forward in the x-axis of the body. # Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the # rotation of the body at the goal to match the rotation of the body currently, so we do not need # to transform the rotation. rot_quat = math_helpers.Quat().from_yaw(yaw) body_tform_goal = math_helpers.SE3Pose(x=x, y=y, z=0, rot=rot_quat) # We can then transform this transform to get the goal position relative to the vision frame. vision_tform_goal = vision_tform_body * body_tform_goal # print ((vision_tform_body.x, vision_tform_body.y), (vision_tform_goal.x, vision_tform_goal.y), ) # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw())) # Command the robot to go to the goal point in the vision frame. The command will stop at the new # position in the vision frame. robot_cmd = RobotCommandBuilder.trajectory_command( # synchro_se2_trajectory_command( # goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=VISION_FRAME_NAME) # robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + timeout) command_id = robot_command_client.robot_command( lease=None, command=robot_cmd, end_time_secs=time.time() + timeout) # This will only issue the command, but it is not blocking. So we wait and check status manually now = time.time() start_time = time.time() end_time = time.time() + timeout while now < end_time: time_until_timeout = end_time - now rpc_timeout = max(time_until_timeout, 1) start_call_time = time.time() try: response = robot_command_client.robot_command_feedback( command_id, timeout=rpc_timeout) except TimedOutError: # Excuse the TimedOutError and let the while check bail us out if we're out of time. print("Response timeout error") pass else: # print (response.status, robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING) # pdb.set_trace() if response.status != robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING: raise ValueError( 'Stand (ID {}) no longer processing (now {})'.format( command_id, response.Status.Name(response.status))) if verbose: print( response.feedback.mobility_feedback. se2_trajectory_feedback.status, basic_command_pb2. SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL) if (response.feedback.mobility_feedback. se2_trajectory_feedback.status == basic_command_pb2. SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL): # basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING): if verbose: print(response) break # delta_t = time.time() - start_call_time # time.sleep(max(min(delta_t, update_time), 0.0)) time.sleep(0.1) now = time.time() if verbose: print("Took %f sec" % (time.time() - start_time))
def get_robot_state(self): ''' Returns tuple of kinematic_state, robot_state kinematic_state: timestamp joint_states [] ko_tform_body body_twist_rt_ko ground_plane_rt_ko vo_tform_body robot_state: power_state battery_states[] comms_states[] system_fault_state estop_states[] behavior_fault_state ''' robot_state = self.robot_state_client.get_robot_state() rs_msg = spot_ros_msgs.msg.RobotState() ''' PowerState conversion ''' rs_msg.power_state.header.stamp.secs = robot_state.power_state.timestamp.seconds rs_msg.power_state.header.stamp.nsecs = robot_state.power_state.timestamp.nanos rs_msg.power_state.motor_power_state = robot_state.power_state.motor_power_state #[enum] rs_msg.power_state.shore_power_state = robot_state.power_state.shore_power_state #[enum] rs_msg.power_state.locomotion_charge_percentage = robot_state.power_state.locomotion_charge_percentage.value #[google.protobuf.DoubleValue] rs_msg.power_state.locomotion_estimated_runtime.secs = robot_state.power_state.locomotion_estimated_runtime.seconds #[google.protobuf.Duration] ''' BatteryState conversion [repeated field] ''' for battery_state in robot_state.battery_states: battery_state_msg = sensor_msgs.msg.BatteryState() header = std_msgs.msg.Header() header.stamp.secs = battery_state.timestamp.seconds header.stamp.nsecs = battery_state.timestamp.nanos header.frame_id = battery_state.identifier #[string] battery_state_msg.header = header battery_state_msg.percentage = battery_state.charge_percentage.value/100 #[double] # NOTE: Using battery_state_msg.charge as the estimated runtime in sec battery_state_msg.charge = battery_state.estimated_runtime.seconds #[google.protobuf.Duration] battery_state_msg.current = battery_state.current.value #[DoubleValue] battery_state_msg.voltage = battery_state.voltage.value #[DoubleValue] # NOTE: Ignoring battery_state.temperatures for now; no field in BatteryState maps directly to it battery_state_msg.power_supply_status = battery_state.status #[enum] rs_msg.battery_states.append(battery_state_msg) ''' CommsState conversion [repeated field] ''' for comms_state in robot_state.comms_states: comms_state_msg = spot_ros_msgs.msg.CommsState() comms_state_msg.header.stamp.secs = comms_state.timestamp.seconds #[google.protobuf.Timestamp] comms_state_msg.header.stamp.nsecs = comms_state.timestamp.nanos #[google.protobuf.Timestamp] comms_state_msg.wifi_mode = comms_state.wifi_state.current_mode #[enum] Note: wifi_state is oneof comms_state_msg.essid = comms_state.wifi_state.essid #[string] rs_msg.comms_states.append(comms_state_msg) ''' SystemFaultState conversion ''' ### faults is Repeated ### for fault in robot_state.system_fault_state.faults: system_fault_msg = spot_ros_msgs.msg.SystemFault() system_fault_msg.header.frame_id = fault.name #[string] system_fault_msg.header.stamp.secs = fault.onset_timestamp.seconds #[google.protobuf.Timestamp] system_fault_msg.header.stamp.nsecs = fault.onset_timestamp.nanos #[google.protobuf.Timestamp] system_fault_msg.duration.secs = fault.duration.seconds #[google.protobuf.Duration] system_fault_msg.duration.nsecs = fault.duration.nanos #[google.protobuf.Duration] system_fault_msg.code = fault.code #[int32] system_fault_msg.uid = fault.uid #[uint64] system_fault_msg.error_message = fault.error_message #[string] system_fault_msg.attributes = fault.attributes #[repeated-string] system_fault_msg.severity = fault.severity #[enum] rs_msg.system_fault_state.faults.append(system_fault_msg) ### historical_faults is Repeated ### for historical_fault in robot_state.system_fault_state.faults: system_fault_msg = spot_ros_msgs.msg.SystemFault() system_fault_msg.header.frame_id = historical_fault.name #[string] system_fault_msg.header.stamp.secs = historical_fault.onset_timestamp.seconds #[google.protobuf.Timestamp] system_fault_msg.header.stamp.nsecs = historical_fault.onset_timestamp.nanos #[google.protobuf.Timestamp] system_fault_msg.duration.secs = historical_fault.duration.seconds #[google.protobuf.Duration] system_fault_msg.duration.nsecs = historical_fault.duration.nanos #[google.protobuf.Duration] system_fault_msg.code = historical_fault.code #[int32] system_fault_msg.uid = historical_fault.uid #[uint64] system_fault_msg.error_message = historical_fault.error_message #[string] system_fault_msg.attributes = historical_fault.attributes #[repeated-string] system_fault_msg.severity = historical_fault.severity #[enum] rs_msg.system_fault_state.historical_faults.append(system_fault_msg) #[map<string,enum>] if robot_state.system_fault_state.aggregated: for key, value in robot_state.system_fault_state.aggregated.items(): kv = diagnostic_msgs.msg.KeyValue() kv.key = key kv.value = value rs_msg.system_fault_state.aggregated.append(kv) ''' EStopState conversion [repeated field] ''' for estop_state in robot_state.estop_states: estop_msg = spot_ros_msgs.msg.EStopState() estop_msg.header.stamp.secs = estop_state.timestamp.seconds #[google.protobuf.Timestamp] estop_msg.header.stamp.nsecs = estop_state.timestamp.nanos #[google.protobuf.Timestamp] estop_msg.header.frame_id = estop_state.name #[string] estop_msg.type = estop_state.type #[enum] estop_msg.state = estop_state.state #[enum] estop_msg.state_description = estop_state.state_description #[string] rs_msg.estop_states.append(estop_msg) ''' KinematicState conversion ''' ks_msg = spot_ros_msgs.msg.KinematicState() ks_msg.header.stamp.secs = robot_state.kinematic_state.acquisition_timestamp.seconds ks_msg.header.stamp.nsecs = robot_state.kinematic_state.acquisition_timestamp.nanos ### joint_states is repeated ### js = sensor_msgs.msg.JointState() js.header.stamp = ks_msg.header.stamp for joint_state in robot_state.kinematic_state.joint_states: js.name.append(joint_state.name) # [string] js.position.append(joint_state.position.value) # Note: angle in rad js.velocity.append(joint_state.velocity.value) # Note: ang vel # NOTE: ang accel. JointState doesn't have accel. Ignoring joint_state.acceleration for now. js.effort.append(joint_state.load.value) # Note: Torque in N-m ks_msg.joint_states = js # SE3Pose representing transform of Spot's Body frame relative to the inertial Vision frame vision_tform_body = get_vision_tform_body(robot_state.kinematic_state.transforms_snapshot) ks_msg.vision_tform_body.translation.x = vision_tform_body.x ks_msg.vision_tform_body.translation.y = vision_tform_body.y ks_msg.vision_tform_body.translation.z = vision_tform_body.z ks_msg.vision_tform_body.rotation.x = vision_tform_body.rot.x ks_msg.vision_tform_body.rotation.y = vision_tform_body.rot.y ks_msg.vision_tform_body.rotation.z = vision_tform_body.rot.z ks_msg.vision_tform_body.rotation.w = vision_tform_body.rot.w # odom_tform_body: SE3Pose representing transform of Spot's Body frame relative to the odometry frame odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot) ks_msg.odom_tform_body.translation.x = odom_tform_body.x ks_msg.odom_tform_body.translation.y = odom_tform_body.y ks_msg.odom_tform_body.translation.z = odom_tform_body.z ks_msg.odom_tform_body.rotation.x = odom_tform_body.rot.x ks_msg.odom_tform_body.rotation.y = odom_tform_body.rot.y ks_msg.odom_tform_body.rotation.z = odom_tform_body.rot.z ks_msg.odom_tform_body.rotation.w = odom_tform_body.rot.w ''' velocity_of_body_in_vision ''' ks_msg.velocity_of_body_in_vision.linear.x = robot_state.kinematic_state.velocity_of_body_in_vision.linear.x ks_msg.velocity_of_body_in_vision.linear.y = robot_state.kinematic_state.velocity_of_body_in_vision.linear.y ks_msg.velocity_of_body_in_vision.linear.z = robot_state.kinematic_state.velocity_of_body_in_vision.linear.z ks_msg.velocity_of_body_in_vision.angular.x = robot_state.kinematic_state.velocity_of_body_in_vision.angular.x ks_msg.velocity_of_body_in_vision.angular.y = robot_state.kinematic_state.velocity_of_body_in_vision.angular.y ks_msg.velocity_of_body_in_vision.angular.z = robot_state.kinematic_state.velocity_of_body_in_vision.angular.z ''' velocity_of_body_in_odom ''' ks_msg.velocity_of_body_in_odom.linear.x = robot_state.kinematic_state.velocity_of_body_in_odom.linear.x ks_msg.velocity_of_body_in_odom.linear.y = robot_state.kinematic_state.velocity_of_body_in_odom.linear.y ks_msg.velocity_of_body_in_odom.linear.z = robot_state.kinematic_state.velocity_of_body_in_odom.linear.z ks_msg.velocity_of_body_in_odom.angular.x = robot_state.kinematic_state.velocity_of_body_in_odom.angular.x ks_msg.velocity_of_body_in_odom.angular.y = robot_state.kinematic_state.velocity_of_body_in_odom.angular.y ks_msg.velocity_of_body_in_odom.angular.z = robot_state.kinematic_state.velocity_of_body_in_odom.angular.z ### BehaviorFaultState conversion '''faults is repeated''' for fault in robot_state.behavior_fault_state.faults: behaviour_fault_state_msg = spot_ros_msgs.msg.BehaviorFaultState() behaviour_fault_state_msg.header.frame_id = fault.behavior_fault_id #[uint32] behaviour_fault_state_msg.header.stamp.secs = fault.onset_timestamp.seconds #[google.protobuf.Timestamp] behaviour_fault_state_msg.header.stamp.nsecs = fault.onset_timestamp.nanos #[google.protobuf.Timestamp] behaviour_fault_state_msg.cause = fault.cause #[enum] behaviour_fault_state_msg.status = fault.status #[enum] rs_msg.behavior_fault_states.append(behaviour_fault_state_msg) ### FootState conversion [repeated] for foot_state in robot_state.foot_state: foot_state_msg = spot_ros_msgs.msg.FootState() foot_state_msg.foot_position_rt_body.x = foot_state.foot_position_rt_body.x #[double] foot_state_msg.foot_position_rt_body.y = foot_state.foot_position_rt_body.y #[double] foot_state_msg.foot_position_rt_body.z = foot_state.foot_position_rt_body.z #[double] foot_state_msg.contact = foot_state.contact #[enum] rs_msg.foot_states.append(foot_state_msg) return ks_msg, rs_msg #kinematic state message, robot state message
def hello_arm(config): """A simple example of using the Boston Dynamics API to command Spot's arm and body at the same time. Please be aware that this demo causes the robot to walk and move its arm. You can have some control over how much the robot moves -- see _L_ROBOT_SQUARE and _L_ARM_CIRCLE -- but regardless, the robot should have at least (_L_ROBOT_SQUARE + 3) m of space in each direction when this demo is used.""" # See hello_spot.py for an explanation of these lines. bosdyn.client.util.setup_logging(config.verbose) sdk = bosdyn.client.create_standard_sdk('HelloSpotClient') robot = sdk.create_robot(config.hostname) robot.authenticate(config.username, config.password) robot.time_sync.wait_for_sync() assert robot.has_arm(), "Robot requires an arm to run this example." # Verify the robot is not estopped and that an external application has registered and holds # an estop endpoint. assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) lease = lease_client.acquire() robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): # Now, we are ready to power on the robot. This call will block until the power # is on. Commands would fail if this did not happen. We can also check that the robot is # powered at any point. robot.logger.info( "Powering on robot... This may take a several seconds.") robot.power_on(timeout_sec=20) assert robot.is_powered_on(), "Robot power on failed." robot.logger.info("Robot powered on.") # Tell the robot to stand up. The command service is used to issue commands to a robot. robot.logger.info("Commanding robot to stand...") command_client = robot.ensure_client( RobotCommandClient.default_service_name) blocking_stand(command_client, timeout_sec=10) robot.logger.info("Robot standing.") time.sleep(1.0) # Unstow the arm # Build the unstow command using RobotCommandBuilder unstow = RobotCommandBuilder.arm_ready_command() # Issue the command via the RobotCommandClient command_client.robot_command(unstow) robot.logger.info("Unstow command issued.") time.sleep(1.0) # Get robot pose in vision frame from robot state (we want to send commands in vision # frame relative to where the robot stands now) robot_state = robot_state_client.get_robot_state() vision_T_world = get_vision_tform_body( robot_state.kinematic_state.transforms_snapshot) # In this demo, the robot will walk in a square while moving its arm in a circle. # There are some parameters that you can set below: # Initialize a robot command message, which we will build out below command = robot_command_pb2.RobotCommand() # points in the square x_vals = np.array([0, 1, 1, 0, 0]) y_vals = np.array([0, 0, 1, 1, 0]) # duration in seconds for each move seconds_arm = _SECONDS_FULL / (_N_POINTS + 1) seconds_body = _SECONDS_FULL / x_vals.size # Commands will be sent in the visual odometry ("vision") frame frame_name = VISION_FRAME_NAME # Build an arm trajectory by assembling points (in meters) # x will be the same for each point x = _L_ROBOT_SQUARE + 0.5 for ii in range(_N_POINTS + 1): # Get coordinates relative to the robot's body y = (_L_ROBOT_SQUARE / 2) - _L_ARM_CIRCLE * (np.cos( 2 * ii * math.pi / _N_POINTS)) z = _VERTICAL_SHIFT + _L_ARM_CIRCLE * (np.sin( 2 * ii * math.pi / _N_POINTS)) # Using the transform we got earlier, transform the points into the world frame x_ewrt_vision, y_ewrt_vision, z_ewrt_vision = vision_T_world.transform_point( x, y, z) # Add a new point to the robot command's arm cartesian command se3 trajectory # This will be an se3 trajectory point point = command.synchronized_command.arm_command.arm_cartesian_command.pose_trajectory_in_task.points.add( ) # Populate this point with the desired position, rotation, and duration information point.pose.position.x = x_ewrt_vision point.pose.position.y = y_ewrt_vision point.pose.position.z = z_ewrt_vision point.pose.rotation.x = vision_T_world.rot.x point.pose.rotation.y = vision_T_world.rot.y point.pose.rotation.z = vision_T_world.rot.z point.pose.rotation.w = vision_T_world.rot.w traj_time = (ii + 1) * seconds_arm duration = seconds_to_duration(traj_time) point.time_since_reference.CopyFrom(duration) # set the frame for the hand trajectory command.synchronized_command.arm_command.arm_cartesian_command.root_frame_name = frame_name # Build a body se2trajectory by first assembling points for ii in range(x_vals.size): # Pull the point in the square relative to the robot and scale according to param x = _L_ROBOT_SQUARE * x_vals[ii] y = _L_ROBOT_SQUARE * y_vals[ii] # Transform desired position into world frame x_ewrt_vision, y_ewrt_vision, z_ewrt_vision = vision_T_world.transform_point( x, y, 0) # Add a new point to the robot command's arm cartesian command se3 trajectory # This will be an se2 trajectory point point = command.synchronized_command.mobility_command.se2_trajectory_request.trajectory.points.add( ) # Populate this point with the desired position, angle, and duration information point.pose.position.x = x_ewrt_vision point.pose.position.y = y_ewrt_vision point.pose.angle = vision_T_world.rot.to_yaw() traj_time = (ii + 1) * seconds_body duration = seconds_to_duration(traj_time) point.time_since_reference.CopyFrom(duration) # set the frame for the body trajectory command.synchronized_command.mobility_command.se2_trajectory_request.se2_frame_name = frame_name # Constrain the robot not to turn, forcing it to strafe laterally. speed_limit = SE2VelocityLimit( max_vel=SE2Velocity(linear=Vec2(x=2, y=2), angular=0), min_vel=SE2Velocity(linear=Vec2(x=-2, y=-2), angular=0)) mobility_params = spot_command_pb2.MobilityParams( vel_limit=speed_limit) command.synchronized_command.mobility_command.params.CopyFrom( RobotCommandBuilder._to_any(mobility_params)) # Send the command using the command client # The SE2TrajectoryRequest requires an end_time, which is set # during the command client call robot.logger.info("Sending arm and body trajectory commands.") command_client.robot_command(command, end_time_secs=time.time() + _SECONDS_FULL) time.sleep(_SECONDS_FULL + 2) # Power the robot off. By specifying "cut_immediately=False", a safe power off command # is issued to the robot. This will attempt to sit the robot before powering off. robot.power_off(cut_immediately=False, timeout_sec=20) assert not robot.is_powered_on(), "Robot power off failed." robot.logger.info("Robot safely powered off.") finally: # If we successfully acquired a lease, return it. lease_client.return_lease(lease)
def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name='odom', precise_position=False): """Send a trajectory motion command to the robot. Args: goal_x: Position X coordinate in meters goal_y: Position Y coordinate in meters goal_heading: Pose heading in radians cmd_duration: Time-to-live for the command in seconds. frame_name: frame_name to be used to calc the target position. 'odom' or 'vision' precise_position: if set to false, the status STATUS_NEAR_GOAL and STATUS_AT_GOAL will be equivalent. If true, the robot must complete its final positioning before it will be considered to have successfully reached the goal. """ self._at_goal = False self._near_goal = False self._last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration if frame_name == 'vision': vision_tform_body = frame_helpers.get_vision_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) vision_tform_goal = vision_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.synchro_se2_trajectory_point_command( goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=frame_helpers.VISION_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) elif frame_name == 'odom': odom_tform_body = frame_helpers.get_odom_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) odom_tform_goal = odom_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.synchro_se2_trajectory_point_command( goal_x=odom_tform_goal.x, goal_y=odom_tform_goal.y, goal_heading=odom_tform_goal.rot.to_yaw(), frame_name=frame_helpers.ODOM_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) else: raise ValueError('frame_name must be \'vision\' or \'odom\'') if response[0]: self._last_trajectory_command = response[2] return response[0], response[1]