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 construct_right_handed_ballvalve_task(velocity_normalized,
force_limit=40,
torque_limit=5):
""" Helper function for manipulating right-handed ball valves
Use this when the hand is to the right of the pivot of the ball valve
And when hand x axis is roughly parallel to the axis of rotation of
the ball valve
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
+ torque_limit (optional): positive value denoting max torque robot will exert along
the axis of rotation of the task
Output:
+ command: api command object
Notes:
If the grasp is such that the hand x axis is not parallel to the axis
of rotation of the ball valve, then use the lever task.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
torque_lim = torque_limit
# Force/torque signs are opposite for right-handed ball valve
force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=1.0)
# The torque vector is provided as additional information denoting the
# axis of rotation of the task.
torque_direction = geometry_pb2.Vec3(x=-1.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_SE3_CIRCLE_FORCE_TORQUE
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=tangential_velocity,
frame_name=frame_name)
return command
def construct_lever_task(velocity_normalized, force_limit=40, torque_limit=5):
""" Helper function for manipulating levers
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
+ torque_limit (optional): positive value denoting max torque robot will exert along
the axis of rotation of the task
Output:
+ command: api command object
Notes:
In this function, we assume the initial motion of the lever is
along the z axis of the hand (up and down). If the initial
grasp is such that the initial motion needs to be something else,
change the force direction.
This function assumes we don't know the plane_normal (i.e. torque_direction)
of the lever. If we do know that, we can specify it as torque_direction
or use the ball valve task types, which assume a specific grasp and specify
what the initial torque_direction is.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
torque_lim = torque_limit
force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=1.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_SE3_CIRCLE_FORCE_TORQUE
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=tangential_velocity,
frame_name=frame_name)
return command
def construct_crank_task(velocity_normalized, force_limit=40):
""" Helper function for manipulating cranks with a free to rotate handle
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
Output:
+ command: api command object
Notes:
In this function, we assume the initial motion of the crank is
along the y axis of the hand (left and right). If the initial
grasp is such that the initial motion needs to be something else,
change the force direction.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure torque in this task.
torque_lim = 5.0
# This assumes the grasp of crank is such that the crank will initially
# move along the hand y axis. Change if that is not the case.
force_direction = geometry_pb2.Vec3(x=0.0, y=1.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_R3_CIRCLE_EXTRADOF_FORCE
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=tangential_velocity,
frame_name=frame_name)
return command
def construct_wheel_task(velocity_normalized, force_limit=40):
""" Helper function for turning wheels while grasping the rim
Use this when the wheel is grasped on the rim. If the grasp
is on a handle that is free to rotate, use the crank task type.
If the handle is not free to rotate, use this task type.
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
Output:
+ command: api command object
Notes:
This assumes initial motion will be along the y axis of the hand,
which is often the case. Change force_direction if that is not true.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure torque in this task.
torque_lim = 5.0
force_direction = geometry_pb2.Vec3(x=0.0, y=1.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_R3_CIRCLE_FORCE
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=tangential_velocity,
frame_name=frame_name)
return command
def construct_knob_task(velocity_normalized, torque_limit=5):
""" Helper function for turning purely rotational knobs
Use this for turning knobs/valves that do not have a lever arm
params:
+ velocity_normalized: normalized task rotational velocity in range [-1.0, 1.0]
+ torque_limit (optional): positive value denoting max torque robot will exert along axis of
rotation of the task
Output:
+ command: api command object
Notes:
This assumes that the axis of rotation of of the knob is roughly parallel
to the x axis of the hand. Change torque_direction if that is not the case.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
rot_velocity_limit = scale_rot_velocity_lim_given_torque_lim(torque_limit)
rotational_velocity = velocity_normalized * rot_velocity_limit
frame_name = "hand"
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure force in this task.
force_lim = 40.0
torque_lim = torque_limit
force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0)
torque_direction = geometry_pb2.Vec3(x=1.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_SE3_ROTATIONAL_TORQUE
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,
rotational_speed=rotational_velocity,
frame_name=frame_name)
return command
def construct_drawer_task(velocity_normalized, force_limit=40):
""" Helper function for opening/closing drawers
params:
+ velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0]
+ force_limit (optional): positive value denoting max force robot will exert along task dimension
Output:
+ command: api command object
Notes:
In this function, we assume the initial motion of the drawer is
along the x axis of the hand (forward and backward). If the initial
grasp is such that the initial motion needs to be something else,
change the force direction.
"""
velocity_normalized = max(min(velocity_normalized, 1.0), -1.0)
velocity_limit = scale_velocity_lim_given_force_lim(force_limit)
tangential_velocity = velocity_normalized * velocity_limit
frame_name = "hand"
force_lim = force_limit
# Setting a placeholder value that doesn't matter, since we don't
# apply a pure torque in this task.
torque_lim = 5.0
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_R3_LINEAR_FORCE
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=tangential_velocity,
frame_name=frame_name)
return command
def force_wrench(config):
"""Commanding a force / wrench with Spot's arm."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ForceTrajectoryClient')
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()
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.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
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(2.0)
# Unstow the arm
unstow = RobotCommandBuilder.arm_ready_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(unstow)
robot.logger.info("Unstow command issued.")
time.sleep(3.0)
# Demonstrate an example force trajectory by ramping up and down a vertical force over
# 10 seconds
f_x0 = 0 # Newtons
f_y0 = 0
f_z0 = 0
f_x1 = 0 # Newtons
f_y1 = 0
f_z1 = -10 # push down
# We won't have any rotational torques
torque_x = 0
torque_y = 0
torque_z = 0
# Duration in seconds.
traj_duration = 5
# First point on the trajectory
force0 = geometry_pb2.Vec3(x=f_x0, y=f_y0, z=f_z0)
torque0 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z)
wrench0 = geometry_pb2.Wrench(force=force0, torque=torque0)
t0 = seconds_to_duration(0)
traj_point0 = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench0, time_since_reference=t0)
# Second point on the trajectory
force1 = geometry_pb2.Vec3(x=f_x1, y=f_y1, z=f_z1)
torque1 = geometry_pb2.Vec3(x=torque_x, y=torque_y, z=torque_z)
wrench1 = geometry_pb2.Wrench(force=force1, torque=torque1)
t1 = seconds_to_duration(traj_duration)
traj_point1 = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench1, time_since_reference=t1)
# Build the trajectory
trajectory = trajectory_pb2.WrenchTrajectory(
points=[traj_point0, traj_point1])
# Build the full request, putting all axes into force mode.
arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request(
root_frame_name=ODOM_FRAME_NAME,
wrench_trajectory_in_task=trajectory,
x_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
y_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
z_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rx_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
ry_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rz_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_cartesian_command=arm_cartesian_command)
synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
robot_command = robot_command_pb2.RobotCommand(
synchronized_command=synchronized_command)
# Send the request
command_client.robot_command(robot_command)
robot.logger.info('Force trajectory command issued...')
time.sleep(5.0 + traj_duration)
# 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 force_wrench(config):
"""Commanding a force / wrench with Spot's arm."""
# See hello_spot.py for an explanation of these lines.
bosdyn.client.util.setup_logging(config.verbose)
sdk = bosdyn.client.create_standard_sdk('ForceWrenchClient')
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."
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
lease_client = robot.ensure_client(
bosdyn.client.lease.LeaseClient.default_service_name)
lease = lease_client.acquire()
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.
# The set of valid commands for a robot depends on hardware configuration. See
# SpotCommandHelper for more detailed examples on command building. The robot
# command service requires timesync between the robot and the client.
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.")
# Send a second stand command with a lowered body height to allow the arm to reach the ground.
stand_command = RobotCommandBuilder.synchro_stand_command(
body_height=-0.15)
command_id = command_client.robot_command(stand_command, timeout=2)
time.sleep(2.0)
# Unstow the arm
unstow = RobotCommandBuilder.arm_ready_command()
# Issue the command via the RobotCommandClient
command_client.robot_command(unstow)
robot.logger.info("Unstow command issued.")
time.sleep(3.0)
# Start in gravity-compensation mode (but zero force)
f_x = 0 # Newtons
f_y = 0
f_z = 0
# We won't have any rotational torques
torque_x = 0
torque_y = 0
torque_z = 0
# Duration in seconds.
seconds = 1000
# Use the helper function to build a single wrench
command = RobotCommandBuilder.arm_wrench_command(
f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME,
seconds)
# Send the request
command_client.robot_command(command)
robot.logger.info('Zero force commanded...')
time.sleep(5.0)
# Drive the arm into the ground with a specified force:
f_z = -5 # Newtons
command = RobotCommandBuilder.arm_wrench_command(
f_x, f_y, f_z, torque_x, torque_y, torque_z, BODY_FRAME_NAME,
seconds)
# Send the request
command_client.robot_command(command)
time.sleep(5.0)
# --------------- #
# Hybrid position-force mode and trajectories.
#
# We'll move the arm in an X/Y trajectory while maintaining some force on the ground.
# Hand will start to the left and move to the right.
hand_x = 0.75 # in front of the robot.
hand_y_start = 0.25 # to the left
hand_y_end = -0.25 # to the right
hand_z = 0 # will be ignored since we'll have a force in the Z axis.
f_z = -5 # Newtons
hand_vec3_start = geometry_pb2.Vec3(x=hand_x,
y=hand_y_start,
z=hand_z)
hand_vec3_end = geometry_pb2.Vec3(x=hand_x, y=hand_y_end, z=hand_z)
qw = 1
qx = 0
qy = 0
qz = 0
quat = geometry_pb2.Quaternion(w=qw, x=qx, y=qy, z=qz)
# Build a position trajectory
hand_pose1_in_flat_body = geometry_pb2.SE3Pose(
position=hand_vec3_start, rotation=quat)
hand_pose2_in_flat_body = geometry_pb2.SE3Pose(
position=hand_vec3_end, rotation=quat)
# Convert the poses to the odometry frame.
robot_state = robot_state_client.get_robot_state()
odom_T_flat_body = get_a_tform_b(
robot_state.kinematic_state.transforms_snapshot,
ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME)
hand_pose1 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose1_in_flat_body)
hand_pose2 = odom_T_flat_body * math_helpers.SE3Pose.from_obj(
hand_pose2_in_flat_body)
traj_time = 5.0
time_since_reference = seconds_to_duration(traj_time)
traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose1.to_proto(),
time_since_reference=seconds_to_duration(0))
traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
pose=hand_pose2.to_proto(),
time_since_reference=time_since_reference)
hand_traj = trajectory_pb2.SE3Trajectory(
points=[traj_point1, traj_point2])
# We'll use a constant wrench here. Build a wrench trajectory with a single point.
# Note that we only need to fill out Z-force because that is the only axis that will
# be in force mode.
force_tuple = odom_T_flat_body.rotation.transform_point(x=0,
y=0,
z=f_z)
force = geometry_pb2.Vec3(x=force_tuple[0],
y=force_tuple[1],
z=force_tuple[2])
torque = geometry_pb2.Vec3(x=0, y=0, z=0)
wrench = geometry_pb2.Wrench(force=force, torque=torque)
# We set this point to happen at 0 seconds. The robot will hold the wrench past that
# time, so we'll end up with a constant value.
duration = seconds_to_duration(0)
traj_point = trajectory_pb2.WrenchTrajectoryPoint(
wrench=wrench, time_since_reference=duration)
trajectory = trajectory_pb2.WrenchTrajectory(points=[traj_point])
arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request(
pose_trajectory_in_task=hand_traj,
root_frame_name=ODOM_FRAME_NAME,
wrench_trajectory_in_task=trajectory,
x_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
y_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
z_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_FORCE,
rx_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
ry_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION,
rz_axis=arm_command_pb2.ArmCartesianCommand.Request.
AXIS_MODE_POSITION)
arm_command = arm_command_pb2.ArmCommand.Request(
arm_cartesian_command=arm_cartesian_command)
synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request(
arm_command=arm_command)
robot_command = robot_command_pb2.RobotCommand(
synchronized_command=synchronized_command)
# Send the request
command_client.robot_command(robot_command)
robot.logger.info('Running mixed position and force mode.')
time.sleep(traj_time + 5.0)
# 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)