def weighted_average_states(cls, base_state, other_state, parameter):
"""Computes the weighted average of two feet states.
:param base_state: One of the states for the weighted average, return this if
parameter is 0
:param other_state: One of the states for the weighted average, return this if
parameter is 1
:param parameter: The normalised weight parameter, the parameter that determines
the weight of the other_state
:return: A FeetState Object of which the positions and velocities of both the
feet are the weighted average of those of the base and other states.
"""
if parameter == 0:
return base_state
elif parameter == 1:
return other_state
resulting_right_foot = Foot.weighted_average_foot(
base_state.right_foot, other_state.right_foot, parameter)
resulting_left_foot = Foot.weighted_average_foot(
base_state.left_foot, other_state.left_foot, parameter)
resulting_time = weighted_average(base_state.time, other_state.time,
parameter)
return cls(resulting_right_foot, resulting_left_foot, resulting_time)
def get_foot_position_interpolated_joint_trajectories(
base_subgait, other_subgait, parameter):
"""Creates a list of joint trajectories with linearly interpolated corresponding foot location.
The foot location corresponding to the resulting trajectories is equal to the weighted average (with the
parameter) of the foor locations corresponding to the base and other subgait.
:param base_subgait: base subgait, return value if parameter is equal to zero
:param other_subgiat: other subgait, return value if parameter is equal to one
:param parameter: Parameter for interpolation, between 0 and 1
:return: A list of interpolated joint trajectories
"""
interpolated_joint_trajectories = []
# for inverse kinematics it is required that all joints have the same number of setpoints as to calculate
# the foot position at a certain time. The inverse kinematics also needs acces to the 'ith' setpoints
# of all joints
Subgait.check_foot_position_interpolation_is_safe(
base_subgait, other_subgait)
number_of_setpoints = len(base_subgait.joints[0].setpoints)
(
base_setpoints_to_interpolate,
other_setpoints_to_interpolate,
) = JointTrajectory.change_order_of_joints_and_setpoints(
base_subgait, other_subgait)
new_setpoints = {joint.name: [] for joint in base_subgait.joints}
# fill all joints in new_setpoints except the ankle joints using the inverse kinematics
for setpoint_index in range(number_of_setpoints):
base_feet_state = FeetState.from_setpoints(
base_setpoints_to_interpolate[setpoint_index])
other_feet_state = FeetState.from_setpoints(
other_setpoints_to_interpolate[setpoint_index])
new_feet_state = FeetState.weighted_average_states(
base_feet_state, other_feet_state, parameter)
setpoints_to_add = FeetState.feet_state_to_setpoints(
new_feet_state)
for joint_name in JOINT_NAMES_IK:
new_setpoints[joint_name].append(setpoints_to_add[joint_name])
# fill the ankle joint using the angle based linear interpolation
for ankle_joint in ["left_ankle", "right_ankle"]:
for base_setpoint, other_setpoint in zip(
base_subgait.get_joint(ankle_joint).setpoints,
other_subgait.get_joint(ankle_joint).setpoints,
):
new_ankle_setpoint_to_add = Setpoint.interpolate_setpoints(
base_setpoint, other_setpoint, parameter)
new_setpoints[ankle_joint].append(new_ankle_setpoint_to_add)
duration = weighted_average(base_subgait.duration,
other_subgait.duration, parameter)
for joint in base_subgait.joints:
interpolated_joint_trajectory_to_add = JointTrajectory(
joint.name, joint.limits, new_setpoints[joint.name], duration)
interpolated_joint_trajectories.append(
interpolated_joint_trajectory_to_add)
return interpolated_joint_trajectories
def interpolate_subgaits(cls,
base_subgait,
other_subgait,
parameter,
use_foot_position=False):
"""Linearly interpolate two subgaits with the parameter to get a new subgait. based on foot_pos, or on angles.
:param base_subgait:
base subgait, return value if parameter is equal to zero
:param other_subgait:
other subgait, return value if parameter is equal to one
:param parameter:
The parameter to use for interpolation. Should be 0 <= parameter <= 1
:param use_foot_position:
Determine whether the interpolation should be done on the foot location or on the joint angles
:return:
The interpolated subgait
"""
if parameter == 1:
return other_subgait
if parameter == 0:
return base_subgait
if not (0 < parameter < 1):
raise ValueError(
"Parameter for interpolation should be in the interval [0, 1], but is {0}"
.format(parameter))
if use_foot_position:
joints = Subgait.get_foot_position_interpolated_joint_trajectories(
base_subgait, other_subgait, parameter)
else:
joints = Subgait.get_joint_angle_interpolated_joint_trajectories(
base_subgait, other_subgait, parameter)
description = "Interpolation between base version {0}, and other version {1} with parameter{2}. Based on foot position: {3}".format(
base_subgait.version, other_subgait.version, parameter,
use_foot_position)
duration = weighted_average(base_subgait.duration,
other_subgait.duration, parameter)
gait_type = (base_subgait.gait_type
if parameter <= 0.5 else other_subgait.gait_type)
version = "{0}{1}_({2})_({3})".format(
PARAMETRIC_GAITS_PREFIX,
parameter,
base_subgait.version,
other_subgait.version,
)
return Subgait(
joints,
duration,
gait_type,
base_subgait.gait_name,
base_subgait.subgait_name,
version,
description,
)
def interpolate_setpoints(base_setpoint, other_setpoint, parameter):
"""Linearly interpolate two setpoints.
:param base_setpoint:
base setpoint, return value if parameter is zero
:param other_setpoint:
other setpoint, return value if parameter is one
:param parameter:
parameter for linear interpolation, 0 <= parameter <= 1
:return:
The interpolated setpoint
"""
time = weighted_average(base_setpoint.time, other_setpoint.time, parameter)
position = weighted_average(
base_setpoint.position, other_setpoint.position, parameter
)
velocity = weighted_average(
base_setpoint.velocity, other_setpoint.velocity, parameter
)
return Setpoint(time, position, velocity)
def weighted_average_foot(base_foot, other_foot, parameter):
"""Computes the weighted average of two Foot objects, result has a velocity of None if it cannot be computed."""
if base_foot.foot_side != other_foot.foot_side:
raise SideSpecificationError(
"expected sides of both base and other foot to be equal but were {base} and "
"{other}.".format(base=base_foot.foot_side,
other=other_foot.foot_side))
resulting_position = weighted_average(base_foot.position,
other_foot.position, parameter)
if base_foot.velocity is not None and other_foot.velocity is not None:
resulting_velocity = weighted_average(base_foot.velocity,
other_foot.velocity,
parameter)
else:
rospy.logwarn(
"one or both of the provided feet does not have a velocity specified, "
"setting the resulting velocity to None")
resulting_velocity = None
return Foot(base_foot.foot_side, resulting_position,
resulting_velocity)