def test_arm_pose_command():
x = 0.75
y = 0
z = 0.25
qw = 1
qx = 0
qy = 0
qz = 0
command = RobotCommandBuilder.arm_pose_command(x, y, z, qw, qx, qy, qz, BODY_FRAME_NAME)
_test_has_synchronized(command)
_test_has_arm(command.synchronized_command)
assert command.synchronized_command.arm_command.HasField("arm_cartesian_command")
arm_cartesian_command = command.synchronized_command.arm_command.arm_cartesian_command
assert arm_cartesian_command.root_frame_name == BODY_FRAME_NAME
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.x == x
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.y == y
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.position.z == z
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.x == qx
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.y == qy
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.z == qz
assert arm_cartesian_command.pose_trajectory_in_task.points[0].pose.rotation.w == qw
# with a build_on_command
mobility_command = RobotCommandBuilder.synchro_sit_command()
command = RobotCommandBuilder.arm_pose_command(x, y, z, qw, qx, qy, qz, BODY_FRAME_NAME,
build_on_command=mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def _move(self, left_x, left_y, right_x):
"""Commands the robot with a velocity command based on left/right stick values.
Args:
left_x: X value of left stick.
left_y: Y value of left stick.
right_x: X value of right stick.
"""
# Stick left_x controls robot v_y
v_y = -left_x * VELOCITY_BASE_SPEED
# Stick left_y controls robot v_x
v_x = left_y * VELOCITY_BASE_SPEED
# Stick right_x controls robot v_rot
v_rot = -right_x * VELOCITY_BASE_ANGULAR
# Recreate mobility_params with the latest information
self.mobility_params = RobotCommandBuilder.mobility_params(
body_height=self.body_height,
locomotion_hint=self.mobility_params.locomotion_hint,
stair_hint=self.mobility_params.stair_hint)
cmd = RobotCommandBuilder.velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot,
params=self.mobility_params)
self.command_client.robot_command_async(cmd,
end_time_secs=time.time() +
VELOCITY_CMD_DURATION)
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 test_synchro_sit_command():
command = RobotCommandBuilder.synchro_sit_command()
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField("sit_request")
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_sit_command(build_on_command=arm_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def test_claw_gripper_close_command():
command = RobotCommandBuilder.claw_gripper_close_command()
_test_has_synchronized(command)
_test_has_gripper(command.synchronized_command)
assert (command.synchronized_command.gripper_command.WhichOneof("command") ==
"claw_gripper_command")
# with a build_on_command
mobility_command = RobotCommandBuilder.synchro_sit_command()
command = RobotCommandBuilder.claw_gripper_close_command(build_on_command=mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_gripper(command.synchronized_command)
def test_arm_carry_command():
command = RobotCommandBuilder.arm_ready_command()
_test_has_synchronized(command)
_test_has_arm(command.synchronized_command)
assert (command.synchronized_command.arm_command.WhichOneof("command") ==
'named_arm_position_command')
# with a build_on_command
mobility_command = RobotCommandBuilder.synchro_sit_command()
command = RobotCommandBuilder.arm_carry_command(build_on_command=mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
def test_synchro_se2_trajectory_point_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
command = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x, goal_y, goal_heading, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x, goal_y, goal_heading, frame, build_on_command=arm_command)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
_test_has_arm(command.synchronized_command)
def velocity_cmd_listener(self, twist):
"""Callback that sends instantaneous velocity [m/s] commands to Spot"""
MAX_MARCHING_TIME = 5
v_x = twist.linear.x
v_y = twist.linear.y
v_rot = twist.angular.z
all_zero = (v_x == 0) and (v_y == 0) and (v_rot == 0)
if (all_zero):
if self.t_last_non_zero_cmd < time.monotonic() - MAX_MARCHING_TIME:
# No need to send command, just return.
return
else:
self.t_last_non_zero_cmd = time.monotonic()
cmd = RobotCommandBuilder.velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot)
try:
self.command_client.robot_command(cmd,
end_time_secs=time.time() +
self.VELOCITY_CMD_DURATION)
except:
print("Invalid command: v_x=${},v_y=${},v_rot${}".format(
v_x, v_y, v_rot))
# rospy.loginfo(
# "Robot velocity cmd sent: v_x=${},v_y=${},v_rot${}".format(v_x, v_y, v_rot))
return []
def test_synchro_se2_trajectory_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
goal_se2 = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
command = RobotCommandBuilder.synchro_se2_trajectory_command(goal_se2, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_se2_trajectory_command(goal_se2, frame,
build_on_command=arm_command)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
_test_has_arm(command.synchronized_command)
def __execute_velocity(self, description, v_x=0.0, v_y=0.0, v_rot=0.0):
self.__execute_command(
description,
RobotCommandBuilder.synchro_velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot),
time.time() + self.__VELOCITY_CMD_DURATION)
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0):
self._start_robot_command(desc,
RobotCommandBuilder.velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot,
body_height=self.height,
locomotion_hint=spot_command_pb2.HINT_AMBLE),
end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def _return_to_origin(self):
self._start_robot_command('fwd_and_rotate',
RobotCommandBuilder.trajectory_command(
goal_x=0.0, goal_y=0.0, goal_heading=0.0,
frame_name=ODOM_FRAME_NAME, params=None, body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_SPEED_SELECT_TROT),
end_time_secs=time.time() + 20)
def construct_hold_pose_task():
""" Helper function for holding the pose of the hand
Use this if you want to hold the position of the hand,
without leaving constrained manipulation.
Output:
+ command: api command object
"""
frame_name = "hand"
force_lim = 80
torque_lim = 10
force_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0)
torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
init_wrench_dir = geometry_pb2.Wrench(force=force_direction,
torque=torque_direction)
task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_HOLD_POSE
command = RobotCommandBuilder.constrained_manipulation_command(
task_type=task_type,
init_wrench_direction_in_frame_name=init_wrench_dir,
force_limit=force_lim,
torque_limit=torque_lim,
tangential_speed=0.0,
frame_name=frame_name)
return command
def self_right_cmd_srv(self, stand):
""" Callback that sends self-right cmd"""
cmd = RobotCommandBuilder.selfright_command()
ret = self.command_client.robot_command(cmd)
rospy.loginfo("Robot self right cmd sent. {}".format(ret))
return []
def trajectory_cmd_srv(self, trajectory):
'''
Callback that specifies waypoint(s) (Point) [m] with a final orientation [rad]
The name of the frame that trajectory is relative to.
The trajectory must be expressed in a gravity aligned frame, so either "vision", "odom", or "flat_body".
Any other provided se2_frame_name will be rejected and the trajectory command will not be exectuted.
'''
# TODO: Support other reference frames (currently only VISION ref. frame)
for pose in trajectory.waypoints.poses:
x = pose.position.x
y = pose.position.y
heading = math.atan2(y,x)
frame = VISION_FRAME_NAME
cmd = RobotCommandBuilder.trajectory_command(
goal_x=x,
goal_y=y,
goal_heading=heading,
frame_name=frame,
)
self.command_client.robot_command(lease=None, command=cmd, end_time_secs=time.time() + self.TRAJECTORY_CMD_TIMEOUT)
robot_state = self.get_robot_state()[0].vision_tform_body
final_pose = geometry_msgs.msg.Pose()
final_pose.position = robot_state.translation
final_pose.orientation = robot_state.rotation
spot_ros_srvs.srv.TrajectoryResponse(final_pose)
def power_off(self):
"""Power off the robot."""
safe_power_off_cmd = RobotCommandBuilder.safe_power_off_command()
self._robot_command_client.robot_command(lease=None,
command=safe_power_off_cmd)
time.sleep(2.5)
print("Powered Off " + str(not self._robot.is_powered_on()))
def pitch_up(robot):
"""Pitch robot up to look for door handle.
Args:
robot: (Robot) Interface to Spot robot.
"""
robot.logger.info("Pitching robot up...")
command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
footprint_R_body = geometry.EulerZXY(0.0, 0.0, -1 * math.pi / 6.0)
cmd = RobotCommandBuilder.synchro_stand_command(
footprint_R_body=footprint_R_body)
cmd_id = command_client.robot_command(cmd)
timeout_sec = 10.0
end_time = time.time() + timeout_sec
while time.time() < end_time:
response = command_client.robot_command_feedback(cmd_id)
synchronized_feedback = response.feedback.synchronized_feedback
status = synchronized_feedback.mobility_command_feedback.stand_feedback.status
if (status ==
basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING):
robot.logger.info("Robot pitched.")
return
time.sleep(1.0)
raise Exception("Failed to pitch robot.")
def go_to_tag(self, fiducial_rt_world):
"""Use the position of the april tag in vision world frame and command the robot."""
# Compute the go-to point (offset by .5m from the fiducial position) and the heading at
# this point.
self._current_tag_world_pose, self._angle_desired = self.offset_tag_pose(
fiducial_rt_world, self._tag_offset)
#Command the robot to go to the tag in kinematic odometry frame
mobility_params = self.set_mobility_params()
tag_cmd = RobotCommandBuilder.trajectory_command(
goal_x=self._current_tag_world_pose[0],
goal_y=self._current_tag_world_pose[1],
goal_heading=self._angle_desired,
frame_name=VISION_FRAME_NAME,
params=mobility_params,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO)
end_time = 5.0
if self._movement_on and self._powered_on:
#Issue the command to the robot
self._robot_command_client.robot_command(
lease=None,
command=tag_cmd,
end_time_secs=time.time() + end_time)
# #Feedback to check and wait until the robot is in the desired position or timeout
start_time = time.time()
current_time = time.time()
while (not self.final_state()
and current_time - start_time < end_time):
time.sleep(.25)
current_time = time.time()
return
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0):
self._start_robot_command(
desc,
RobotCommandBuilder.synchro_velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot),
end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def _on_quit(self):
"""Cleanup on quit from the command line interface."""
# Sit the robot down + power off after the navigation command is complete.
if self._powered_on and not self._started_powered_on:
self._robot_command_client.robot_command(
RobotCommandBuilder.safe_power_off_command(),
end_time_secs=time.time())
def _selfright(self):
"""Executes selfright command, which causes the robot to automatically turn if
it is on its back.
"""
cmd = RobotCommandBuilder.selfright_command()
self._issue_robot_command(cmd)
def get_go_to(world_tform_object,
robot_state,
mobility_params,
dist_margin=1.2):
"""Gets trajectory command to a goal location
Args:
world_tform_object (SE3Pose): Transform from vision frame to target object
robot_state (RobotState): Current robot state
mobility_params (MobilityParams): Mobility parameters
dist_margin (float): Distance margin to target
"""
vo_tform_robot = get_vision_tform_body(
robot_state.kinematic_state.transforms_snapshot)
delta_ewrt_vo = np.array([
world_tform_object.x - vo_tform_robot.x,
world_tform_object.y - vo_tform_robot.y, 0
])
norm = np.linalg.norm(delta_ewrt_vo)
if norm == 0:
return None
delta_ewrt_vo_norm = delta_ewrt_vo / norm
heading = _get_heading(delta_ewrt_vo_norm)
vo_tform_goal = np.array([
world_tform_object.x - delta_ewrt_vo_norm[0] * dist_margin,
world_tform_object.y - delta_ewrt_vo_norm[1] * dist_margin
])
tag_cmd = RobotCommandBuilder.trajectory_command(
goal_x=vo_tform_goal[0],
goal_y=vo_tform_goal[1],
goal_heading=heading,
frame_name=VISION_FRAME_NAME,
params=mobility_params)
return tag_cmd
def _battery_change_pose(self):
# Default HINT_RIGHT, maybe add option to choose direction?
self._start_robot_command(
'battery_change_pose',
RobotCommandBuilder.battery_change_pose_command(
dir_hint=basic_command_pb2.BatteryChangePoseCommand.Request.
HINT_RIGHT))
def stand(self, monitor_command=True):
"""If the e-stop is enabled, and the motor power is enabled, stand the robot up."""
response = self._robot_command(
RobotCommandBuilder.stand_command(params=self._mobility_params))
if monitor_command:
self._last_stand_command = response[2]
return response[0], response[1]
def get_walking_params(max_linear_vel, max_rotation_vel):
max_vel_linear = geometry_pb2.Vec2(x=max_linear_vel, y=max_linear_vel)
max_vel_se2 = geometry_pb2.SE2Velocity(linear=max_vel_linear,
angular=max_rotation_vel)
vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2)
params = RobotCommandBuilder.mobility_params()
params.vel_limit.CopyFrom(vel_limit)
return params
def test_synchro_se2_trajectory_point_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
command = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x, goal_y, goal_heading, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
def relative_move(dx,
dy,
dyaw,
frame_name,
robot_command_client,
robot_state_client,
stairs=False):
transforms = robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot
# Build the transform for where we want the robot to be relative to where the body currently is.
body_tform_goal = math_helpers.SE2Pose(x=dx, y=dy, angle=dyaw)
# We do not want to command this goal in body frame because the body will move, thus shifting
# our goal. Instead, we transform this offset to get the goal position in the output frame
# (which will be either odom or vision).
out_tform_body = get_se2_a_tform_b(transforms, frame_name, BODY_FRAME_NAME)
out_tform_goal = out_tform_body * body_tform_goal
# Command the robot to go to the goal point in the specified frame. The command will stop at the
# new position.
robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x=out_tform_goal.x,
goal_y=out_tform_goal.y,
goal_heading=out_tform_goal.angle,
frame_name=frame_name,
params=RobotCommandBuilder.mobility_params(stair_hint=stairs))
end_time = 10.0
cmd_id = robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() +
end_time)
# Wait until the robot has reached the goal.
while True:
feedback = robot_command_client.robot_command_feedback(cmd_id)
mobility_feedback = feedback.feedback.synchronized_feedback.mobility_command_feedback
if mobility_feedback.status != RobotCommandFeedbackStatus.STATUS_PROCESSING:
print("Failed to reach the goal")
return False
traj_feedback = mobility_feedback.se2_trajectory_feedback
if (traj_feedback.status == traj_feedback.STATUS_AT_GOAL
and traj_feedback.body_movement_status
== traj_feedback.BODY_STATUS_SETTLED):
print("Arrived at the goal.")
return True
time.sleep(1)
def _battery_change_pose(self):
"""Executes the battery-change pose command which causes the robot to sit down if
standing then roll to its [right]/left side for easier battery changing.
"""
cmd = RobotCommandBuilder.battery_change_pose_command(
dir_hint=basic_command_pb2.BatteryChangePoseCommand.Request.
HINT_RIGHT)
self._issue_robot_command(cmd)
def _selfright(self):
"""Executes selfright command, which causes the robot to automatically turn if
it is on its back.
"""
if not self.motors_powered:
return
cmd = RobotCommandBuilder.selfright_command()
self.command_client.robot_command_async(cmd)
def _force_safe_power_off(self):
"""Safely powers off the robot.
"""
if self.robot.is_powered_on():
lease = self.lease_client.take()
with LeaseKeepAlive(self.lease_client):
cmd = RobotCommandBuilder.safe_power_off_command()
self.command_client.robot_command(cmd)
self.lease_client.return_lease(lease)
