Refactors simulation an raspberry pi project
This commit is contained in:
Rune Harlyk
@@ -0,0 +1,371 @@
|
||||
from enum import Enum
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Phase(Enum):
|
||||
STANCE = 0
|
||||
SWING = 1
|
||||
|
||||
|
||||
def TransToRp(T):
|
||||
T = np.array(T)
|
||||
return T[0:3, 0:3], T[0:3, 3]
|
||||
|
||||
|
||||
class BezierGait():
|
||||
|
||||
def __init__(self, leg_phases=[0.0, 0.0, 0.5, 0.5], dt=0.01, t_swing=0.2):
|
||||
self.leg_phases = leg_phases
|
||||
self.prev_foot_pos = np.zeros((4, 3))
|
||||
|
||||
self.num_control_points = 11
|
||||
|
||||
self.dt = dt
|
||||
self.time = 0.0
|
||||
self.touch_down_time = 0.0
|
||||
self.last_touch_down_time = 0.0
|
||||
|
||||
# Trajectory Mode
|
||||
self.phase = Phase.SWING
|
||||
|
||||
# Swing Phase value [0, 1] of Reference Foot
|
||||
self.sw_ref = 0.0
|
||||
self.st_ref = 0.0
|
||||
# Whether Reference Foot has Touched Down
|
||||
self.touch_down = False
|
||||
|
||||
# Stance Time
|
||||
self.t_swing = t_swing
|
||||
|
||||
# Reference Leg
|
||||
self.ref_idx = 0
|
||||
|
||||
# Store all leg phases
|
||||
self.phases = self.leg_phases
|
||||
|
||||
def reset(self):
|
||||
self.prev_foot_pos.fill(0)
|
||||
self.time = 0.0
|
||||
self.touch_down_time = 0.0
|
||||
self.last_touch_down_time = 0.0
|
||||
self.phase = Phase.SWING
|
||||
self.sw_ref = 0.0
|
||||
self.st_ref = 0.0
|
||||
self.touch_down = False
|
||||
|
||||
def get_phase(self, index):
|
||||
"""Retrieves the phase of an individual leg.
|
||||
|
||||
NOTE modification
|
||||
from original paper:
|
||||
|
||||
if ti < -t_swing:
|
||||
ti += t_stride
|
||||
|
||||
This is to avoid a phase discontinuity if the user selects
|
||||
a Step Length and Velocity combination that causes t_stance > t_swing.
|
||||
|
||||
:param index: the leg's index, used to identify the required
|
||||
phase lag
|
||||
:param t_stance: the current user-specified stance period
|
||||
:param t_swing: the swing period (constant, class member)
|
||||
:return: Leg Phase, and StanceSwing (bool) to indicate whether
|
||||
leg is in stance or swing mode
|
||||
"""
|
||||
t_stride = self.t_stance + self.t_swing
|
||||
time_index = self.time_index(index, t_stride)
|
||||
|
||||
if time_index < -self.t_swing:
|
||||
time_index += t_stride
|
||||
|
||||
is_stance_phase = time_index >= 0.0 and time_index <= self.t_stance
|
||||
if is_stance_phase:
|
||||
return self.get_stance_phase(time_index, index)
|
||||
|
||||
return self.get_swing_phase(time_index, index)
|
||||
|
||||
def get_stance_phase(self, time_index, index):
|
||||
leg_phase = time_index / float(self.t_stance)
|
||||
if self.t_stance == 0.0:
|
||||
leg_phase = 0.0
|
||||
if index == self.ref_idx:
|
||||
self.phase = Phase.STANCE
|
||||
return leg_phase, Phase.STANCE
|
||||
|
||||
def get_swing_phase(self, time_index, index):
|
||||
leg_phase = 0.0
|
||||
if time_index >= -self.t_swing and time_index < 0.0:
|
||||
leg_phase = min((time_index + self.t_swing) / self.t_swing, 1.0)
|
||||
elif time_index > self.t_stance and time_index <= self.t_stride:
|
||||
leg_phase = min((time_index - self.t_stance) / self.t_swing, 1.0)
|
||||
# Touchdown at End of Swing
|
||||
leg_phase = min(leg_phase, 1.0)
|
||||
if index == self.ref_idx:
|
||||
self.phase = Phase.SWING
|
||||
self.sw_ref = leg_phase
|
||||
if self.sw_ref >= 0.999:
|
||||
self.touch_down = True
|
||||
return leg_phase, Phase.SWING
|
||||
|
||||
def time_index(self, index, t_stride):
|
||||
"""Retrieves the time index for the individual leg
|
||||
|
||||
:param index: the leg's index, used to identify the required
|
||||
phase lag
|
||||
:param t_stride: the total leg movement period (t_stance + t_swing)
|
||||
:return: the leg's time index
|
||||
"""
|
||||
# NOTE: for some reason python's having numerical issues w this
|
||||
# setting to 0 for ref leg by force
|
||||
if index == self.ref_idx:
|
||||
self.leg_phases[index] = 0.0
|
||||
return self.last_touch_down_time - self.leg_phases[index] * t_stride
|
||||
|
||||
def update_clock(self, dt):
|
||||
"""Increments the Bezier gait generator's internal clock (self.time)
|
||||
|
||||
:param dt: the time step
|
||||
phase lag
|
||||
:return: the leg's time index
|
||||
"""
|
||||
self.t_stride = self.t_stance + self.t_swing
|
||||
self._check_touch_down()
|
||||
self.last_touch_down_time = self.time - self.touch_down_time
|
||||
if self.last_touch_down_time > self.t_stride:
|
||||
self.last_touch_down_time = self.t_stride
|
||||
elif self.last_touch_down_time < 0.0:
|
||||
self.last_touch_down_time = 0.0
|
||||
self.time += dt
|
||||
|
||||
if self.t_stride < self.t_swing + dt:
|
||||
self.time = 0.0
|
||||
self.last_touch_down_time = 0.0
|
||||
self.touch_down_time = 0.0
|
||||
self.sw_ref = 0.0
|
||||
|
||||
def _check_touch_down(self):
|
||||
"""Checks whether a reference leg touchdown
|
||||
has occurred, and whether this warrants
|
||||
resetting the touchdown time
|
||||
"""
|
||||
if self.sw_ref >= 0.9 and self.touch_down:
|
||||
self.touch_down_time = self.time
|
||||
self.touch_down = False
|
||||
self.sw_ref = 0.0
|
||||
|
||||
def _binomial(self, n, k):
|
||||
return np.math.factorial(n) / (np.math.factorial(k) * np.math.factorial(n - k))
|
||||
|
||||
def _bern_stein_poly(self, t, n, k, point):
|
||||
return point * self._binomial(n, k) * np.power(t, k) * np.power(1 - t, n - k)
|
||||
|
||||
def _bezier_swing(self, phase, L, lateral_fraction, clearance_height=0.04):
|
||||
STEP = np.array(
|
||||
[-L] * 2 + [-L * 1.5] * 3 + [0.0] * 3 + [L * 1.5] * 2 + [L * 1.4, L]
|
||||
)
|
||||
Z = np.array(
|
||||
[0.0] * 2
|
||||
+ [clearance_height * 0.9] * 5
|
||||
+ [clearance_height * 1.1] * 3
|
||||
+ [0.0] * 2
|
||||
)
|
||||
X, Y = STEP * np.cos(lateral_fraction), STEP * np.sin(lateral_fraction)
|
||||
n = self.num_control_points
|
||||
|
||||
stepX = sum(self._bern_stein_poly(phase, n, i, X[i]) for i in range(n))
|
||||
stepY = sum(self._bern_stein_poly(phase, n, i, Y[i]) for i in range(n))
|
||||
stepZ = sum(self._bern_stein_poly(phase, n, i, Z[i]) for i in range(n))
|
||||
|
||||
return stepX, stepY, stepZ
|
||||
|
||||
def sine_stance(self, phase, L, lateral_fraction, penetration_depth=0.00):
|
||||
"""Calculates the step coordinates for the Sinusoidal stance period
|
||||
|
||||
:param phase: current trajectory phase
|
||||
:param L: step length
|
||||
:param lateral_fraction: determines how lateral the movement is
|
||||
:param penetration_depth: foot penetration depth during stance phase
|
||||
|
||||
:returns: X,Y,Z Foot Coordinates relative to unmodified body
|
||||
"""
|
||||
# moves from +L to -L
|
||||
step = L * (1.0 - 2.0 * phase)
|
||||
stepX = step * np.cos(lateral_fraction)
|
||||
stepY = step * np.sin(lateral_fraction)
|
||||
stepZ = 0.0
|
||||
if L != 0.0:
|
||||
stepZ = -penetration_depth * np.cos((np.pi * (stepX + stepY)) / (2.0 * L))
|
||||
|
||||
return stepX, stepY, stepZ
|
||||
|
||||
def yaw_circle(self, T_bf, index):
|
||||
""" Calculates the required rotation of the trajectory plane
|
||||
for yaw motion
|
||||
|
||||
:param T_bf: default body-to-foot Vector
|
||||
:param index: the foot index in the container
|
||||
:returns: phi_arc, the plane rotation angle required for yaw motion
|
||||
"""
|
||||
|
||||
# Foot Magnitude depending on leg type
|
||||
DefaultBodyToFoot_Magnitude = np.sqrt(T_bf[0]**2 + T_bf[1]**2)
|
||||
|
||||
# Rotation Angle depending on leg type
|
||||
DefaultBodyToFoot_Direction = np.arctan2(T_bf[1], T_bf[0])
|
||||
|
||||
# Previous leg coordinates relative to default coordinates
|
||||
g_xyz = self.prev_foot_pos[index] - np.array([T_bf[0], T_bf[1], T_bf[2]])
|
||||
|
||||
# Modulate Magnitude to keep tracing circle
|
||||
g_mag = np.sqrt((g_xyz[0])**2 + (g_xyz[1])**2)
|
||||
th_mod = np.arctan2(g_mag, DefaultBodyToFoot_Magnitude)
|
||||
|
||||
# Angle Traced by Foot for Rotation
|
||||
phi_arc = np.pi / 2.0 + th_mod
|
||||
phi_arc += DefaultBodyToFoot_Direction * 1 if index == 1 or index == 2 else -1
|
||||
|
||||
return phi_arc
|
||||
|
||||
def swing_step(self, phase, gaitState, T_bf, index):
|
||||
"""Calculates the step coordinates for the Bezier (swing) period
|
||||
using a combination of forward and rotational step coordinates
|
||||
initially decomposed from user input of
|
||||
L, lateral_fraction and yaw_rate
|
||||
|
||||
:param phase: current trajectory phase
|
||||
:param L: step length
|
||||
:param lateral_fraction: determines how lateral the movement is
|
||||
:param yaw_rate: the desired body yaw rate
|
||||
:param clearance_height: foot clearance height during swing phase
|
||||
:param T_bf: default body-to-foot Vector
|
||||
:param key: indicates which foot is being processed
|
||||
:param index: the foot index in the container
|
||||
|
||||
:returns: Foot Coordinates relative to unmodified body
|
||||
"""
|
||||
|
||||
# Yaw foot angle for tangent-to-circle motion
|
||||
phi_arc = self.yaw_circle(T_bf, index)
|
||||
|
||||
# Get Foot Coordinates for Forward Motion
|
||||
X_delta_lin, Y_delta_lin, Z_delta_lin = self._bezier_swing(
|
||||
phase,
|
||||
gaitState.step_length,
|
||||
gaitState.lateral_fraction,
|
||||
gaitState.clearance_height,
|
||||
)
|
||||
|
||||
X_delta_rot, Y_delta_rot, Z_delta_rot = self._bezier_swing(
|
||||
phase, gaitState.yaw_rate, phi_arc, gaitState.clearance_height
|
||||
)
|
||||
|
||||
coord = np.array(
|
||||
[
|
||||
X_delta_lin + X_delta_rot,
|
||||
Y_delta_lin + Y_delta_rot,
|
||||
Z_delta_lin + Z_delta_rot,
|
||||
]
|
||||
)
|
||||
|
||||
self.prev_foot_pos[index] = coord
|
||||
|
||||
return coord
|
||||
|
||||
def stance_step(self, phase, gaitState, T_bf, index):
|
||||
"""Calculates the step coordinates for the Sine (stance) period
|
||||
using a combination of forward and rotational step coordinates
|
||||
initially decomposed from user input of
|
||||
L, lateral_fraction and yaw_rate
|
||||
|
||||
:param phase: current trajectory phase
|
||||
:param gaitState: current gait state
|
||||
:param T_bf: default body-to-foot Vector
|
||||
:param index: the foot index in the container
|
||||
|
||||
:returns: Foot Coordinates relative to unmodified body
|
||||
"""
|
||||
|
||||
# Yaw foot angle for tangent-to-circle motion
|
||||
phi_arc = self.yaw_circle(T_bf, index)
|
||||
|
||||
# Get Foot Coordinates for Forward Motion
|
||||
X_delta_lin, Y_delta_lin, Z_delta_lin = self.sine_stance(
|
||||
phase,
|
||||
gaitState.step_length,
|
||||
gaitState.lateral_fraction,
|
||||
gaitState.penetration_depth,
|
||||
)
|
||||
|
||||
X_delta_rot, Y_delta_rot, Z_delta_rot = self.sine_stance(
|
||||
phase, gaitState.yaw_rate, phi_arc, gaitState.penetration_depth
|
||||
)
|
||||
|
||||
coord = np.array([
|
||||
X_delta_lin + X_delta_rot, Y_delta_lin + Y_delta_rot,
|
||||
Z_delta_lin + Z_delta_rot
|
||||
])
|
||||
|
||||
self.prev_foot_pos[index] = coord
|
||||
|
||||
return coord
|
||||
|
||||
def foot_step(self, gaitState, body_foot, index):
|
||||
"""Calculates the step coordinates in either the Bezier or
|
||||
Sine portion of the trajectory depending on the retrieved phase
|
||||
|
||||
:param T_bf: default body-to-foot Vector
|
||||
:param index: the foot index in the container
|
||||
|
||||
:returns: Foot Coordinates relative to unmodified body
|
||||
"""
|
||||
leg_phase, foot_phase = self.get_phase(index)
|
||||
stored_phase = leg_phase
|
||||
if foot_phase == Phase.SWING:
|
||||
stored_phase += 1.0
|
||||
|
||||
# Just for keeping track
|
||||
self.phases[index] = stored_phase
|
||||
if foot_phase == Phase.STANCE:
|
||||
return self.stance_step(leg_phase, gaitState, body_foot, index)
|
||||
elif foot_phase == Phase.SWING:
|
||||
return self.swing_step(leg_phase, gaitState, body_foot, index)
|
||||
|
||||
def generate_trajectory(self, bodyState, gaitState, dt):
|
||||
"""Calculates the step coordinates for each foot"""
|
||||
gaitState.yaw_rate *= dt
|
||||
|
||||
self.t_stance = 2.0 * abs(gaitState.step_length) / abs(gaitState.step_velocity)
|
||||
if gaitState.step_velocity == 0.0:
|
||||
self.t_stance = 0.0
|
||||
gaitState.step_length = 0.0
|
||||
self.touch_down = False
|
||||
self.time = 0.0
|
||||
self.last_touch_down_time = 0.0
|
||||
|
||||
# Catch infeasible timestep
|
||||
if self.t_stance < dt:
|
||||
self.t_stance = 0.0
|
||||
gaitState.step_length = 0.0
|
||||
self.touch_down = False
|
||||
self.time = 0.0
|
||||
self.last_touch_down_time = 0.0
|
||||
gaitState.yaw_rate = 0.0
|
||||
self.t_stance = min(self.t_stance, 1.3 * self.t_swing)
|
||||
|
||||
if gaitState.contacts[0] == 1 and self.t_stance > dt:
|
||||
self.touch_down = True
|
||||
|
||||
self.update_clock(dt)
|
||||
|
||||
ref_dS = {"FL": 0.0, "FR": 0.5, "BL": 0.5, "BR": 0.0}
|
||||
for i, (key, Tbf_in) in enumerate(bodyState.worldFeetPositions.items()):
|
||||
self.ref_idx = i if key == "FL" else self.ref_idx
|
||||
self.leg_phases[i] = ref_dS[key]
|
||||
_, leg_feet_positions = TransToRp(Tbf_in)
|
||||
step_coord = (
|
||||
self.foot_step(gaitState, leg_feet_positions, i)
|
||||
if self.t_stance > 0.0
|
||||
else np.array([0.0, 0.0, 0.0])
|
||||
)
|
||||
for j in range(3):
|
||||
bodyState.worldFeetPositions[key][j, 3] += step_coord[j]
|
||||
@@ -0,0 +1,97 @@
|
||||
#!/usr/bin/env python
|
||||
# https://www.researchgate.net/publication/320307716_Inverse_Kinematic_Analysis_Of_A_Quadruped_Robot
|
||||
import numpy as np
|
||||
|
||||
|
||||
class LegIK():
|
||||
def __init__(self,
|
||||
legtype="RIGHT",
|
||||
shoulder_length=0.04,
|
||||
elbow_length=0.1,
|
||||
wrist_length=0.125,
|
||||
hip_lim=[-0.548, 0.548],
|
||||
shoulder_lim=[-2.17, 0.97],
|
||||
leg_lim=[-0.1, 2.59]):
|
||||
self.legtype = legtype
|
||||
self.shoulder_length = shoulder_length
|
||||
self.elbow_length = elbow_length
|
||||
self.wrist_length = wrist_length
|
||||
self.hip_lim = hip_lim
|
||||
self.shoulder_lim = shoulder_lim
|
||||
self.leg_lim = leg_lim
|
||||
|
||||
def get_domain(self, x, y, z):
|
||||
"""
|
||||
Calculates the leg's Domain and caps it in case of a breach
|
||||
|
||||
:param x,y,z: hip-to-foot distances in each dimension
|
||||
:return: Leg Domain D
|
||||
"""
|
||||
D = (y**2 + (-z)**2 - self.shoulder_length**2 +
|
||||
(-x)**2 - self.elbow_length**2 - self.wrist_length**2) / (
|
||||
2 * self.wrist_length * self.elbow_length)
|
||||
if D > 1 or D < -1:
|
||||
# DOMAIN BREACHED
|
||||
# print("---------DOMAIN BREACH---------")
|
||||
D = np.clip(D, -1.0, 1.0)
|
||||
return D
|
||||
else:
|
||||
return D
|
||||
|
||||
def solve(self, xyz_coord):
|
||||
"""
|
||||
Generic Leg Inverse Kinematics Solver
|
||||
|
||||
:param xyz_coord: hip-to-foot distances in each dimension
|
||||
:return: Joint Angles required for desired position
|
||||
"""
|
||||
x = xyz_coord[0]
|
||||
y = xyz_coord[1]
|
||||
z = xyz_coord[2]
|
||||
D = self.get_domain(x, y, z)
|
||||
if self.legtype == "RIGHT":
|
||||
return self.RightIK(x, y, z, D)
|
||||
else:
|
||||
return self.LeftIK(x, y, z, D)
|
||||
|
||||
def RightIK(self, x, y, z, D):
|
||||
"""
|
||||
Right Leg Inverse Kinematics Solver
|
||||
|
||||
:param x,y,z: hip-to-foot distances in each dimension
|
||||
:param D: leg domain
|
||||
:return: Joint Angles required for desired position
|
||||
"""
|
||||
wrist_angle = np.arctan2(-np.sqrt(1 - D**2), D)
|
||||
sqrt_component = y**2 + (-z)**2 - self.shoulder_length**2
|
||||
if sqrt_component < 0.0:
|
||||
# print("NEGATIVE SQRT")
|
||||
sqrt_component = 0.0
|
||||
shoulder_angle = -np.arctan2(z, y) - np.arctan2(
|
||||
np.sqrt(sqrt_component), -self.shoulder_length)
|
||||
elbow_angle = np.arctan2(-x, np.sqrt(sqrt_component)) - np.arctan2(
|
||||
self.wrist_length * np.sin(wrist_angle),
|
||||
self.elbow_length + self.wrist_length * np.cos(wrist_angle))
|
||||
joint_angles = np.array([-shoulder_angle, elbow_angle, wrist_angle])
|
||||
return joint_angles
|
||||
|
||||
def LeftIK(self, x, y, z, D):
|
||||
"""
|
||||
Left Leg Inverse Kinematics Solver
|
||||
|
||||
:param x,y,z: hip-to-foot distances in each dimension
|
||||
:param D: leg domain
|
||||
:return: Joint Angles required for desired position
|
||||
"""
|
||||
wrist_angle = np.arctan2(-np.sqrt(1 - D**2), D)
|
||||
sqrt_component = y**2 + (-z)**2 - self.shoulder_length**2
|
||||
if sqrt_component < 0.0:
|
||||
print("NEGATIVE SQRT")
|
||||
sqrt_component = 0.0
|
||||
shoulder_angle = -np.arctan2(z, y) - np.arctan2(
|
||||
np.sqrt(sqrt_component), self.shoulder_length)
|
||||
elbow_angle = np.arctan2(-x, np.sqrt(sqrt_component)) - np.arctan2(
|
||||
self.wrist_length * np.sin(wrist_angle),
|
||||
self.elbow_length + self.wrist_length * np.cos(wrist_angle))
|
||||
joint_angles = np.array([-shoulder_angle, elbow_angle, wrist_angle])
|
||||
return joint_angles
|
||||
@@ -0,0 +1,182 @@
|
||||
#!/usr/bin/env python
|
||||
import numpy as np
|
||||
|
||||
# NOTE: Code snippets from Modern Robotics at Northwestern University:
|
||||
# See https://github.com/NxRLab/ModernRobotics
|
||||
|
||||
|
||||
def RpToTrans(R, p):
|
||||
"""
|
||||
Converts a rotation matrix and a position vector into homogeneous
|
||||
transformation matrix
|
||||
|
||||
:param R: A 3x3 rotation matrix
|
||||
:param p: A 3-vector
|
||||
:return: A homogeneous transformation matrix corresponding to the inputs
|
||||
|
||||
Example Input:
|
||||
R = np.array([[1, 0, 0],
|
||||
[0, 0, -1],
|
||||
[0, 1, 0]])
|
||||
p = np.array([1, 2, 5])
|
||||
|
||||
Output:
|
||||
np.array([[1, 0, 0, 1],
|
||||
[0, 0, -1, 2],
|
||||
[0, 1, 0, 5],
|
||||
[0, 0, 0, 1]])
|
||||
"""
|
||||
return np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
|
||||
|
||||
|
||||
def TransToRp(T):
|
||||
"""
|
||||
Converts a homogeneous transformation matrix into a rotation matrix
|
||||
and position vector
|
||||
|
||||
:param T: A homogeneous transformation matrix
|
||||
:return R: The corresponding rotation matrix,
|
||||
:return p: The corresponding position vector.
|
||||
|
||||
Example Input:
|
||||
T = np.array([[1, 0, 0, 0],
|
||||
[0, 0, -1, 0],
|
||||
[0, 1, 0, 3],
|
||||
[0, 0, 0, 1]])
|
||||
|
||||
Output:
|
||||
(np.array([[1, 0, 0],
|
||||
[0, 0, -1],
|
||||
[0, 1, 0]]),
|
||||
np.array([0, 0, 3]))
|
||||
"""
|
||||
T = np.array(T)
|
||||
return T[0:3, 0:3], T[0:3, 3]
|
||||
|
||||
|
||||
def TransInv(T):
|
||||
"""
|
||||
Inverts a homogeneous transformation matrix
|
||||
|
||||
:param T: A homogeneous transformation matrix
|
||||
:return: The inverse of T
|
||||
Uses the structure of transformation matrices to avoid taking a matrix
|
||||
inverse, for efficiency.
|
||||
|
||||
Example input:
|
||||
T = np.array([[1, 0, 0, 0],
|
||||
[0, 0, -1, 0],
|
||||
[0, 1, 0, 3],
|
||||
[0, 0, 0, 1]])
|
||||
Output:
|
||||
np.array([[1, 0, 0, 0],
|
||||
[0, 0, 1, -3],
|
||||
[0, -1, 0, 0],
|
||||
[0, 0, 0, 1]])
|
||||
"""
|
||||
R, p = TransToRp(T)
|
||||
Rt = np.array(R).T
|
||||
return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]]
|
||||
|
||||
|
||||
def Adjoint(T):
|
||||
"""
|
||||
Computes the adjoint representation of a homogeneous transformation
|
||||
matrix
|
||||
|
||||
:param T: A homogeneous transformation matrix
|
||||
:return: The 6x6 adjoint representation [AdT] of T
|
||||
|
||||
Example Input:
|
||||
T = np.array([[1, 0, 0, 0],
|
||||
[0, 0, -1, 0],
|
||||
[0, 1, 0, 3],
|
||||
[0, 0, 0, 1]])
|
||||
Output:
|
||||
np.array([[1, 0, 0, 0, 0, 0],
|
||||
[0, 0, -1, 0, 0, 0],
|
||||
[0, 1, 0, 0, 0, 0],
|
||||
[0, 0, 3, 1, 0, 0],
|
||||
[3, 0, 0, 0, 0, -1],
|
||||
[0, 0, 0, 0, 1, 0]])
|
||||
"""
|
||||
R, p = TransToRp(T)
|
||||
return np.r_[np.c_[R, np.zeros((3, 3))], np.c_[np.dot(VecToso3(p), R), R]]
|
||||
|
||||
|
||||
def VecToso3(omg):
|
||||
"""
|
||||
Converts a 3-vector to an so(3) representation
|
||||
|
||||
:param omg: A 3-vector
|
||||
:return: The skew symmetric representation of omg
|
||||
|
||||
Example Input:
|
||||
omg = np.array([1, 2, 3])
|
||||
Output:
|
||||
np.array([[ 0, -3, 2],
|
||||
[ 3, 0, -1],
|
||||
[-2, 1, 0]])
|
||||
"""
|
||||
return np.array([[0, -omg[2], omg[1]], [omg[2], 0, -omg[0]],
|
||||
[-omg[1], omg[0], 0]])
|
||||
|
||||
|
||||
def RPY(roll, pitch, yaw):
|
||||
"""
|
||||
Creates a Roll, Pitch, Yaw Transformation Matrix
|
||||
|
||||
:param roll: roll component of matrix
|
||||
:param pitch: pitch component of matrix
|
||||
:param yaw: yaw component of matrix
|
||||
:return: The transformation matrix
|
||||
|
||||
Example Input:
|
||||
roll = 0.0
|
||||
pitch = 0.0
|
||||
yaw = 0.0
|
||||
Output:
|
||||
np.array([[1, 0, 0, 0],
|
||||
[0, 1, 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1]])
|
||||
"""
|
||||
Roll = np.array([[1, 0, 0, 0], [0, np.cos(roll), -np.sin(roll), 0],
|
||||
[0, np.sin(roll), np.cos(roll), 0], [0, 0, 0, 1]])
|
||||
Pitch = np.array([[np.cos(pitch), 0, np.sin(pitch), 0], [0, 1, 0, 0],
|
||||
[-np.sin(pitch), 0, np.cos(pitch), 0], [0, 0, 0, 1]])
|
||||
Yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0, 0],
|
||||
[np.sin(yaw), np.cos(yaw), 0, 0], [0, 0, 1, 0],
|
||||
[0, 0, 0, 1]])
|
||||
return np.matmul(np.matmul(Roll, Pitch), Yaw)
|
||||
|
||||
|
||||
def RotateTranslate(rotation, position):
|
||||
"""
|
||||
Creates a Transformation Matrix from a Rotation, THEN, a Translation
|
||||
|
||||
:param rotation: pure rotation matrix
|
||||
:param translation: pure translation matrix
|
||||
:return: The transformation matrix
|
||||
"""
|
||||
trans = np.eye(4)
|
||||
trans[0, 3] = position[0]
|
||||
trans[1, 3] = position[1]
|
||||
trans[2, 3] = position[2]
|
||||
|
||||
return np.dot(rotation, trans)
|
||||
|
||||
|
||||
def TransformVector(xyz_coord, rotation, translation):
|
||||
"""
|
||||
Transforms a vector by a specified Rotation THEN Translation Matrix
|
||||
|
||||
:param xyz_coord: the vector to transform
|
||||
:param rotation: pure rotation matrix
|
||||
:param translation: pure translation matrix
|
||||
:return: The transformed vector
|
||||
"""
|
||||
xyz_vec = np.append(xyz_coord, 1.0)
|
||||
|
||||
Transformed = np.dot(RotateTranslate(rotation, translation), xyz_vec)
|
||||
return Transformed[:3]
|
||||
@@ -0,0 +1,224 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import numpy as np
|
||||
from .LegKinematics import LegIK
|
||||
from .LieAlgebra import RpToTrans, TransToRp, TransInv, RPY, TransformVector
|
||||
from collections import OrderedDict
|
||||
|
||||
|
||||
class SpotModel:
|
||||
def __init__(
|
||||
self,
|
||||
shoulder_length=0.055,
|
||||
elbow_length=0.10652,
|
||||
wrist_length=0.145,
|
||||
hip_x=0.23,
|
||||
hip_y=0.075,
|
||||
foot_x=0.23,
|
||||
foot_y=0.185,
|
||||
height=0.20,
|
||||
com_offset=0.016,
|
||||
shoulder_lim=[-0.548, 0.548],
|
||||
elbow_lim=[-2.17, 0.97],
|
||||
wrist_lim=[-0.1, 2.59],
|
||||
):
|
||||
"""
|
||||
Spot Micro Kinematics
|
||||
"""
|
||||
# COM offset in x direction
|
||||
self.com_offset = com_offset
|
||||
|
||||
# Leg Parameters
|
||||
self.shoulder_length = shoulder_length
|
||||
self.elbow_length = elbow_length
|
||||
self.wrist_length = wrist_length
|
||||
|
||||
# Leg Vector desired_positions
|
||||
|
||||
# Distance Between Hips
|
||||
# Length
|
||||
self.hip_x = hip_x
|
||||
# Width
|
||||
self.hip_y = hip_y
|
||||
|
||||
# Distance Between Feet
|
||||
# Length
|
||||
self.foot_x = foot_x
|
||||
# Width
|
||||
self.foot_y = foot_y
|
||||
|
||||
# Body Height
|
||||
self.height = height
|
||||
|
||||
# Joint Parameters
|
||||
self.shoulder_lim = shoulder_lim
|
||||
self.elbow_lim = elbow_lim
|
||||
self.wrist_lim = wrist_lim
|
||||
|
||||
# Dictionary to store Leg IK Solvers
|
||||
self.Legs = OrderedDict()
|
||||
self.Legs["FL"] = LegIK(
|
||||
"LEFT",
|
||||
self.shoulder_length,
|
||||
self.elbow_length,
|
||||
self.wrist_length,
|
||||
self.shoulder_lim,
|
||||
self.elbow_lim,
|
||||
self.wrist_lim,
|
||||
)
|
||||
self.Legs["FR"] = LegIK(
|
||||
"RIGHT",
|
||||
self.shoulder_length,
|
||||
self.elbow_length,
|
||||
self.wrist_length,
|
||||
self.shoulder_lim,
|
||||
self.elbow_lim,
|
||||
self.wrist_lim,
|
||||
)
|
||||
self.Legs["BL"] = LegIK(
|
||||
"LEFT",
|
||||
self.shoulder_length,
|
||||
self.elbow_length,
|
||||
self.wrist_length,
|
||||
self.shoulder_lim,
|
||||
self.elbow_lim,
|
||||
self.wrist_lim,
|
||||
)
|
||||
self.Legs["BR"] = LegIK(
|
||||
"RIGHT",
|
||||
self.shoulder_length,
|
||||
self.elbow_length,
|
||||
self.wrist_length,
|
||||
self.shoulder_lim,
|
||||
self.elbow_lim,
|
||||
self.wrist_lim,
|
||||
)
|
||||
|
||||
# Dictionary to store Hip and Foot Transforms
|
||||
|
||||
# Transform of Hip relative to world frame
|
||||
# With Body Centroid also in world frame
|
||||
Rwb = np.eye(3)
|
||||
self.WorldToHip = OrderedDict()
|
||||
|
||||
self.ph_FL = np.array([self.hip_x / 2.0, self.hip_y / 2.0, 0])
|
||||
self.WorldToHip["FL"] = RpToTrans(Rwb, self.ph_FL)
|
||||
|
||||
self.ph_FR = np.array([self.hip_x / 2.0, -self.hip_y / 2.0, 0])
|
||||
self.WorldToHip["FR"] = RpToTrans(Rwb, self.ph_FR)
|
||||
|
||||
self.ph_BL = np.array([-self.hip_x / 2.0, self.hip_y / 2.0, 0])
|
||||
self.WorldToHip["BL"] = RpToTrans(Rwb, self.ph_BL)
|
||||
|
||||
self.ph_BR = np.array([-self.hip_x / 2.0, -self.hip_y / 2.0, 0])
|
||||
self.WorldToHip["BR"] = RpToTrans(Rwb, self.ph_BR)
|
||||
|
||||
# Transform of Foot relative to world frame
|
||||
# With Body Centroid also in world frame
|
||||
self.WorldToFoot = OrderedDict()
|
||||
|
||||
self.pf_FL = np.array([self.foot_x / 2.0, self.foot_y / 2.0, -self.height])
|
||||
self.WorldToFoot["FL"] = RpToTrans(Rwb, self.pf_FL)
|
||||
|
||||
self.pf_FR = np.array([self.foot_x / 2.0, -self.foot_y / 2.0, -self.height])
|
||||
self.WorldToFoot["FR"] = RpToTrans(Rwb, self.pf_FR)
|
||||
|
||||
self.pf_BL = np.array([-self.foot_x / 2.0, self.foot_y / 2.0, -self.height])
|
||||
self.WorldToFoot["BL"] = RpToTrans(Rwb, self.pf_BL)
|
||||
|
||||
self.pf_BR = np.array([-self.foot_x / 2.0, -self.foot_y / 2.0, -self.height])
|
||||
self.WorldToFoot["BR"] = RpToTrans(Rwb, self.pf_BR)
|
||||
|
||||
def HipToFoot(self, orn, pos, T_bf):
|
||||
"""
|
||||
Converts a desired position and orientation wrt Spot's
|
||||
home position, with a desired body-to-foot Transform
|
||||
into a body-to-hip Transform, which is used to extract
|
||||
and return the Hip To Foot Vector.
|
||||
|
||||
:param orn: A 3x1 np.array([]) with Spot's Roll, Pitch, Yaw angles
|
||||
:param pos: A 3x1 np.array([]) with Spot's X, Y, Z coordinates
|
||||
:param T_bf: Dictionary of desired body-to-foot Transforms.
|
||||
:return: Hip To Foot Vector for each of Spot's Legs.
|
||||
"""
|
||||
|
||||
# Following steps in attached document: SpotBodyIK.
|
||||
# TODO: LINK DOC
|
||||
|
||||
# Only get Rot component
|
||||
Rb, _ = TransToRp(RPY(orn[0], orn[1], orn[2]))
|
||||
pb = pos
|
||||
T_wb = RpToTrans(Rb, pb)
|
||||
|
||||
# Dictionary to store vectors
|
||||
HipToFoot_List = OrderedDict()
|
||||
|
||||
for i, (key, T_wh) in enumerate(self.WorldToHip.items()):
|
||||
# ORDER: FL, FR, FR, BL, BR
|
||||
|
||||
# Extract vector component
|
||||
_, p_bf = TransToRp(T_bf[key])
|
||||
|
||||
# Step 1, get T_bh for each leg
|
||||
T_bh = np.dot(TransInv(T_wb), T_wh)
|
||||
|
||||
# Step 2, get T_hf for each leg
|
||||
|
||||
# VECTOR ADDITION METHOD
|
||||
_, p_bh = TransToRp(T_bh)
|
||||
p_hf0 = p_bf - p_bh
|
||||
|
||||
# TRANSFORM METHOD
|
||||
T_hf = np.dot(TransInv(T_bh), T_bf[key])
|
||||
_, p_hf1 = TransToRp(T_hf)
|
||||
|
||||
# They should yield the same result
|
||||
if p_hf1.all() != p_hf0.all():
|
||||
print("NOT EQUAL")
|
||||
|
||||
p_hf = p_hf1
|
||||
|
||||
HipToFoot_List[key] = p_hf
|
||||
|
||||
return HipToFoot_List
|
||||
|
||||
def IK(self, orn, pos, T_bf):
|
||||
"""
|
||||
Uses HipToFoot() to convert a desired position
|
||||
and orientation wrt Spot's home position into a
|
||||
Hip To Foot Vector, which is fed into the LegIK solver.
|
||||
|
||||
Finally, the resultant joint angles are returned
|
||||
from the LegIK solver for each leg.
|
||||
|
||||
:param orn: A 3x1 np.array([]) with Spot's Roll, Pitch, Yaw angles
|
||||
:param pos: A 3x1 np.array([]) with Spot's X, Y, Z coordinates
|
||||
:param T_bf: Dictionary of desired body-to-foot Transforms.
|
||||
:return: Joint angles for each of Spot's joints.
|
||||
"""
|
||||
|
||||
# Following steps in attached document: SpotBodyIK.
|
||||
# TODO: LINK DOC
|
||||
|
||||
# Modify x by com offset
|
||||
pos[0] += self.com_offset
|
||||
|
||||
# 4 legs, 3 joints per leg
|
||||
joint_angles = np.zeros((4, 3))
|
||||
|
||||
# print("T_bf: {}".format(T_bf))
|
||||
|
||||
# Steps 1 and 2 of pipeline here
|
||||
HipToFoot = self.HipToFoot(orn, pos, T_bf)
|
||||
|
||||
for i, (key, p_hf) in enumerate(HipToFoot.items()):
|
||||
# ORDER: FL, FR, FR, BL, BR
|
||||
|
||||
# print("LEG: {} \t HipToFoot: {}".format(key, p_hf))
|
||||
|
||||
# Step 3, compute joint angles from T_hf for each leg
|
||||
joint_angles[i, :] = self.Legs[key].solve(p_hf)
|
||||
|
||||
# print("-----------------------------")
|
||||
|
||||
return joint_angles
|
||||
@@ -0,0 +1,5 @@
|
||||
|
||||
|
||||
class Spot:
|
||||
def __init__(self) -> None:
|
||||
pass
|
||||
Reference in New Issue
Block a user