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]
