def update_pose(self, twist):
transform = Vector3(twist.linear.x * 100, twist.linear.y * 100,
twist.linear.z * 100)
rotation = Vector3(math.degrees(twist.angular.x),
math.degrees(twist.angular.y),
math.degrees(twist.angular.z))
self.controller.set_relaxed_pose(transform, rotation)
def move_body_relative(self, request):
current_positions = self.gait_engine.get_current_leg_positions()
corrected_rotation = Vector3.ros_vector3_to_overload_vector(
request.rotation).rad_to_degree()
relative_vector_overload = Vector3.ros_vector3_to_overload_vector(
request.translation) * 100.0
desired_position = (
current_positions -
relative_vector_overload).rotate(corrected_rotation)
task_finished_event = Event()
self.motion_queue.put((task_finished_event, desired_position))
task_finished_event.wait()
return True
def move_legs(self, move_legs_cmd):
local_frame = "base_link"
command_frame = move_legs_cmd.header.frame_id
ros_transform = self.tf_buffer.lookup_transform(
local_frame, command_frame, rospy.Time()).transform
frame_translation_ros, frame_rotation_ros = ros_transform.translation, ros_transform.rotation
frame_rotation = Quaternion(frame_rotation_ros.w, frame_rotation_ros.x,
frame_rotation_ros.y, frame_rotation_ros.z)
frame_translation = Vector3.ros_vector3_to_overload_vector(
frame_translation_ros)
move_legs_overloaded = LegPositions.ros_leg_positions_to_leg_positions(
move_legs_cmd.leg_positions)
new_positions = LegPositions(
(move_legs_overloaded.left_front * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.right_front * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.left_middle * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.right_middle * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.left_rear * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.right_rear * frame_rotation +
frame_translation) * 100.0)
current_positions = self.gait_engine.get_current_leg_positions()
desired_position = current_positions.update_from_other(
new_positions, LegFlags(move_legs_cmd.selected_legs))
task_finished_event = Event()
self.motion_queue.put((task_finished_event, desired_position))
# debug marker
# self.display_marker(desired_position.left_front.x / 100, desired_position.left_front.y / 100, desired_position.left_front.z / 100)
task_finished_event.wait()
return True
def get_transform_for_link(self, from_frame_id, to_frame_id):
ros_transform = self.tf_buffer.lookup_transform(
from_frame_id, to_frame_id, rospy.Time()).transform
frame_translation_ros, frame_rotation_ros = ros_transform.translation, ros_transform.rotation
frame_rotation = Quaternion(frame_rotation_ros.w, frame_rotation_ros.x,
frame_rotation_ros.y, frame_rotation_ros.z)
frame_translation = Vector3.ros_vector3_to_overload_vector(
frame_translation_ros)
return frame_translation, frame_rotation
def move_body(self, move_legs_cmd):
local_frame = "base_link"
command_frame = move_legs_cmd.header.frame_id
ros_transform = self.tf_buffer.lookup_transform(
local_frame, command_frame, rospy.Time()).transform
frame_translation_ros, frame_rotation_ros = ros_transform.translation, ros_transform.rotation
frame_rotation = Quaternion(frame_rotation_ros.w, frame_rotation_ros.x,
frame_rotation_ros.y, frame_rotation_ros.z)
frame_translation = Vector3.ros_vector3_to_overload_vector(
frame_translation_ros)
move_vector_overload = (-Vector3.ros_vector3_to_overload_vector(
move_legs_cmd.core_movement) * frame_rotation +
frame_translation) * 100.0
current_positions = self.gait_engine.get_current_leg_positions()
new_positions = current_positions.transform(
move_vector_overload, LegFlags(move_legs_cmd.used_legs))
task_finished_event = Event()
self.motion_queue.put((task_finished_event, new_positions))
task_finished_event.wait()
return True
def update_single_leg(self, msg):
position = Vector3(msg.position.x * 100, msg.position.y * 100,
msg.position.z * 100)
self.controller.update_single_leg_command(msg.selected_leg, position,
msg.single_leg_mode_on,
msg.fast_mode)
def position_for_foot(relative_vector, current_position):
relative_vector_overload = Vector3.ros_vector3_to_overload_vector(
relative_vector) * 100.0
return (current_position + relative_vector_overload)
def move_until_hit(self, move_legs_cmd):
local_frame = "base_link"
command_frame = move_legs_cmd.header.frame_id
ros_transform = self.tf_buffer.lookup_transform(
local_frame, command_frame, rospy.Time()).transform
frame_translation_ros, frame_rotation_ros = ros_transform.translation, ros_transform.rotation
frame_rotation = Quaternion(frame_rotation_ros.w, frame_rotation_ros.x,
frame_rotation_ros.y, frame_rotation_ros.z)
frame_translation = Vector3.ros_vector3_to_overload_vector(
frame_translation_ros)
move_legs_overloaded = LegPositions.ros_leg_positions_to_leg_positions(
move_legs_cmd.leg_positions)
new_positions = LegPositions(
(move_legs_overloaded.left_front * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.right_front * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.left_middle * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.right_middle * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.left_rear * frame_rotation +
frame_translation) * 100.0,
(move_legs_overloaded.right_rear * frame_rotation +
frame_translation) * 100.0)
current_positions = self.gait_engine.get_current_leg_positions()
desired_position = current_positions.update_from_other(
new_positions, LegFlags(move_legs_cmd.selected_legs))
# move check loop
move_done = False
move_dist = 0.5 # distance to move with each step in cm
colliding_legs = LegFlags.NONE
midstep_positions = current_positions.clone()
while not move_done:
still_moving = False
if not self.last_feet_msg.left_front:
still_moving = still_moving or midstep_positions.left_front.move_towards_at_speed(
desired_position.left_front, move_dist)
else:
colliding_legs |= LegFlags.LEFT_FRONT
if not self.last_feet_msg.right_front:
still_moving = still_moving or midstep_positions.right_front.move_towards_at_speed(
desired_position.right_front, move_dist)
else:
colliding_legs |= LegFlags.RIGHT_FRONT
if not self.last_feet_msg.left_middle:
still_moving = still_moving or midstep_positions.left_middle.move_towards_at_speed(
desired_position.left_middle, move_dist)
else:
colliding_legs |= LegFlags.LEFT_MIDDLE
if not self.last_feet_msg.right_middle:
still_moving = still_moving or midstep_positions.right_middle.move_towards_at_speed(
desired_position.right_middle, move_dist)
else:
colliding_legs |= LegFlags.RIGHT_MIDDLE
if not self.last_feet_msg.left_rear:
still_moving = still_moving or midstep_positions.left_rear.move_towards_at_speed(
desired_position.left_rear, move_dist)
else:
colliding_legs |= LegFlags.LEFT_REAR
if not self.last_feet_msg.right_rear:
still_moving = still_moving or midstep_positions.right_rear.move_towards_at_speed(
desired_position.right_rear, move_dist)
else:
colliding_legs |= LegFlags.RIGHT_REAR
if still_moving:
task_finished_event = Event()
self.motion_queue.put((task_finished_event, midstep_positions))
task_finished_event.wait()
else:
move_done = True
return int(colliding_legs), midstep_positions