def test_interpolation_correct(self):
parameter = 0.3
other_setpoint = Setpoint(1, 1, 1)
expected_result = Setpoint(
self.setpoint.time * parameter + (1 - parameter) * 1,
self.setpoint.position * parameter + (1 - parameter) * 1,
self.setpoint.velocity * parameter + (1 - parameter) * 1)
self.assertEqual(
expected_result,
Setpoint.interpolate_setpoints(self.setpoint, other_setpoint,
parameter))
def create_subgait_of_trajectory(self, joint_trajectory, subgait_name):
"""Create a subgait using the joint trajectory generated by the capture point pose.
:param joint_trajectory: The capture point pose trajectory
:param subgait_name: the normal subgait name
:return: a populated subgait object generated from the joint trajectory
"""
normal_subgait = deepcopy(self.default_walk[subgait_name])
balance_duration = joint_trajectory.points[-1].time_from_start.to_sec()
balance_joints = []
for joint_index, joint_name in enumerate(joint_trajectory.joint_names):
normal_joint = normal_subgait.get_joint(joint_name)
setpoints = []
for joint_trajectory_point in joint_trajectory.points:
time = joint_trajectory_point.time_from_start.to_sec()
position = joint_trajectory_point.positions[joint_index]
velocity = joint_trajectory_point.velocities[joint_index]
setpoints.append(Setpoint(time, position, velocity))
balance_joints.append(
JointTrajectory(joint_name, normal_joint.limits, setpoints,
balance_duration))
balance_subgait = BalanceGait.to_subgait(balance_joints,
balance_duration,
subgait_name=subgait_name)
return balance_subgait
def test_final_position(self):
position = 1.0
subgait = Subgait(
[
JointTrajectory(
"test",
Limits(0.0, 0.0, 0.0),
[
Setpoint(0.0, 0.0, 0.0),
Setpoint(0.5, position, 0.0),
],
1.0,
)
],
1.0,
)
self.assertDictEqual(subgait.final_position, {"test": position})
def test_invalid_boundary_points_nonzero_end_nonzero_speed(self):
# Last setpoint at t = 1 =/= duration has nonzero speed.
setpoints = [
Setpoint(0.5 * t, (self.duration - t), t / 2.0) for t in self.times
]
joint_trajectory = JointTrajectory(self.joint_name, self.limits,
setpoints, self.duration)
self.assertFalse(joint_trajectory._validate_boundary_points())
def setUp(self):
self.joint_name = 'test_joint'
self.limits = Limits(-1, 1, 2)
self.duration = 2.0
self.times = [0, self.duration / 2.0, self.duration]
self.setpoints = [Setpoint(t, 2 * t, t / 2.0) for t in self.times]
self.joint_trajectory = JointTrajectory(self.joint_name, self.limits,
self.setpoints, self.duration)
def test_set_duration(self):
expected = Setpoint(
self.joint_trajectory.setpoints[-1].time * 2,
self.joint_trajectory.setpoints[-1].position,
self.joint_trajectory.setpoints[-1].velocity / 2,
)
self.joint_trajectory.set_duration(self.duration * 2)
self.assertEqual(self.joint_trajectory.setpoints[-1], expected)
def from_setpoints(cls, setpoint_dic):
"""Calculate the position and velocity of the foot (or rather ankle) from joint angles.
:param setpoint_dic:
Dictionary of setpoints from which the feet positions and velocities need to be calculated, should all
be around the same time
:return:
A FeetState object with a left and right foot which each have a position and velocity corresponding to the
setpoint dictionary
"""
for joint in JOINT_NAMES_IK:
if joint not in setpoint_dic:
raise KeyError(
"expected setpoint dictionary to contain joint {joint}, but {joint} was missing."
.format(joint=joint))
foot_state_left = Foot.calculate_foot_position(
setpoint_dic["left_hip_aa"].position,
setpoint_dic["left_hip_fe"].position,
setpoint_dic["left_knee"].position,
Side.left,
)
foot_state_right = Foot.calculate_foot_position(
setpoint_dic["right_hip_aa"].position,
setpoint_dic["right_hip_fe"].position,
setpoint_dic["right_knee"].position,
Side.right,
)
next_joint_positions = Setpoint.calculate_next_positions_joint(
setpoint_dic)
next_foot_state_left = Foot.calculate_foot_position(
next_joint_positions["left_hip_aa"].position,
next_joint_positions["left_hip_fe"].position,
next_joint_positions["left_knee"].position,
Side.left,
)
next_foot_state_right = Foot.calculate_foot_position(
next_joint_positions["right_hip_aa"].position,
next_joint_positions["right_hip_fe"].position,
next_joint_positions["right_knee"].position,
Side.right,
)
foot_state_left.add_foot_velocity_from_next_state(next_foot_state_left)
foot_state_right.add_foot_velocity_from_next_state(
next_foot_state_right)
# Set the time of the new setpoints as the weighted average of the original setpoint times
feet_state_time = 0
for setpoint in setpoint_dic.values():
feet_state_time += setpoint.time
feet_state_time = feet_state_time / len(setpoint_dic)
# feet state
return cls(foot_state_right, foot_state_left, feet_state_time)
def test_invalid_boundary_points_nonzero_start_nonzero_speed(self):
# First setpoint at t = 1 has nonzero speed.
setpoints = [
Setpoint(0.5 * t + 1, (self.duration - t), (self.duration - t) * 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_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(0.5 * t + 0.5, (self.duration - t), t * 2 - t**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 test_invalid_joint_transition_velocity(self):
next_setpoints = [
Setpoint(t, 2 * (self.duration - t), (self.duration - t))
for t in self.times
]
next_joint_trajectory = JointTrajectory(self.joint_name, self.limits,
next_setpoints, self.duration)
self.assertFalse(
self.joint_trajectory.validate_joint_transition(
next_joint_trajectory))
def _transition_setpoint(old_setpoint, new_setpoint, new_factor):
"""Create a transition setpoint with the use of the old setpoint, new setpoint and transition factor."""
old_factor = 1.0 - new_factor
position = (old_setpoint.position *
old_factor) + (new_setpoint.position * new_factor)
velocity = (old_setpoint.velocity *
old_factor) + (new_setpoint.velocity * new_factor)
return Setpoint(new_setpoint.time, position, velocity)
def setUp(self):
self.setpoint = Setpoint(1.123412541, 0.034341255,
123.162084549) # 0.0343412512 123.16208454
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_interpolate_trajectories_correct_duration(self):
parameter = 0.5
other_duration = self.duration + 1
other_times = [0, other_duration / 2.0, other_duration]
other_setpoints = [Setpoint(t, 2 * t, t / 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 * parameter + (1 - parameter) * other_duration)
def test_find_known_position_sit(self):
setpoint_dict = {
"left_hip_aa": Setpoint(0, 0, 0),
"left_hip_fe": Setpoint(0, pi / 2, 0),
"left_knee": Setpoint(0, pi / 2, 0),
"right_hip_aa": Setpoint(0, 0, 0),
"right_hip_fe": Setpoint(0, pi / 2, 0),
"right_knee": Setpoint(0, pi / 2, 0),
}
[
l_ul,
l_ll,
_,
_,
r_ul,
r_ll,
_,
_,
_,
] = get_lengths_robot_for_inverse_kinematics(Side.both)
resulting_state = FeetState.from_setpoints(setpoint_dict)
expected_right_foot = Foot(Side.right,
Vector3d(r_ul + 0.1395, 0.1705, r_ll),
Vector3d(0, 0, 0))
expected_left_foot = Foot(Side.right,
Vector3d(l_ul + 0.1395, -0.1705, l_ll),
Vector3d(0, 0, 0))
expected_state = FeetState(expected_right_foot, expected_left_foot, 0)
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 test_set_forward_backward_swing_inverse_kinematics(self):
[
l_ul,
l_ll,
_,
_,
r_ul,
r_ll,
_,
_,
_,
] = get_lengths_robot_for_inverse_kinematics(Side.both)
parameter = 0.99
base_setpoint_dict = {
"left_hip_aa": Setpoint(0, 0, 0),
"left_hip_fe": Setpoint(0, 0, 0),
"left_knee": Setpoint(0, 0, 0),
"right_hip_aa": Setpoint(0, 0, 0),
"right_hip_fe": Setpoint(0, 0, 0),
"right_knee": Setpoint(0, 0, 0),
}
other_setpoint_dict = {
"left_hip_aa": Setpoint(0, 0, 0),
"left_hip_fe": Setpoint(0, pi / 2, 0),
"left_knee": Setpoint(0, 0, 0),
"right_hip_aa": Setpoint(0, 0, 0),
"right_hip_fe": Setpoint(0, pi / 2, 0),
"right_knee": Setpoint(0, 0, 0),
}
base_state = FeetState.from_setpoints(base_setpoint_dict)
other_state = FeetState.from_setpoints(other_setpoint_dict)
resulting_state = FeetState.weighted_average_states(
base_state, other_state, parameter)
base_expected_right_foot = Foot(Side.right,
Vector3d(0.1395, 0.1705, r_ul + r_ll),
Vector3d(0, 0, 0))
base_expected_left_foot = Foot(Side.right,
Vector3d(0.1395, -0.1705, l_ul + l_ll),
Vector3d(0, 0, 0))
base_expected_state = FeetState(base_expected_right_foot,
base_expected_left_foot, 0)
other_expected_right_foot = Foot(
Side.right, Vector3d(r_ul + r_ll + 0.1395, 0.1705, 0),
Vector3d(0, 0, 0))
other_expected_left_foot = Foot(
Side.right, Vector3d(l_ul + l_ll + 0.1395, -0.1705, 0),
Vector3d(0, 0, 0))
other_expected_state = FeetState(other_expected_right_foot,
other_expected_left_foot, 0)
expected_state = FeetState.weighted_average_states(
base_expected_state, other_expected_state, parameter)
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 test_unequal_velocity(self):
other_setpoint = Setpoint(1.12341254, 0.03434126, 0)
self.assertNotEqual(self.setpoint, other_setpoint)
def test_get_interpolated_setpoints_invalid_time_too_low(self):
setpoint = self.joint_trajectory.get_interpolated_setpoint(-1)
self.assertEqual(setpoint, Setpoint(-1, self.setpoints[0].position, 0))
def setUp(self):
self.setpoint = Setpoint(1.123412541, 0.0343412512, 123.162084)
def test_equal(self):
other_setpoint = Setpoint(1.123410541132, 0.03433998, 123.1621)
self.assertEqual(self.setpoint, other_setpoint)
def test_unequal_time(self):
other_setpoint = Setpoint(1.12349, 0.0343, 123.1621)
self.assertNotEqual(self.setpoint, other_setpoint)
def test_get_interpolated_setpoints_invalid_time_too_high(self):
setpoint = self.joint_trajectory.get_interpolated_setpoint(self.duration + 1)
self.assertEqual(
setpoint, Setpoint(self.duration + 1, self.setpoints[-1].position, 0)
)
def test_equal(self):
other_setpoint = Setpoint(1.1234125445313287, 0.034341264534,
123.16208455453)
self.assertEqual(self.setpoint, other_setpoint)
def test_get_interpolated_setpoints_home_subgait(self):
self.joint_trajectory.setpoints = [Setpoint(3, 1, 1)]
setpoint = self.joint_trajectory.get_interpolated_setpoint(1)
self.assertEqual(setpoint, Setpoint(1, 1, 1))
def test_unequal_time(self):
other_setpoint = Setpoint(1.123491381, 0.03434126, 123.16208455)
self.assertNotEqual(self.setpoint, other_setpoint)
def test_unequal_position(self):
other_setpoint = Setpoint(1.12341254, -0.03434126, 123.16208455)
self.assertNotEqual(self.setpoint, other_setpoint)
def calculate_joint_angles_from_foot_position(foot_position, foot_side,
time):
"""Calculates the angles of the joints corresponding to a certain position of the right or left foot.
More information on the calculations of the haa, hfe and kfe angles, aswell as on the velocity calculations can
be found at https://confluence.projectmarch.nl:8443/display/62tech/%28Inverse%29+kinematics. This function
assumes that the desired z position of the foot is positive.
:param foot_position: A Vecor3d object which specifies the desired position of the foot.
:param foot_side: A string which specifies to which side the foot_position belongs and thus which joint angles
should be computed.
:param time: The time of the foot_position and the resulting setpoints.
:return:
A dictionary of Setpoints for each joint on the requested side with the correct angle at the provided time.
"""
# Get relevant lengths from robot model, ul = upper leg etc. see get_lengths_robot_for_inverse_kinematics()
# and unpack desired position
[ul, ll, hl, ph,
base] = get_lengths_robot_for_inverse_kinematics(foot_side)
x_position = foot_position.x
y_position = foot_position.y
z_position = foot_position.z
# Change y positive direction to the desired foot, change origin to pivot of haa joint, for ease of calculation.
if foot_side == Side.left:
y_position = -(y_position + base / 2.0)
elif foot_side == Side.right:
y_position = y_position - base / 2.0
else:
raise SideSpecificationError(foot_side)
# Check if the HAA can be calculated (if the desired position is not too close to the origin)
if ph / sqrt(z_position * z_position + y_position * y_position) > 1:
raise SubgaitInterpolationError(
"The desired {foot} foot position, ({x}, {y}, {z}), is out of reach"
.format(foot=foot_side,
x=x_position,
y=y_position,
z=z_position))
# Then calculate the haa angle. This calculation assumes that pos_z > 0
haa = Foot.calculate_haa_angle(z_position, y_position, ph)
# Transform the desired x and z position to arrive at an easier system to calculate
# the hfe and kfe angles
transformed_x = round(x_position - hl,
MID_CALCULATION_PRECISION_DIGITS)
transformed_z = round(
sqrt(-ph * ph + y_position * y_position + z_position * z_position),
MID_CALCULATION_PRECISION_DIGITS,
)
if transformed_x * transformed_x + transformed_z * transformed_z > (
ll + ul) * (ll + ul):
raise SubgaitInterpolationError(
"The desired {foot} foot position, ({x}, {y}, {z}), is out of reach"
.format(foot=foot_side,
x=x_position,
y=y_position,
z=z_position))
hfe, kfe = Foot.calculate_hfe_kfe_angles(transformed_x, transformed_z,
ul, ll)
# angle positions
return {
foot_side.value + "_hip_aa": Setpoint(time, haa),
foot_side.value + "_hip_fe": Setpoint(time, hfe),
foot_side.value + "_knee": Setpoint(time, kfe),
}
