def handle_max_vel(self, req):
"""
Handle a max_velocity service call. This will modify the mobility params to have a limit on the maximum
velocity that the robot can move during motion commmands. This affects trajectory commands and velocity
commands
Args:
req: SetVelocityRequest containing requested maximum velocity
Returns: SetVelocityResponse
"""
try:
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.vel_limit.CopyFrom(SE2VelocityLimit(max_vel=math_helpers.SE2Velocity(req.velocity_limit.linear.x,
req.velocity_limit.linear.y,
req.velocity_limit.angular.z).to_proto()))
self.spot_wrapper.set_mobility_params(mobility_params)
return SetVelocityResponse(True, 'Success')
except Exception as e:
return SetVelocityResponse(False, 'Error:{}'.format(e))
def test_express_velocity_types():
snapshot_text = """
child_to_parent_edge_map {
key: "vision"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 1
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "body"
value: {
parent_frame_name: ""
}
}
"""
frame_tree = _create_snapshot(snapshot_text)
assert frame_helpers.validate_frame_tree_snapshot(frame_tree)
test_vel1 = math_helpers.SE3Velocity(1.1, 2.2, 3.3, 4.4, 5.5, 6.6)
assert type(test_vel1.linear_velocity_x) == float
assert test_vel1.linear_velocity_x == 1.1
assert test_vel1.linear.x == 1.1
test_vel2 = math_helpers.SE3Velocity(1.1, 2.2, 3.3, 4.4, 5.5, 6.6)
test_vel2_proto = test_vel2.to_proto()
body_vel = frame_helpers.express_se3_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel2)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 56.1
assert body_vel.linear.x == 56.1
body_vel = frame_helpers.express_se3_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel2_proto)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 56.1
assert body_vel.linear.x == 56.1
test_vel3 = math_helpers.SE2Velocity(1.1, 2.2, 3.3)
test_vel3_proto = test_vel3.to_proto()
body_vel = frame_helpers.express_se2_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel3)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 1.1
assert body_vel.linear.x == 1.1
body_vel = frame_helpers.express_se2_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel3_proto)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 1.1
assert body_vel.linear.x == 1.1
def test_express_velocity_new_frame():
snapshot_text = """
child_to_parent_edge_map {
key: "vision"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 1
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "odom"
value: {
parent_frame_name: "vision"
parent_tform_child: {
position: {
x: 2
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "special"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 3
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "fiducial_404"
value: {
parent_frame_name: "vision"
parent_tform_child: {
position: {
x: 4
z: 0
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "body"
value: {
parent_frame_name: ""
}
}
"""
frame_tree = _create_snapshot(snapshot_text)
assert frame_helpers.validate_frame_tree_snapshot(frame_tree)
# Transform SE(2) velocity
vel_of_body_in_vision = math_helpers.SE2Velocity(1, 1, 2)
vel_of_body_in_odom = frame_helpers.express_se2_velocity_in_new_frame(
frame_tree, "vision", "odom", vel_of_body_in_vision)
assert vel_of_body_in_odom is not None
assert type(vel_of_body_in_vision) == math_helpers.SE2Velocity
assert math.fabs(vel_of_body_in_odom.angular - 2) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.x - 1) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.y - 5) < 1e-6
# Transform SE(3) velocity
vel_of_body_in_vision = math_helpers.SE3Velocity(1, 2, 3, 1, 2, 3)
vel_of_body_in_odom = frame_helpers.express_se3_velocity_in_new_frame(
frame_tree, "vision", "odom", vel_of_body_in_vision)
assert vel_of_body_in_odom is not None
assert type(vel_of_body_in_vision) == math_helpers.SE3Velocity
assert math.fabs(vel_of_body_in_odom.angular.x - 1) < 1e-6
assert math.fabs(vel_of_body_in_odom.angular.y - 2) < 1e-6
assert math.fabs(vel_of_body_in_odom.angular.z - 3) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.x - 21) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.y - (-2)) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.z - (-1)) < 1e-6