def set_default_body_control():
"""Set default body control params to current body position"""
footprint_R_body = geometry.EulerZXY()
position = geo.Vec3(x=0.0, y=0.0, z=0.0)
rotation = footprint_R_body.to_quaternion()
pose = geo.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
return spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
def bodyPoseCallback(self, data):
"""Callback for cmd_vel command"""
q = Quaternion()
q.x = data.orientation.x
q.y = data.orientation.y
q.z = data.orientation.z
q.w = data.orientation.w
position = geometry_pb2.Vec3(z=data.position.z)
pose = geometry_pb2.SE3Pose(position=position, rotation=q)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.body_control.CopyFrom(body_control)
self.spot_wrapper.set_mobility_params(mobility_params)
def mobility_params(body_height=0.0, footprint_R_body=geometry.EulerZXY(),
locomotion_hint=spot_command_pb2.HINT_AUTO, stair_hint=False,
external_force_params=None):
"""Helper to create Mobility params for spot mobility commands. This function is designed
to help get started issuing commands, but lots of options are not exposed via this
interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be
chosen by the robot.
Returns:
spot.MobilityParams, params for spot mobility commands.
"""
# Simplified body control params
position = geometry_pb2.Vec3(z=body_height)
rotation = footprint_R_body.to_quaternion()
pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
frame = geometry_pb2.Frame(base_frame=geometry_pb2.FRAME_BODY)
traj = trajectory_pb2.SE3Trajectory(points=[point], frame=frame)
body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
return spot_command_pb2.MobilityParams(body_control=body_control,
locomotion_hint=locomotion_hint,
stair_hint=stair_hint,
external_force_params=external_force_params)
def mobility_params(body_height=0.0,
footprint_R_body=geometry.EulerZXY(),
locomotion_hint=spot_command_pb2.HINT_AUTO,
stair_hint=False,
external_force_params=None):
"""Helper to create Mobility params for spot mobility commands. This function is designed
to help get started issuing commands, but lots of options are not exposed via this
interface. See spot.robot_command_pb2 for more details. If unset, good defaults will be
chosen by the robot.
Args:
body_height: Body height in meters.
footprint_R_body(EulerZXY): The orientation of the body frame with respect to the
footprint frame (gravity aligned framed with yaw computed
from the stance feet)
locomotion_hint: Locomotion hint to use for the command.
stair_hint: Boolean to specify if stair mode should be used.
external_force_params(spot.BodyExternalForceParams): Robot body external force
parameters.
Returns:
spot.MobilityParams, params for spot mobility commands.
"""
# Simplified body control params
position = geometry_pb2.Vec3(z=body_height)
rotation = footprint_R_body.to_quaternion()
pose = geometry_pb2.SE3Pose(position=position, rotation=rotation)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
body_control = spot_command_pb2.BodyControlParams(
base_offset_rt_footprint=traj)
return spot_command_pb2.MobilityParams(
body_control=body_control,
locomotion_hint=locomotion_hint,
stair_hint=stair_hint,
external_force_params=external_force_params)