def test_get_interpolated_setpoints_invalid_time_too_high(self):
setpoint = self.joint_trajectory.get_interpolated_setpoint(
self.duration + Duration(seconds=1))
self.assertEqual(
setpoint,
Setpoint(self.duration + Duration(seconds=1),
self.setpoints[-1].position, 0),
)
def setUp(self):
self.joint_name = "test_joint"
self.limits = Limits(-1, 1, 2)
self.duration = Duration(seconds=2.0)
self.times = [Duration(), self.duration / 2.0, self.duration]
self.setpoints = [
Setpoint(t, 2 * t.seconds, t.seconds / 2.0) for t in self.times
]
self.joint_trajectory = JointTrajectory(self.joint_name, self.limits,
self.setpoints, self.duration)
def _parse_duration_parameter(self, name: str) -> Duration:
"""Get a duration parameter from the parameter server.
Returns 0 if the parameter does not exist.
Clamps the duration to 0 if it is negative.
"""
if self.has_parameter(name):
value = self.get_parameter(name).value
if value < 0:
value = 0
return Duration(seconds=value)
else:
return Duration(0)
def test_interpolate_trajectories_correct_duration(self):
parameter = 0.5
other_duration = self.duration + Duration(seconds=1)
other_times = [Duration(), other_duration / 2.0, other_duration]
other_setpoints = [
Setpoint(t, 2 * t.seconds, t.seconds / 2.0) for t in other_times
]
other_trajectory = JointTrajectory(self.joint_name, self.limits,
other_setpoints, other_duration)
new_trajectory = JointTrajectory.interpolate_joint_trajectories(
self.joint_trajectory, other_trajectory, parameter)
self.assertEqual(
new_trajectory.duration,
self.duration.weighted_average(other_duration, parameter),
)
def test_interpolation_correct(self):
time, position, velocity = 1, 1, 1
parameter = 0.3
other_setpoint = Setpoint(Duration(seconds=time), position, velocity)
expected_result = Setpoint(
Duration(
seconds=self.setpoint.time.seconds * (1 - parameter) + parameter * time
),
self.setpoint.position * (1 - parameter) + parameter * position,
self.setpoint.velocity * (1 - parameter) + parameter * velocity,
)
self.assertEqual(
expected_result,
Setpoint.interpolate_setpoints(self.setpoint, other_setpoint, parameter),
)
def __init__(self,
node: Node,
gait_name: str = "balanced_walk",
default_walk: Gait = None):
self.gait_name = gait_name
self._node = node
self._default_walk = default_walk
self._constructing = False
self.logger = Logger(self._node, __class__.__name__)
self._current_subgait = None
self._current_subgait_duration = Duration(0)
self._start_time = None
self._end_time = None
self._current_time = None
self.capture_point_event = Event()
self.capture_point_result = None
self._capture_point_service = {
"left_leg":
node.create_client(srv_name="/march/capture_point/foot_left",
srv_type=CapturePointPose),
"right_leg":
node.create_client(srv_name="/march/capture_point/foot_right",
srv_type=CapturePointPose),
}
self.moveit_event = Event()
self.moveit_trajectory_result = None
self._moveit_trajectory_service = node.create_client(
srv_name="/march/moveit/get_trajectory",
srv_type=GetMoveItTrajectory)
def start(
self,
current_time: Time,
first_subgait_delay: Optional[Duration] = ZERO_DURATION,
) -> GaitUpdate:
"""Start the gait.
Creates a trajectory command to go towards the idle position in the given duration.
:returns Returns a GaitUpdate that usually contains a TrajectoryCommand.
"""
if first_subgait_delay is not None and first_subgait_delay > Duration(
0):
self._start_time = current_time + first_subgait_delay
self._end_time = self._start_time + self._duration
self._scheduled_early = True
return GaitUpdate.should_schedule_early(
TrajectoryCommand(
self._get_trajectory_msg(),
self._duration,
self._name,
self._start_time,
))
else:
self._start_time = current_time
self._end_time = self._start_time + self._duration
return GaitUpdate.should_schedule(
TrajectoryCommand(
self._get_trajectory_msg(),
self._duration,
self._name,
self._start_time,
))
def schedule(self, command: TrajectoryCommand):
"""Schedules a new trajectory.
:param TrajectoryCommand command: The trajectory command to schedule
"""
self._failed = False
stamp = command.start_time.to_msg()
command.trajectory.header.stamp = stamp
goal = FollowJointTrajectoryGoal(trajectory=command.trajectory)
self._trajectory_goal_pub.publish(
FollowJointTrajectoryActionGoal(
header=Header(stamp=stamp),
goal_id=GoalID(stamp=stamp, id=str(command)),
goal=goal,
)
)
info_log_message = f"Scheduling {command.name}"
debug_log_message = f"Subgait {command.name} starts "
if self._node.get_clock().now() < command.start_time:
time_difference = Duration.from_ros_duration(
command.start_time - self._node.get_clock().now()
)
debug_log_message += f"in {round(time_difference.seconds, 3)}s"
else:
debug_log_message += "now"
self._goals.append(command)
self.logger.info(info_log_message)
self.logger.debug(debug_log_message)
def __init__(self, gait_name, subgaits, graph):
super(SemiDynamicSetpointsGait, self).__init__(f"dynamic_{gait_name}",
subgaits, graph)
self._should_freeze = False
self._is_frozen = False
self._freeze_duration = Duration(0)
self._freeze_position = None
def get_joint_states_from_foot_state(foot_state: Foot,
time: float) -> dict:
"""Translate between feet_state and a list of setpoints.
:param foot_state:
A fully populated Foot object.
:param time:
The time of the Foot state and resulting setpoints.
:return:
A dictionary of setpoints, the foot location and velocity of
which corresponds with the feet_state.
"""
joint_states = Foot.calculate_joint_angles_from_foot_position(
foot_state.position, foot_state.foot_side, time)
# Find the joint angles a moment later using the foot position a
# moment later use this together with the current joint angles to
# calculate the joint velocity
next_position = Foot.calculate_next_foot_position(foot_state)
next_joint_positions = Foot.calculate_joint_angles_from_foot_position(
next_position.position,
next_position.foot_side,
time + Duration(seconds=VELOCITY_SCALE_FACTOR),
)
for joint in JOINT_NAMES_IK:
if joint in joint_states and joint in next_joint_positions:
joint_states[joint].add_joint_velocity_from_next_angle(
next_joint_positions[joint])
return joint_states
def start(
self,
current_time: Time,
first_subgait_delay: Optional[
Duration] = DEFAULT_FIRST_SUBGAIT_START_DELAY,
) -> GaitUpdate:
"""Start the gait.
Sets current subgait to the first subgait, resets the
time and generates the first trajectory command.
May optionally delay the first subgait.
:param first_subgait_delay Optional duration to delay the first subgait by.
:return: A TrajectoryCommand message with the trajectory of the first subgait.
"""
self._reset()
self._current_time = current_time
self._current_subgait = self.subgaits[self.graph.start_subgaits()[0]]
self._next_subgait = self._current_subgait
# Delay first subgait if duration is greater than zero
if first_subgait_delay > Duration(0):
self._start_is_delayed = True
self._update_time_stamps(self._current_subgait,
first_subgait_delay)
return GaitUpdate.should_schedule_early(
self._command_from_current_subgait())
else:
self._start_is_delayed = False
self._update_time_stamps(self._current_subgait)
return GaitUpdate.should_schedule(
self._command_from_current_subgait())
def test_update_subgait_start_delayed_did_start(self):
self.gait.start(Time(seconds=0), Duration(seconds=3))
gait_update = self.gait.update(Time(seconds=3.5))
self.assertEqual(self.gait.subgait_name, "right_open")
self.assertEqual(self.gait._start_time, Time(seconds=3))
self.assertEqual(self.gait._end_time, Time(seconds=4.5))
self.assertFalse(self.gait._start_is_delayed)
self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def test_update_subgait_schedule_early(self):
self.gait.start(Time(seconds=0))
gait_update = self.gait.update(Time(seconds=1), Duration(seconds=0.8))
self.assertEqual(self.gait.subgait_name, "right_open")
self.assertEqual(self.gait._start_time, Time(seconds=0))
self.assertEqual(self.gait._end_time, Time(seconds=1.5))
self.assertTrue(self.gait._scheduled_early)
self.assertEqual(
gait_update,
GaitUpdate.should_schedule_early(
TrajectoryCommand.from_subgait(
self.gait.subgaits["left_swing"], Time(seconds=1.5))),
)
gait_update = self.gait.update(Time(seconds=1.6),
Duration(seconds=0.8))
self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def from_dict(
cls,
robot: urdf.Robot,
subgait_dict: dict,
gait_name: str,
subgait_name: str,
version: str,
):
"""
List parameters from the yaml file in organized lists.
:param robot:
The robot corresponding to the given sub-gait file
:param subgait_dict:
The dictionary extracted from the yaml file
:param gait_name:
The name of the parent gait
:param subgait_name:
The name of the child (sub)gait
:param version:
The version of the yaml file
:returns
A populated Subgait object
"""
if robot is None:
raise GaitError("Cannot create gait without a loaded robot.")
duration = Duration(nanoseconds=subgait_dict["duration"])
joint_list = []
for name, points in sorted(subgait_dict["joints"].items(),
key=lambda item: item[0]):
urdf_joint = cls.joint_class.get_joint_from_urdf(robot, name)
if urdf_joint is None or urdf_joint.type == "fixed":
continue
limits = Limits.from_urdf_joint(urdf_joint)
joint_list.append(
cls.joint_class.from_setpoint_dict(name, limits, points,
duration))
subgait_type = (subgait_dict["gait_type"]
if subgait_dict.get("gait_type") else "")
subgait_description = (subgait_dict["description"]
if subgait_dict.get("description") else "")
return cls(
joint_list,
duration,
subgait_type,
gait_name,
subgait_name,
version,
subgait_description,
robot,
)
def from_begin_point(self, begin_time: Duration, position: float) -> None:
"""
Manipulate the gait to start at given time.
Removes all set points after the given begin time. Adds the begin position
with 0 velocity at the start. It also adds 1 second at the beginning
to allow speeding up to the required speed again.
:param begin_time: The time to start
:param position: The position to start from
"""
begin_time -= Duration(seconds=1)
for setpoint in reversed(self.setpoints):
if setpoint.time <= begin_time:
self.setpoints.remove(setpoint)
else:
setpoint.time -= begin_time
self.setpoints = [
Setpoint(time=Duration(), position=position, velocity=0)
] + self.setpoints
self._duration = self._duration - begin_time
def setUp(self):
self.setpoint = Setpoint(
Duration(seconds=1.123412541), 0.034341255, 123.162084549
) # 0.0343412512 123.162084
self.setpoint_dict = {
"left_hip_aa": self.setpoint,
"left_hip_fe": self.setpoint,
"left_knee": self.setpoint,
"right_hip_aa": self.setpoint,
"right_hip_fe": self.setpoint,
"right_knee": self.setpoint,
}
def test_valid_boundary_points_nonzero_start_end_zero_speed(self):
# First setpoint at t = 0.5 and last setpoint at t = 1.5 =/= duration have zero speed.
setpoints = [
Setpoint(
t * 0.5 + Duration(seconds=0.5),
(self.duration - t).seconds,
t.seconds * 2 - t.seconds**2,
) for t in self.times
]
joint_trajectory = JointTrajectory(self.joint_name, self.limits,
setpoints, self.duration)
self.assertTrue(joint_trajectory._validate_boundary_points())
def _get_extra_ankle_setpoint(self) -> Setpoint:
"""Returns an extra setpoint for the swing leg ankle
that can be used to create a push off.
:returns: An extra setpoint for the swing leg ankle
:rtype: Setpoint
"""
return Setpoint(
Duration(self.time[SetpointTime.PUSH_OFF_INDEX]),
self.push_off_position,
0.0,
)
def test_invalid_boundary_points_nonzero_start_nonzero_speed(self):
# First setpoint at t = 1 has nonzero speed.
setpoints = [
Setpoint(
t * 0.5 + Duration(seconds=1),
(self.duration - t).seconds,
(self.duration - t).seconds * 2,
) for t in self.times
]
joint_trajectory = JointTrajectory(self.joint_name, self.limits,
setpoints, self.duration)
self.assertFalse(joint_trajectory._validate_boundary_points())
def test_weighted_average_states(self):
parameter = 0.8
base_right_foot = Foot(Side.right, Vector3d(0, 0, 0), Vector3d(0, 0, 0))
base_left_foot = Foot(Side.left, Vector3d(0, 0, 0), Vector3d(0, 0, 0))
base_state = FeetState(base_right_foot, base_left_foot, Duration(seconds=0.1))
other_right_foot = Foot(Side.right, Vector3d(1, 1, 1), Vector3d(1, 1, 1))
other_left_foot = Foot(Side.left, Vector3d(1, 1, 1), Vector3d(1, 1, 1))
other_state = FeetState(
other_right_foot, other_left_foot, Duration(seconds=0.1)
)
resulting_state = FeetState.weighted_average_states(
base_state, other_state, parameter
)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.left_foot.position)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.left_foot.velocity)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.right_foot.position)
self.assertEqual(Vector3d(0.8, 0.8, 0.8), resulting_state.right_foot.velocity)
def test_find_known_position_forward(self):
setpoint_dict = {
"left_ankle": Setpoint(Duration(), 0, 0),
"left_hip_aa": Setpoint(Duration(), 0, 0),
"left_hip_fe": Setpoint(Duration(), pi / 2, 0),
"left_knee": Setpoint(Duration(), 0, 0),
"right_ankle": Setpoint(Duration(), 0, 0),
"right_hip_aa": Setpoint(Duration(), 0, 0),
"right_hip_fe": Setpoint(Duration(), pi / 2, 0),
"right_knee": Setpoint(Duration(), 0, 0),
}
[
l_ul,
l_ll,
l_hl,
l_ph,
r_ul,
r_ll,
r_hl,
r_ph,
base,
] = get_lengths_robot_for_inverse_kinematics(Side.both)
resulting_state = FeetState.from_setpoint_dict(setpoint_dict)
expected_right_foot = Foot(
Side.right,
Vector3d(r_ul + r_ll + r_hl, base / 2.0 + r_ph, 0),
Vector3d(0, 0, 0),
)
expected_left_foot = Foot(
Side.right,
Vector3d(l_ul + l_ll + l_hl, -base / 2.0 - l_ph, 0),
Vector3d(0, 0, 0),
)
expected_state = FeetState(expected_right_foot, expected_left_foot, Duration())
self.assertTrue(
Vector3d.is_close_enough(
expected_state.left_foot.position, resulting_state.left_foot.position
)
)
self.assertTrue(
Vector3d.is_close_enough(
expected_state.right_foot.position, resulting_state.right_foot.position
)
)
def _joint_dict_to_setpoint_dict(joint_dict: dict) -> dict:
"""Creates a setpoint_dict from a joint_dict.
:param joint_dict: A dictionary containing joint names and positions.
:type: dict
:returns: A dictionary containing joint names and setpoints.
:rtype: dict
"""
setpoint_dict = {}
for name, position in joint_dict.items():
setpoint_dict[name] = Setpoint(Duration(0), position, 0)
return setpoint_dict
def test_start_delayed(self):
gait_update = self.gait.start(Time(seconds=0), Duration(seconds=1))
self.assertEqual(self.gait.subgait_name, "right_open")
self.assertEqual(self.gait._start_time, Time(seconds=1))
self.assertEqual(self.gait._end_time, Time(seconds=2.5))
self.assertTrue(self.gait._start_is_delayed)
self.assertEqual(
gait_update,
GaitUpdate.should_schedule_early(
self.gait._command_from_current_subgait()),
)
def test_inverse_kinematics_reversed_position(self):
right_foot = Foot(Side.right, Vector3d(0.18, 0.08, 0.6), Vector3d(0, 0, 0))
left_foot = Foot(Side.left, Vector3d(0.18, -0.08, 0.6), Vector3d(0, 0, 0))
desired_state = FeetState(right_foot, left_foot, Duration(seconds=0.1))
new_setpoints = FeetState.feet_state_to_setpoints(desired_state)
resulting_position = FeetState.from_setpoint_dict(new_setpoints)
dif_left = (
desired_state.left_foot.position - resulting_position.left_foot.position
)
dif_right = (
desired_state.right_foot.position - resulting_position.right_foot.position
)
self.assertLess(dif_left.norm(), 0.0001)
self.assertLess(dif_right.norm(), 1 / 0.0001)
def _current_gait_cb(self, gait_name, subgait_name, version,
duration: Duration, gait_type):
"""Standard callback when gait changes, publishes the current gait
More callbacke can be added using add_gait_callback"""
self.logger.debug(f"Current subgait updated to {subgait_name}")
self.current_gait_pub.publish(
CurrentGait(
header=Header(
stamp=self._gait_selection.get_clock().now().to_msg()),
gait=gait_name,
subgait=subgait_name,
version=version,
duration=duration.to_msg(),
gait_type=gait_type,
))
def _new_trajectory_command(self) -> TrajectoryCommand:
"""Update the trajectory values and generate a new trajectory command.
:return Return a TrajectoryCommand for the next subgait.
"""
trajectory = self.get_joint_trajectory_msg(self._current_subgait)
time_from_start = trajectory.points[-1].time_from_start
self._current_subgait_duration = Duration.from_msg(time_from_start)
self._start_time = self._current_time
self._end_time = self._start_time + self._current_subgait_duration
return TrajectoryCommand(
trajectory,
self._current_subgait_duration,
self.subgait_name,
self._start_time,
)
def get_interpolated_setpoint(self, time: Duration) -> Setpoint:
"""Get a setpoint on a certain time.
If there is no setpoint at this time point, it will interpolate
between the setpoints.
"""
if time < Duration(seconds=0):
return self.setpoint_class(time, self.setpoints[0].position, 0)
if time > self.duration:
return self.setpoint_class(time, self.setpoints[-1].position, 0)
return self.setpoint_class(
time,
float(self.interpolated_position(time.seconds)),
float(self.interpolated_velocity(time.seconds)),
)
def from_setpoint_dict(cls, name: str, limits: Limits,
setpoint_dict: List[dict],
duration: Duration) -> JointTrajectory:
"""Creates a list of joint trajectories.
:param str name: Name of the joint
:param limits: Joint limits from the URDF
:param list(dict) setpoints: A list of setpoints from the subgait configuration
:param duration: The total duration of the trajectory
"""
setpoints = [
Setpoint(
time=Duration(nanoseconds=setpoint["time_from_start"]),
position=setpoint["position"],
velocity=setpoint["velocity"],
) for setpoint in setpoint_dict
]
return cls(name, limits, setpoints, duration)
def calculate_next_positions_joint(cls, setpoint_dic: dict) -> dict:
"""
Calculate the position of the joints a moment later.
Calculates using the approximation:
next_position = position + current_velocity * time_difference
:param setpoint_dic: A dictionary of setpoints with positions and velocities
:return: A dictionary with the positions of the joints 1 / VELOCITY_SCALE_FACTOR
seconds later
"""
next_positions = {}
for joint in setpoint_dic.keys():
next_positions[joint] = cls(
setpoint_dic[joint].time +
Duration(seconds=VELOCITY_SCALE_FACTOR),
setpoint_dic[joint].position +
setpoint_dic[joint].velocity * VELOCITY_SCALE_FACTOR,
)
return next_positions
def _get_trajectory_command(self, stop=False) -> TrajectoryCommand:
"""Return a TrajectoryCommand based on current subgait_id
:return: TrajectoryCommand with the current subgait and start time.
:rtype: TrajectoryCommand
"""
if self._start_is_delayed:
self._end_time = self._start_time
if stop:
self._end = True
self.logger.info("Stopping dynamic gait.")
else:
self.foot_location = self._get_foot_location(self.subgait_id)
stop = self._check_msg_time(self.foot_location)
self._publish_foot_location(self.subgait_id, self.foot_location)
self.logger.info(
f"Stepping to location ({self.foot_location.point.x}, {self.foot_location.point.y})"
)
self.dynamic_subgait = DynamicSubgait(
self.gait_selection,
self.start_position,
self.subgait_id,
self.joint_names,
self.foot_location.point,
self.joint_soft_limits,
stop,
)
trajectory = self.dynamic_subgait.get_joint_trajectory_msg()
return TrajectoryCommand(
trajectory,
Duration(self.dynamic_subgait_duration),
self.subgait_id,
self._end_time,
)
