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 _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)
)
print('matches what is given in the source file.')
exit(1)
try:
model = hebi.robot_model.import_from_hrdf("hrdf/A-2085-06.hrdf")
except Exception as e:
print('Could not load HRDF: {0}'.format(e))
exit(1)
# Go to the XYZ positions at four corners of the box, and create a rotation matrix
# that has the end effector point straight forward.
xyz_targets = np.array([[0.20, 0.40, 0.40, 0.20], [0.30, 0.30, -0.30, -0.30],
[0.10, 0.10, 0.10, 0.10]])
xyz_cols = xyz_targets.shape[1]
rotation_target = math_utils.rotate_y(pi / 2)
# Convert these to joint angle waypoints using IK solutions for each of the xyz locations
# as well as the desired orientation of the end effector. Copy the initial waypoint at the end so we close the square.
# Choose an "elbow up" initial configuration for IK
elbow_up_angles = [0, pi / 4.0, pi / 2.0, pi / 4.0, -pi, pi / 2.0]
joint_targets = np.empty((group.size, xyz_cols + 1))
for col in range(xyz_cols):
so3_objective = hebi.robot_model.endeffector_so3_objective(rotation_target)
ee_position_objective = hebi.robot_model.endeffector_position_objective(
xyz_targets[:, col])
ik_res_angles = model.solve_inverse_kinematics(elbow_up_angles,
so3_objective,
ee_position_objective)
print('Arm end-effector is now following the mobile device pose.')
print('The control interface has the following commands:')
print(' B1 - Reset/re-align poses.')
print(' This takes the arm to home and aligns with mobile device.')
print(' A3 - Scale down the translation commands to the arm.')
print(' Sliding all the way down means the end-effector only rotates.')
print(' A6 - Control the gripper (if the arm has gripper).')
print(' Sliding down closes the gripper, sliding up opens.')
print(' B8 - Quits the demo.')
cmd = hebi.GroupCommand(group.size)
# Move to current coordinates
xyz_target_init = np.asarray([0.5, 0.0, 0.1])
rot_mat_target_init = rotate_y(np.pi)
def get_ik(xyz_target, rot_target, ik_seed):
# Helper function to rebuild the IK solver with the appropriate objective functions
return kin.solve_inverse_kinematics(
ik_seed_pos, endeffector_position_objective(xyz_target),
endeffector_so3_objective(rot_target))
# Startup
while not abort_flag:
group.get_next_feedback(reuse_fbk=fbk)
cmd.position = None
cmd.velocity = None