def update_stance(self, translation_vel, rotate_vel, dt):
"""
TODO: Document
"""
# stance_vel_xyz = translation_vel + rotate_vel.cross(cmd_stance_xyz)
self._stance_vel_xyz[:] = translation_vel + np.cross(rotate_vel, self._cmd_stance_xyz)
# FIXME: Cut down on allocations here
self._cmd_stance_xyz += translation_vel * dt
self._cmd_stance_xyz[:] = rotate_z(rotate_vel[2] * dt) @ rotate_y(rotate_vel[1] * dt) @ rotate_x(rotate_vel[0] * dt) @ self._cmd_stance_xyz
self._kinematics.get_end_effector(self._feedback_view.position, self._ee_frame)
np.copyto(self._fbk_stance_xyz, self._ee_frame[0:3, 3])
# Update home stance to match the current z height
self._level_home_stance_xyz[2] += translation_vel[2] * dt
self._home_stance_xyz[:] = rotate_x(0.2 * translation_vel[1]) @ rotate_y(-0.2 * translation_vel[0]) @ self._level_home_stance_xyz
def __init__(self, val_lock, name, group_indices):
assert name == 'Left' or name == 'Right'
super(Leg, self).__init__(val_lock, group_indices, name)
self._name = name
base_frame = np.identity(4, dtype=np.float64)
hip_t = np.identity(4, dtype=np.float64)
hip_t[0:3, 0:3] = math_utils.rotate_x(np.pi * 0.5)
hip_t[0:3, 3] = [0.0, 0.0225, 0.055]
self._hip_t = hip_t
kin = self._robot
kin.add_actuator('X5-9')
kin.add_link('X5', extension=0.375, twist=np.pi)
kin.add_actuator('X5-4')
kin.add_link('X5', extension=0.325, twist=np.pi)
home_knee_angle = np.deg2rad(130)
home_hip_angle = (np.pi + home_knee_angle) * 0.5
if name == 'Left':
self._direction = 1.0
home_angles = np.array([home_hip_angle, home_knee_angle],
dtype=np.float64)
base_frame[0:3, 3] = [0.0, 0.15, 0.0]
base_frame[0:3, 0:3] = math_utils.rotate_x(np.pi * -0.5)
else:
self._direction = -1.0
home_angles = np.array([-home_hip_angle, -home_knee_angle],
dtype=np.float64)
base_frame[0:3, 3] = [0.0, -0.15, 0.0]
base_frame[0:3, 0:3] = math_utils.rotate_x(np.pi * 0.5)
kin.base_frame = base_frame
self.home_angles = home_angles
masses = kin.masses
self._set_mass(np.sum(masses))
self._set_masses(masses)
# ----------------------
# Populate cached fields
output_frame_count = kin.get_frame_count('output')
com_frame_count = kin.get_frame_count('CoM')
self._current_xyz = np.asmatrix(
np.zeros((3, com_frame_count), dtype=np.float64))
for i in range(output_frame_count):
self._current_fk.append(
np.asmatrix(np.zeros((4, 4), dtype=np.float64)))
for i in range(com_frame_count):
self._current_coms.append(
np.asmatrix(np.zeros((4, 4), dtype=np.float64)))
# Calculate home FK
self._hip_angle = home_hip_angle
self._knee_angle = home_knee_angle
self._knee_velocity = 0.0
self._user_commanded_knee_velocity = 0.0
self._knee_angle_max = 2.65
self._knee_angle_min = 0.65
self._e_term = np.asmatrix(np.empty((6, 1), dtype=np.float64))
# Additionally calculate commanded endeffector position
self._current_cmd_tip_fk = np.asmatrix(
np.zeros((4, 4), dtype=np.float64))
def _calculate_lean_angle(self):
"""
The CoM of all individual bodies and pose estimate have been updated at this point
"""
rpy_modules = self._rpy
imu_modules = self._imu_modules
# {roll,pitch}_angle are in radians here
roll_angle = np.nanmean(rpy_modules[0, imu_modules])
pitch_angle = np.nanmean(rpy_modules[1, imu_modules])
# Update roll transform (rotation matrix)
math_utils.rotate_x(roll_angle, output=self._roll_rot)
# Update pitch transform (rotation matrix)
math_utils.rotate_y(pitch_angle, output=self._pitch_rot)
self._roll_angle = degrees(roll_angle)
self._pitch_angle = degrees(pitch_angle)
# Rotate by the pitch angle about the Y axis, followed by
# rotating by the roll angle about the X axis
np.matmul(self._pitch_rot, self._roll_rot, out=self._T_pose_rot)
# rad/s
self._feedback_lean_angle_velocity = self._pose_gyros_mean[1]
# Find the mean of the two legs' translation vectors at the endeffector
# and store it in `self._ground_point`
np.add(self._left_leg.current_tip_fk[0:3, 3],
self._right_leg.current_tip_fk[0:3, 3],
out=self._ground_point)
self._ground_point *= 0.5
# Get the vector starting from the "ground point" and ending at the
# position of the current center of mass, then rotate it according to
# the current pitch angle of Igor, storing the result in `self._line_com`
np.subtract(self._com, self._ground_point, out=self._line_com)
np.dot(self._pitch_rot, self._line_com, out=self._line_com)
# Get the magnitude of the "line CoM" and store it for later use.
# This is used to scale parameters in the balance controller.
self._height_com = np.linalg.norm(self._line_com)
# Using the line center of mass, find the feedback lean angle
self._feedback_lean_angle = degrees(
atan2(self._line_com[0, 0], self._line_com[2, 0]))
# Update Igor's center of mass by applying the transforms in the following order:
# 1) pitch rotation
# 2) roll rotation
np.dot(self._T_pose_rot, self._com, out=self._com)
# Copy the pose rotation matrix back into the pose transformation matrix
self._pose_transform[0:3, 0:3] = self._T_pose_rot
# Update CoM of legs based on current pose estimate from calculated lean angle
l_leg_coms = self._left_leg.current_coms
r_leg_coms = self._right_leg.current_coms
# Both legs have the same amount of kinematic bodies, so
# do calculations for both legs in same loop
for i in range(len(l_leg_coms)):
leg_com = l_leg_coms[i]
np.matmul(self._pose_transform, leg_com, out=leg_com)
leg_com = r_leg_coms[i]
np.matmul(self._pose_transform, leg_com, out=leg_com)