You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
243 lines
11 KiB
Python
243 lines
11 KiB
Python
7 months ago
|
from math import asin, atan, atan2, pi, sqrt
|
||
|
|
from math_ops.Matrix_3x3 import Matrix_3x3
|
||
|
|
from math_ops.Math_Ops import Math_Ops as M
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
class Inverse_Kinematics():
|
||
|
|
|
||
|
|
# leg y deviation, upper leg height, upper leg depth, lower leg length, knee extra angle, max ankle z
|
||
|
|
NAO_SPECS_PER_ROBOT = ((0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091),
|
||
|
|
(0.055, 0.13832, 0.005, 0.11832, atan(0.005/0.13832), -0.106),
|
||
|
|
(0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091),
|
||
|
|
(0.072954143,0.147868424, 0.005, 0.127868424, atan(0.005/0.147868424), -0.114),
|
||
|
|
(0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091))
|
||
|
|
|
||
|
|
TORSO_HIP_Z = 0.115 # distance in the z-axis, between the torso and each hip (same for all robots)
|
||
|
|
TORSO_HIP_X = 0.01 # distance in the x-axis, between the torso and each hip (same for all robots) (hip is 0.01m to the back)
|
||
|
|
|
||
|
|
def __init__(self, robot) -> None:
|
||
|
|
self.robot = robot
|
||
|
|
self.NAO_SPECS = Inverse_Kinematics.NAO_SPECS_PER_ROBOT[robot.type]
|
||
|
|
|
||
|
|
def torso_to_hip_transform(self, coords, is_batch=False):
|
||
|
|
'''
|
||
|
|
Convert cartesian coordinates that are relative to torso to coordinates that are relative the center of both hip joints
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
coords : array_like
|
||
|
|
One 3D position or list of 3D positions
|
||
|
|
is_batch : `bool`
|
||
|
|
Indicates if coords is a batch of 3D positions
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
coord : `list` or ndarray
|
||
|
|
A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned
|
||
|
|
'''
|
||
|
|
if is_batch:
|
||
|
|
return [c + (Inverse_Kinematics.TORSO_HIP_X, 0, Inverse_Kinematics.TORSO_HIP_Z) for c in coords]
|
||
|
|
else:
|
||
|
|
return coords + (Inverse_Kinematics.TORSO_HIP_X, 0, Inverse_Kinematics.TORSO_HIP_Z)
|
||
|
|
|
||
|
|
|
||
|
|
def head_to_hip_transform(self, coords, is_batch=False):
|
||
|
|
'''
|
||
|
|
Convert cartesian coordinates that are relative to head to coordinates that are relative the center of both hip joints
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
coords : array_like
|
||
|
|
One 3D position or list of 3D positions
|
||
|
|
is_batch : `bool`
|
||
|
|
Indicates if coords is a batch of 3D positions
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
coord : `list` or ndarray
|
||
|
|
A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned
|
||
|
|
'''
|
||
|
|
coords_rel_torso = self.robot.head_to_body_part_transform( "torso", coords, is_batch )
|
||
|
|
return self.torso_to_hip_transform(coords_rel_torso, is_batch)
|
||
|
|
|
||
|
|
def get_body_part_pos_relative_to_hip(self, body_part_name):
|
||
|
|
''' Get body part position relative to the center of both hip joints '''
|
||
|
|
bp_rel_head = self.robot.body_parts[body_part_name].transform.get_translation()
|
||
|
|
return self.head_to_hip_transform(bp_rel_head)
|
||
|
|
|
||
|
|
def get_ankle_pos_relative_to_hip(self, is_left):
|
||
|
|
''' Internally calls get_body_part_pos_relative_to_hip() '''
|
||
|
|
return self.get_body_part_pos_relative_to_hip("lankle" if is_left else "rankle")
|
||
|
|
|
||
|
|
def get_linear_leg_trajectory(self, is_left:bool, p1, p2=None, foot_ori3d=(0,0,0), dynamic_pose:bool=True, resolution=100):
|
||
|
|
'''
|
||
|
|
Compute leg trajectory so that the ankle moves linearly between two 3D points (relative to hip)
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
is_left : `bool`
|
||
|
|
set to True to select left leg, False to select right leg
|
||
|
|
p1 : array_like, length 3
|
||
|
|
if p2 is None:
|
||
|
|
p1 is the target position (relative to hip), and the initial point is given by the ankle's current position
|
||
|
|
if p2 is not None:
|
||
|
|
p1 is the initial point (relative to hip)
|
||
|
|
p2 : array_like, length 3 / `None`
|
||
|
|
target position (relative to hip) or None (see p1)
|
||
|
|
foot_ori3d : array_like, length 3
|
||
|
|
rotation around x,y,z (rotation around x & y are biases, relative to a vertical pose, or dynamic pose, if enabled)
|
||
|
|
dynamic_pose : `bool`
|
||
|
|
enable dynamic feet rotation to be parallel to the ground, based on IMU
|
||
|
|
resolution : int
|
||
|
|
interpolation resolution; more resolution is always better, but it takes more time to compute;
|
||
|
|
having more points does not make the movement slower, because if there are excessive points they are removed
|
||
|
|
during the analytical optimization
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
trajecory : `tuple`
|
||
|
|
indices, [[values_1,error_codes_1], [values_2,error_codes_2], ...]
|
||
|
|
See leg() for further details
|
||
|
|
'''
|
||
|
|
|
||
|
|
if p2 is None:
|
||
|
|
p2 = np.asarray(p1, float)
|
||
|
|
p1 = self.get_body_part_pos_relative_to_hip('lankle' if is_left else 'rankle')
|
||
|
|
else:
|
||
|
|
p1 = np.asarray(p1, float)
|
||
|
|
p2 = np.asarray(p2, float)
|
||
|
|
|
||
|
|
vec = (p2 - p1) / resolution
|
||
|
|
|
||
|
|
|
||
|
|
hip_points = [p1 + vec * i for i in range(1,resolution+1)]
|
||
|
|
interpolation = [self.leg(p, foot_ori3d, is_left, dynamic_pose) for p in hip_points]
|
||
|
|
|
||
|
|
indices = [2,4,6,8,10,12] if is_left else [3,5,7,9,11,13]
|
||
|
|
|
||
|
|
last_joint_values = self.robot.joints_position[indices[0:4]] #exclude feet joints to compute ankle trajectory
|
||
|
|
next_step = interpolation[0]
|
||
|
|
trajectory = []
|
||
|
|
|
||
|
|
for p in interpolation[1:-1]:
|
||
|
|
if np.any(np.abs(p[1][0:4]-last_joint_values) > 7.03):
|
||
|
|
trajectory.append(next_step[1:3])
|
||
|
|
last_joint_values = next_step[1][0:4]
|
||
|
|
next_step = p
|
||
|
|
else:
|
||
|
|
next_step = p
|
||
|
|
|
||
|
|
trajectory.append(interpolation[-1][1:3])
|
||
|
|
|
||
|
|
return indices, trajectory
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
def leg(self, ankle_pos3d, foot_ori3d, is_left:bool, dynamic_pose:bool):
|
||
|
|
'''
|
||
|
|
Compute inverse kinematics for the leg, considering as input the relative 3D position of the ankle and 3D orientation* of the foot
|
||
|
|
*the yaw can be controlled directly, but the pitch and roll are biases (see below)
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
ankle_pos3d : array_like, length 3
|
||
|
|
(x,y,z) position of ankle in 3D, relative to the center of both hip joints
|
||
|
|
foot_ori3d : array_like, length 3
|
||
|
|
rotation around x,y,z (rotation around x & y are biases, relative to a vertical pose, or dynamic pose, if enabled)
|
||
|
|
is_left : `bool`
|
||
|
|
set to True to select left leg, False to select right leg
|
||
|
|
dynamic_pose : `bool`
|
||
|
|
enable dynamic feet rotation to be parallel to the ground, based on IMU
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
indices : `list`
|
||
|
|
indices of computed joints
|
||
|
|
values : `list`
|
||
|
|
values of computed joints
|
||
|
|
error_codes : `list`
|
||
|
|
list of error codes
|
||
|
|
Error codes:
|
||
|
|
(-1) Foot is too far (unreachable)
|
||
|
|
(x) Joint x is out of range
|
||
|
|
'''
|
||
|
|
|
||
|
|
error_codes = []
|
||
|
|
leg_y_dev, upper_leg_height, upper_leg_depth, lower_leg_len, knee_extra_angle, _ = self.NAO_SPECS
|
||
|
|
sign = -1 if is_left else 1
|
||
|
|
|
||
|
|
# Then we translate to origin of leg by shifting the y coordinate
|
||
|
|
ankle_pos3d = np.asarray(ankle_pos3d) + (0,sign*leg_y_dev,0)
|
||
|
|
|
||
|
|
# First we rotate the leg, then we rotate the coordinates to abstract from the rotation
|
||
|
|
ankle_pos3d = Matrix_3x3().rotate_z_deg(-foot_ori3d[2]).multiply(ankle_pos3d)
|
||
|
|
|
||
|
|
# Use geometric solution to compute knee angle and foot pitch
|
||
|
|
dist = np.linalg.norm(ankle_pos3d) #dist hip <-> ankle
|
||
|
|
sq_dist = dist * dist
|
||
|
|
sq_upper_leg_h = upper_leg_height * upper_leg_height
|
||
|
|
sq_lower_leg_l = lower_leg_len * lower_leg_len
|
||
|
|
sq_upper_leg_l = upper_leg_depth * upper_leg_depth + sq_upper_leg_h
|
||
|
|
upper_leg_len = sqrt(sq_upper_leg_l)
|
||
|
|
knee = M.acos((sq_upper_leg_l + sq_lower_leg_l - sq_dist)/(2 * upper_leg_len * lower_leg_len)) + knee_extra_angle # Law of cosines
|
||
|
|
foot = M.acos((sq_lower_leg_l + sq_dist - sq_upper_leg_l)/(2 * lower_leg_len * dist)) # foot perpendicular to vec(origin->ankle_pos)
|
||
|
|
|
||
|
|
# Check if target is reachable
|
||
|
|
if dist > upper_leg_len + lower_leg_len:
|
||
|
|
error_codes.append(-1)
|
||
|
|
|
||
|
|
# Knee and foot
|
||
|
|
knee_angle = pi - knee
|
||
|
|
foot_pitch = foot - atan(ankle_pos3d[0] / np.linalg.norm(ankle_pos3d[1:3]))
|
||
|
|
foot_roll = atan(ankle_pos3d[1] / min(-0.05, ankle_pos3d[2])) * -sign # avoid instability of foot roll (not relevant above -0.05m)
|
||
|
|
|
||
|
|
# Raw hip angles if all joints were straightforward
|
||
|
|
raw_hip_yaw = foot_ori3d[2]
|
||
|
|
raw_hip_pitch = foot_pitch - knee_angle
|
||
|
|
raw_hip_roll = -sign * foot_roll
|
||
|
|
|
||
|
|
# Rotate 45deg due to yaw joint orientation, then rotate yaw, roll and pitch
|
||
|
|
m = Matrix_3x3().rotate_y_rad(raw_hip_pitch).rotate_x_rad(raw_hip_roll).rotate_z_deg(raw_hip_yaw).rotate_x_deg(-45*sign)
|
||
|
|
|
||
|
|
# Get actual hip angles considering the yaw joint orientation
|
||
|
|
hip_roll = (pi/4) - (sign * asin(m.m[1,2])) #Add pi/4 due to 45deg rotation
|
||
|
|
hip_pitch = - atan2(m.m[0,2],m.m[2,2])
|
||
|
|
hip_yaw = sign * atan2(m.m[1,0],m.m[1,1])
|
||
|
|
|
||
|
|
# Convert rad to deg
|
||
|
|
values = np.array([hip_yaw,hip_roll,hip_pitch,-knee_angle,foot_pitch,foot_roll]) * 57.2957795 #rad to deg
|
||
|
|
|
||
|
|
# Set feet rotation bias (based on vertical pose, or dynamic_pose)
|
||
|
|
values[4] -= foot_ori3d[1]
|
||
|
|
values[5] -= foot_ori3d[0] * sign
|
||
|
|
|
||
|
|
indices = [2,4,6,8,10,12] if is_left else [3,5,7,9,11,13]
|
||
|
|
|
||
|
|
if dynamic_pose:
|
||
|
|
|
||
|
|
# Rotation of torso in relation to foot
|
||
|
|
m : Matrix_3x3 = Matrix_3x3.from_rotation_deg((self.robot.imu_torso_roll, self.robot.imu_torso_pitch, 0))
|
||
|
|
m.rotate_z_deg(foot_ori3d[2], True)
|
||
|
|
|
||
|
|
roll = m.get_roll_deg()
|
||
|
|
pitch = m.get_pitch_deg()
|
||
|
|
|
||
|
|
# Simple balance algorithm
|
||
|
|
correction = 1 #correction to motivate a vertical torso (in degrees)
|
||
|
|
roll = 0 if abs(roll) < correction else roll - np.copysign(correction,roll)
|
||
|
|
pitch = 0 if abs(pitch) < correction else pitch - np.copysign(correction,pitch)
|
||
|
|
|
||
|
|
values[4] += pitch
|
||
|
|
values[5] += roll * sign
|
||
|
|
|
||
|
|
|
||
|
|
# Check and limit range of joints
|
||
|
|
for i in range(len(indices)):
|
||
|
|
if values[i] < self.robot.joints_info[indices[i]].min or values[i] > self.robot.joints_info[indices[i]].max:
|
||
|
|
error_codes.append(indices[i])
|
||
|
|
values[i] = np.clip(values[i], self.robot.joints_info[indices[i]].min, self.robot.joints_info[indices[i]].max)
|
||
|
|
|
||
|
|
|
||
|
|
return indices, values, error_codes
|
||
|
|
|