Example: Kinematics
[1]:
'''
%pip install nullspace
'''
[1]:
'\n%pip install nullspace\n'
[2]:
from sympy import sin, cos, eye
from sympy import symbols, simplify, nsimplify
from sympy import Array, Matrix, ZeroMatrix, BlockMatrix
import sympy as sp
import numpy as np
from nullspace.rbda import Rx, Ry, Rz
from nullspace.se3 import hat, Ex, Ey, Ez, SXForm, getE, getr, perfect_Ez_pi
from nullspace import ensure_positive_Z, autodiff_svd, svd_regulated
[3]:
from nullspace.models.structure import q_sym
from nullspace.models.structure import robotBuilder
o = robotBuilder('structure.yaml')
abad0 = o[0][0]
abad1 = o[1][0]
abad2 = o[2][0]
abad3 = o[3][0]
hip0 = o[0][1]
hip1 = o[1][1]
hip2 = o[2][1]
hip3 = o[3][1]
knee0 = o[0][2]
knee1 = o[1][2]
knee2 = o[2][2]
knee3 = o[3][2]
foot0 = o[0][3]
foot1 = o[1][3]
foot2 = o[2][3]
foot3 = o[3][3]
[4]:
# Control variables
'''
Top view of distance vectors.
v3
1<-------0
|\ /|
| \ v2/ |
| \ / |
| \/ |
v1| /\ |v4
| / \ |
| / v0\ |
vv vv
3<-------2
v5
'''
v0 = foot2 - foot1
v1 = foot3 - foot1
v2 = foot3 - foot0
v3 = foot1 - foot0
v4 = foot2 - foot0
v5 = foot3 - foot2
# 6 distances between each foots
s0 = v0.transpose() * v0
s1 = v1.transpose() * v1
s2 = v2.transpose() * v2
s3 = v3.transpose() * v3
s4 = v4.transpose() * v4
s5 = v5.transpose() * v5
dist = Matrix([s0, s1, s2, s3, s4, s5])
n = v5.cross(v1)
cen = (foot0 + foot1 + foot2 + foot3) / 4
A = Matrix([foot0 - cen, foot1 - cen, foot2 - cen, foot3 - cen]).reshape(4,3).T
var = Matrix(q_sym)
[5]:
# Jacobian of control variables
jc = dist.jacobian(var)
# For support_normal_option = 'cross'
j_ori = Matrix([n, v5]).jacobian(var)
# This format matches the frist two columns of normalized orthogonal basis of orientation.
# The 3rd/last column isn't needed here since there are only 6 independent vars and other
# 3 could be derived.
# For support_normal_option = 'svd'
j_ori_tail3 = v5.jacobian(var)
# j_ori_head3 will be achieved by autodiff_svd directly at specific q_star,
# there isn't analytical jacobian expression for derivative of normal vector if
# normal vector is calculated by best fitting from SVD.
j_pos = cen.jacobian(var)
#jc = jc.subs(links_params)
#j_ori = j_ori.subs(links_params)
#j_pos = j_pos.subs(links_params)
[6]:
# FourLegRobot and ThreeLinkArm
%matplotlib notebook
%matplotlib notebook
# DON'T DELETE, This isn't redundant in my knowledge of 2020
from math import cos, sin, pi
import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d.art3d as art3d
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from matplotlib.patches import Circle, PathPatch, Rectangle, Polygon
import time
R_floor = np.array([[ 0.8660254, 0. , -0.5 ],
[ 0. , 1. , 0. ],
[ 0.5 , 0. , 0.8660254]])
# R_floor = Ry(-pi/6) @ Rx(pi/6)
Pos_floor = np.array([0.2, 0.1, 0.3])
class ThreeLinkArm:
def __init__(self, joint_angles, num, A, H, K, F):
'''
joint_angles:
The 3d vector of abad, hip and knee joint value.
num:
The number of iteration. This number is used to
control the alpha of Arm plot.
A, H, K and F: sympy objs
Each of them could be thought as a mathematical
function which takes 3 joints value and return
abad, hip, knee and foot positions.
'''
self.body_ax = None
# ax is dependent injection
self.num = num
self.abad = np.array([i for i in A], dtype=float)
self.H = H
self.K = K
self.F = F
def forward_kinematics(self, name):
if name == 0:
d = {q_sym[0]: self.joint_angles[0],
q_sym[1]: self.joint_angles[1],
q_sym[2]: self.joint_angles[2]}
elif name == 1:
d = {q_sym[3]: self.joint_angles[0],
q_sym[4]: self.joint_angles[1],
q_sym[5]: self.joint_angles[2]}
elif name == 2:
d = {q_sym[6]: self.joint_angles[0],
q_sym[7]: self.joint_angles[1],
q_sym[8]: self.joint_angles[2]}
else:
d = {q_sym[9]: self.joint_angles[0],
q_sym[10]: self.joint_angles[1],
q_sym[11]: self.joint_angles[2]}
self.hip = np.array([i for i in self.H.subs(d)], dtype=float)
self.knee = np.array([i for i in self.K.subs(d)], dtype=float)
self.foot = np.array([i for i in self.F.subs(d)], dtype=float)
def plot(self, i, abad_color, hip_color, knee_color):
# plot one leg. index 0, 1 and 2 are meaning x, y, z composite here
# plot abad - hip arm
self.body_ax.plot([self.abad[0], self.hip[0]],
[self.abad[1], self.hip[1]],
[self.abad[2], self.hip[2]],
color = abad_color)
# plot hip - knee
self.body_ax.plot([self.hip[0], self.knee[0]],
[self.hip[1], self.knee[1]],
[self.hip[2], self.knee[2]],
color = hip_color)
# plot knee - foot
self.body_ax.plot([self.knee[0], self.foot[0]],
[self.knee[1], self.foot[1]],
[self.knee[2], self.foot[2]],
color = knee_color)
if i == num - 1:
self.body_ax.plot(self.foot[0],self.foot[1], self.foot[2],'b')
class FourLegRobot:
def __init__(self, zeroth, first, second, third, num, support_normal_option):
self.num = num
self.it = None
# Ori
# Support and Body
self.support_normal_option = support_normal_option
self.EdgeY_SupportOnBody_Target = None
self.EdgeY_SupportOnBody_Current = None
self.Normal_SupportOnBody_Target = None
self.Normal_SupportOnBody_Current = None
self.Ori_SupportOnBody_Target = None
self.Ori_SupportOnBody_Current = None
self.Ori_SupportOnBody_Error = np.array([0] * 6, dtype = float)
self.Ori_BodyOnSupport_Target = None
self.Ori_BodyOnSupport_Current = None
self.Ori_SupportOnHorizon_Current = None # R0, Estimation of Floor Ori
# Body and Horizon
# This is INPUT target of ori which in Horioin Coordinate
self.Ori_BodyOnHorizon_Target = None
self.Ori_BodyOnHorizon_Current = None
self.IMU_BodyOnHorizon_Sim = None # A alias of self.Ori_BodyOnHorizon_Current
# Pos
# This is INPUT target of pos which is calibrated to Horizontal Support Coordinate
# to make it match human visual feelings and dynamics.
# This is HorizontalSupport not Horizon since we don't no/don't need to know
# where the robot is. It might be on Everest or someone's home.
self.Pos_BodyOnHorizontalSupport_Target = None
self.Pos_BodyOnSupport_Target = None
self.Pos_SupportOnBody_Target = None
self.Pos_SupportCentroidOnBody_Current = None
self.Pos_SupportOnBody_Error = None
self.arms = [None, None, None, None]
self.arms[0] = zeroth
self.arms[1] = first
self.arms[2] = second
self.arms[3] = third
self.body_ax = plt.subplot(3, 1, 1, projection='3d')
self.support_ax = plt.subplot(3, 1, 2, projection='3d')
self.horizon_ax = plt.subplot(3, 1, 3, projection='3d')
start_point = self.arms[2].abad[:2]
self.body_length = self.arms[1].abad[0] * 2
self.body_width = self.arms[1].abad[1] * 2
rec = Rectangle(start_point,
self.body_length,
self.body_width,
angle=0.0,
color=(0.1,0.1,0.1),
alpha = 0.2)
# r: Base body which is simplified to a rectangle
self.body_ax.add_patch(rec)
art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z")
self.body_ax.set_xlim(-0.3, 0.3)
self.body_ax.set_ylim(-0.3, 0.3)
self.body_ax.set_zlim(-0.4, 0.2)
self.support_ax.set_xlim(-0.3, 0.3)
self.support_ax.set_ylim(-0.3, 0.3)
self.support_ax.set_zlim(-0.2, 0.4)
self.horizon_ax.set_xlim(-0.3, 0.3)
self.horizon_ax.set_ylim(-0.3, 0.3)
self.horizon_ax.set_zlim(0, 0.6)
plt.gca().set_aspect("auto")
self.arms[0].body_ax = self.body_ax
self.arms[1].body_ax = self.body_ax
self.arms[2].body_ax = self.body_ax
self.arms[3].body_ax = self.body_ax
def set_Ori_BodyOnSupport_Target_byRotation(self, R):
'''
Set Ori by Rotation matrix of Body relative to support area.
'''
self.Ori_BodyOnSupport_Target = R
self.Ori_SupportOnBody_Target = self.Ori_BodyOnSupport_Target.T
self.Normal_SupportOnBody_Target = self.Ori_SupportOnBody_Target[:, 2]
self.EdgeY_SupportOnBody_Target = self.Ori_SupportOnBody_Target[:, 1]
def set_Ori_BodyOnSupport_byYZ(self, yz):
col1 = yz[:3]
col2 = yz[3:]
col1 = col1 / np.linalg.norm(col1)
col2 = col2 / np.linalg.norm(col2)
col0 = np.cross(col1, col2)
R = np.column_stack((col0, col1, col2))
self.set_Ori_BodyOnSupport_Target_byRotation(R)
def set_Ori_BodyOnSupport_Target_byRPY(self, roll, pitch, yaw):
R = Rz(yaw) @ Ry(pitch) @ Rx(roll)
self.set_Ori_BodyOnSupport_Target_byRotation(R)
def set_Ori_BodyOnHorizon_Target_byRPY(self, roll, pitch, yaw):
self.Ori_BodyOnHorizon_Target = Rz(yaw) @ Ry(pitch) @ Rx(roll)
def set_Pos_SupportOnBody_Target(self, Pos_BodyOnSupport_Target):
self.Pos_SupportOnBody_Target = -(self.Ori_SupportOnBody_Target
@ Pos_BodyOnSupport_Target)
def set_Ori_SupportOnBody_Current(self, edgeY, normal):
x = np.cross(edgeY, normal)
self.Ori_SupportOnBody_Current = np.column_stack((x, edgeY, normal))
def get_edgeY(self):
'''
Top View
1 --- 0
| |
| |
| |
3 --- 2
Get the edge vector along Y-ish direction inside the best-fit plane.
The edge vector start point is the projection point of middle point
between foot 0 and foot 2 on best-fit plane.
The edge vector end point is the projection point of middle point
between foot 1 and foot 3 on best-fit plane.
Returns
-------
vec : np.array
edgeY-ish vector
'''
if self.support_normal_option == 'svd':
middle_point_bt13 = (self.arms[1].foot + self.arms[3].foot) / 2
mp13_projection = self.get_point_projection_on_plane(self.Pos_SupportCentroidOnBody_Current,
self.Normal_SupportOnBody_Current,
middle_point_bt13)
middle_point_bt02 = (self.arms[0].foot + self.arms[2].foot) / 2
mp02_projection = self.get_point_projection_on_plane(self.Pos_SupportCentroidOnBody_Current,
self.Normal_SupportOnBody_Current,
middle_point_bt02)
vec = mp13_projection - mp02_projection
else:
vec = self.arms[3].foot - self.arms[2].foot
return vec
def get_point_projection_on_plane(self, c, n, p):
'''
Calculate the point coordinate on certain plane.
Parameters
----------
c : numpy array
Plane is discribed by a point and normal vector.
c is the point of this discription.
n : numpy array
Plane is discribed by a point and normal vector.
n is the normal vector of this discription.
p : numpy array
The target point in space that need to be projected
on plane.
Returns
-------
res : numpy array
Point coordinate on plane.
'''
t = sum(n * c) - sum(n * p) / sum(n * n)
# * is element-wise here.
res = p + t * n
return res
def update_joints(self, q_values):
self.arms[0].joint_angles = q[0:3]
self.arms[1].joint_angles = q[3:6]
self.arms[2].joint_angles = q[6:9]
self.arms[3].joint_angles = q[9:12]
self.arms[0].forward_kinematics(0)
self.arms[1].forward_kinematics(1)
self.arms[2].forward_kinematics(2)
self.arms[3].forward_kinematics(3)
def prepare_plot(self):
progress = self.it / self.num
self.base_color = (0.8 - 0.7 * progress,
0.8 - 0.7 * progress,
0.8 - 0.7 * progress,
0.7)
self.abad_color = (0.8 - 0.7 * progress,
0.8 - 0.7 * progress,
0.8 - 0.7 * progress,
0.7)
self.hip_color = (0.7 - 0.7 * progress,
0.8 - 0.7 * progress,
0.9 - 0.7 * progress,
0.7)
self.knee_color = (0.9 - 0.5 * progress,
0.8 - 0.5 * progress,
0.7 - 0.5 * progress,
0.7)
self.alpha = (self.it + 1) / self.num
self.support_color = (0.745, 0.458, 0.941) # Purple
def plot_on_body(self):
# Plot each arms
self.arms[0].plot(self.it, self.abad_color, self.hip_color, self.knee_color)
self.arms[1].plot(self.it, self.abad_color, self.hip_color, self.knee_color)
self.arms[2].plot(self.it, self.abad_color, self.hip_color, self.knee_color)
self.arms[3].plot(self.it, self.abad_color, self.hip_color, self.knee_color)
# Plot quadrilateral supporting area in purple color("0.745, 0.458, 0.941")
self.body_ax.plot([self.arms[1].foot[0], self.arms[0].foot[0]],
[self.arms[1].foot[1], self.arms[0].foot[1]],
[self.arms[1].foot[2], self.arms[0].foot[2]],
color=self.support_color,
alpha=self.alpha)
self.body_ax.plot([self.arms[3].foot[0], self.arms[1].foot[0]],
[self.arms[3].foot[1], self.arms[1].foot[1]],
[self.arms[3].foot[2], self.arms[1].foot[2]],
color=self.support_color,
alpha=self.alpha)
self.body_ax.plot([self.arms[3].foot[0], self.arms[2].foot[0]],
[self.arms[3].foot[1], self.arms[2].foot[1]],
[self.arms[3].foot[2], self.arms[2].foot[2]],
color=self.support_color,
alpha=self.alpha)
self.body_ax.plot([self.arms[0].foot[0], self.arms[2].foot[0]],
[self.arms[0].foot[1], self.arms[2].foot[1]],
[self.arms[0].foot[2], self.arms[2].foot[2]],
color=self.support_color,
alpha=self.alpha)
def plot_on_support_each_arm(self, arm):
'''
Calculate each joints location of one leg under horizon coordinate.
There are 4 joints and 3 links.
The location of abad joint is identical with node location on base body.
'''
pos = self.Pos_SupportCentroidOnBody_Current
R = self.Ori_SupportOnBody_Current
abad_on_support = R.T @ (arm.abad - pos)
hip_on_support = R.T @ (arm.hip - pos)
knee_on_support = R.T @ (arm.knee - pos)
foot_on_support = R.T @ (arm.foot - pos)
self.support_ax.plot([abad_on_support[0], hip_on_support[0]],
[abad_on_support[1], hip_on_support[1]],
[abad_on_support[2], hip_on_support[2]],
color=self.abad_color)
self.support_ax.plot([hip_on_support[0], knee_on_support[0]],
[hip_on_support[1], knee_on_support[1]],
[hip_on_support[2], knee_on_support[2]],
color=self.hip_color)
self.support_ax.plot([knee_on_support[0], foot_on_support[0]],
[knee_on_support[1], foot_on_support[1]],
[knee_on_support[2], foot_on_support[2]],
color=self.knee_color)
def plot_on_support(self):
'''
Plot on support area coordinate with Centroid as origin.
'''
pos = self.Pos_SupportCentroidOnBody_Current
R = self.Ori_SupportOnBody_Current
# Calculate base body rectangle, 4 nodes, location on support coordinate
s0 = R.T @ (self.arms[0].abad - pos)
s1 = R.T @ (self.arms[1].abad - pos)
s2 = R.T @ (self.arms[2].abad - pos)
s3 = R.T @ (self.arms[3].abad - pos)
# Plot base body on support_ax
self.support_ax.plot([s0[0], s1[0]],
[s0[1], s1[1]],
[s0[2], s1[2]],
color=self.base_color)
self.support_ax.plot([s0[0], s2[0]],
[s0[1], s2[1]],
[s0[2], s2[2]],
color=self.base_color)
self.support_ax.plot([s3[0], s1[0]],
[s3[1], s1[1]],
[s3[2], s1[2]],
color=self.base_color)
self.support_ax.plot([s3[0], s2[0]],
[s3[1], s2[1]],
[s3[2], s2[2]],
color=self.base_color)
# Plot 4 legs
for i in range(4):
self.plot_on_support_each_arm(self.arms[i])
def plot_on_horizon_each_arm(self, arm):
'''
Calculate each joints location of one leg under horizon coordinate.
There are 4 joints and 3 links.
The location of abad joint is identical with node location on base body.
'''
pos = self.Pos_SupportCentroidOnBody_Current
R = self.Ori_SupportOnBody_Current
abad_on_horizon = R_floor @ (R.T @ (arm.abad - pos) + Pos_floor)
hip_on_horizon = R_floor @ (R.T @ (arm.hip - pos) + Pos_floor)
knee_on_horizon = R_floor @ (R.T @ (arm.knee - pos) + Pos_floor)
foot_on_horizon = R_floor @ (R.T @ (arm.foot - pos) + Pos_floor)
self.horizon_ax.plot([abad_on_horizon[0], hip_on_horizon[0]],
[abad_on_horizon[1], hip_on_horizon[1]],
[abad_on_horizon[2], hip_on_horizon[2]],
color=self.abad_color)
self.horizon_ax.plot([hip_on_horizon[0], knee_on_horizon[0]],
[hip_on_horizon[1], knee_on_horizon[1]],
[hip_on_horizon[2], knee_on_horizon[2]],
color=self.hip_color)
self.horizon_ax.plot([knee_on_horizon[0], foot_on_horizon[0]],
[knee_on_horizon[1], foot_on_horizon[1]],
[knee_on_horizon[2], foot_on_horizon[2]],
color=self.knee_color)
# Plot a circle as representing floor contact area/point.
'''
if self.it == 0:
cir = Circle((foot_on_horizon[0], foot_on_horizon[1]),
radius=0.05,
color=(0.1, 0.1, 0.1),
alpha=0.5)
self.horizon_ax.add_patch(cir)
art3d.pathpatch_2d_to_3d(cir, z=foot_on_horizon[2], zdir="z")
'''
return foot_on_horizon
def plot_on_horizon(self):
'''
Plot on horizon coordinate
'''
pos = self.Pos_SupportCentroidOnBody_Current
R = self.Ori_SupportOnBody_Current
# Calculate base body rectangle, 4 nodes, location on support coordinate.
h0 = R_floor @ (R.T @ (self.arms[0].abad - pos) + Pos_floor)
h1 = R_floor @ (R.T @ (self.arms[1].abad - pos) + Pos_floor)
h2 = R_floor @ (R.T @ (self.arms[2].abad - pos) + Pos_floor)
h3 = R_floor @ (R.T @ (self.arms[3].abad - pos) + Pos_floor)
# Plot base body on horizon_ax
self.horizon_ax.plot([h0[0], h1[0]],
[h0[1], h1[1]],
[h0[2], h1[2]],
color=self.base_color)
self.horizon_ax.plot([h0[0], h2[0]],
[h0[1], h2[1]],
[h0[2], h2[2]],
color=self.base_color)
self.horizon_ax.plot([h3[0], h1[0]],
[h3[1], h1[1]],
[h3[2], h1[2]],
color=self.base_color)
self.horizon_ax.plot([h3[0], h2[0]],
[h3[1], h2[1]],
[h3[2], h2[2]],
color=self.base_color)
# Plot 4 legs
verts = [None, None, None, None]
for i in range(4):
verts[i] = self.plot_on_horizon_each_arm(self.arms[i])
# self.horizon_ax.plot(*((h0 + h1 + h2 + h3)/4), marker='.', color = 'k')
if self.it == 0:
triangles = [[verts[0], verts[1], verts[2]],
[verts[1], verts[2], verts[3]],
[verts[2], verts[3], verts[0]],
[verts[3], verts[0], verts[1]]]
self.horizon_ax.add_collection(Poly3DCollection(triangles,
edgecolor=(0.8, 0.8, 0.8),
color = (0.5, 0.5, 0.5),
alpha=0.2))
# Update Error
def update_error(self):
# Estimator
# Normal
if self.support_normal_option == 'svd':
normal = svd_regulated(np.array(A.subs(q_star), dtype=float))[0][:, -1] # This normal is normalized already
self.Normal_SupportOnBody_Current = normal
else:
normal = np.cross(self.arms[2].foot - self.arms[3].foot, self.arms[1].foot - self.arms[3].foot)
self.Normal_SupportOnBody_Current = normal / np.linalg.norm(normal)
# Centroid
self.Pos_SupportCentroidOnBody_Current = (self.arms[0].foot + self.arms[1].foot + self.arms[2].foot + self.arms[3].foot) / 4
# EdgeY
edgeY = self.get_edgeY()
self.EdgeY_SupportOnBody_Current = edgeY / np.linalg.norm(edgeY)
# After Ori Estimator, Ori prepare
self.set_Ori_SupportOnBody_Current(self.EdgeY_SupportOnBody_Current, self.Normal_SupportOnBody_Current)
self.Ori_BodyOnSupport_Current = self.Ori_SupportOnBody_Current.T
# This need to be updated with real IMU, maybe from 4 points of body rectangle.
# Maybe not need to be updated, since 4 points of body rectangles need to be
# calculated by god view which need R_floor. Maybe use R_floor directly is just fine.
self.IMU_BodyOnHorizon_Sim = R_floor @ self.Ori_SupportOnBody_Current.T
self.Ori_BodyOnHorizon_Current = self.IMU_BodyOnHorizon_Sim
self.Ori_SupportOnHorizon_Current = self.Ori_BodyOnHorizon_Current @ self.Ori_BodyOnSupport_Current.T
print('Compare')
print(self.Ori_SupportOnHorizon_Current)
print(R_floor)
self.set_Ori_BodyOnSupport_Target_byRotation(self.Ori_SupportOnHorizon_Current.T @ self.Ori_BodyOnHorizon_Target)
# Pos prepare
self.Pos_BodyOnSupport_Target = self.Ori_SupportOnHorizon_Current.T @ self.Pos_BodyOnHorizontalSupport_Target
print('Pos_BodyOnSupport_Target: ', self.Pos_BodyOnSupport_Target)
self.set_Pos_SupportOnBody_Target(self.Pos_BodyOnSupport_Target) # SE(3) conversion
self.Ori_SupportOnBody_Error[0: 3] = self.Normal_SupportOnBody_Target - self.Normal_SupportOnBody_Current
self.Ori_SupportOnBody_Error[3: 6] = self.EdgeY_SupportOnBody_Target - self.EdgeY_SupportOnBody_Current
self.Pos_SupportOnBody_Error = self.Pos_SupportOnBody_Target - self.Pos_SupportCentroidOnBody_Current
# Pos_SupportOnBody_Error = np.array([0.01, 0, 0])
[7]:
# Whole control Figure
'''
Top View
1 --- 0
| |
| |
| |
3 --- 2
'''
%matplotlib notebook
%matplotlib notebook
plt.rcParams["figure.figsize"] = (10,20)
# The iter number
num = 20
# The init value of joints, q0 - q11
q = np.array([pi/30, -pi/2, 4/5*pi,
-pi/30, -pi/2, 2/3*pi,
pi/30, -pi/2, 2/3*pi,
-pi/30, -pi/2, 4/5*pi])
robot = FourLegRobot(ThreeLinkArm(q[0:3], num, abad0, hip0, knee0, foot0),
ThreeLinkArm(q[3:6], num, abad1, hip1, knee1, foot1),
ThreeLinkArm(q[6:9], num, abad2, hip2, knee2, foot2),
ThreeLinkArm(q[9:12], num, abad3, hip3, knee3, foot3),
num, support_normal_option = 'svd')
# Ori task: normalized and orthogonal orientation of support area relative to base body
# Below matches
# R = Ry(-pi/6) @ Rx(pi/12)
# Y_BodyOnSupport_Target = R[:, 1]
# Z_BodyOnSupport_Target = R[:, 2]
# YZ_BodyOnSupport_Target = np.array([-0.12940952, 0.96592583, 0.22414387, -0.48296291, -0.25881905, 0.8365163])
# robot.set_Ori_BodyOnSupport_byYZ(YZ_BodyOnSupport_Target)
# robot.set_Ori_BodyOnSupport_Target_byRPY(0, pi/6, 0)
robot.set_Ori_BodyOnHorizon_Target_byRPY(0, 0, 0)
# Pos task: 3d vector of middle point of foot position relative to base body
robot.Pos_BodyOnHorizontalSupport_Target = np.array([0, 0, 0.26])
for i in range(num):
print('i: ', i)
robot.it = i
robot.update_joints(q)
# Check distance
# q_star = {q0: q[0], q1: q[1], q2: q[2], q3: q[3], q4: q[4], q5: q[5], q6: q[6], q7: q[7], q8: q[8], q9: q[9], q10: q[10], q11: q[11]}
q_star = dict(zip(q_sym, q))
print(s0.subs(q_star))
print(s1.subs(q_star))
print(s2.subs(q_star))
print(s3.subs(q_star))
print(s4.subs(q_star))
print(s5.subs(q_star))
robot.update_error()
# PLOT
robot.prepare_plot()
robot.plot_on_body()
robot.plot_on_support()
robot.plot_on_horizon()
# NULLSPACE CONTROLLER
# Contact Task
# Sustitute symbolic expression with actual joint value
jc_np = np.array(jc.subs(q_star), dtype=float)
# Ori Task
# Calcualte Null space of contact jacobian
Nc = np.eye(12) - np.linalg.pinv(jc_np, rcond=1e-11) @ jc_np
# Calcualte j_pre for ori task which is in null space of contact jacobian
if robot.support_normal_option == 'svd':
j_ori_tail3_num = np.array(j_ori_tail3.subs(q_star), dtype = float)
# NOTICE!
# This is the jacobian of v5 or edgeY, NOT the jacobian of normalized v5.
# However, this jacobian of v5 still ensure we will go along correct direction.
dU, _, _ = autodiff_svd(A ,q_star)
j_ori_head3_num = dU[:,:,-1].T
j_pre = np.concatenate((j_ori_head3_num, j_ori_tail3_num)) @ Nc
else:
# support_normal_option = 'cross'
j_pre = np.array(j_ori.subs(q_star), dtype=float) @ Nc
# Pos Task
# Calculate Null space of both ori and contact task.
N = np.eye(12) - np.linalg.pinv(j_pre, rcond=1e-11) @ j_pre
# Calculate j_pre2 for pos task which is in null space of both contact jacobian and ori
j_pre2 = j_pos.subs(q_star) @ Nc @ N
j_pre2 = np.array(j_pre2, dtype=float)
# Calculate delta q for ori task
dq = np.linalg.pinv(j_pre, rcond=1e-11) @ robot.Ori_SupportOnBody_Error
# Calculate delta q for pos task
dq2 = np.linalg.pinv(j_pre2, rcond=1e-11) @ robot.Pos_SupportOnBody_Error
'''
print("Ori Pre NUll space checking")
print(np.isclose(jc_np * np.linalg.pinv(j_pre, rcond=1e-9), np.zeros((1, 3))))
print("Pos Pre Null space checking")
print(np.isclose(j_pre * np.linalg.pinv(j_pre2, rcond=1e-9), np.zeros((3, 3))))
print()
'''
# Update q. As error decrease, make Kp become bigger and bigger to coverge
# faster.
# The reason why there are 2 seperated Kp: The jacobian of support
# normal vector(head 3 of j_ori) achieved by svd method is for normalized
# support normal vector(U[:, -1]) while the jacobian of support normal
# vector (head 3 of j_ori) achieved by cross method is for raw normal
# vector(cross result). So, the scale need to be adjusted.
# If we wanna united 2 Kp, a possible approach might be updating 'svd'
# method so that the normal jacobian(head 3 of j_ori) isn't for normalized
# normal vector. It's just use U[:, -1] * S(Sigular value array, the middle
# result, of standard SVD) instead of normalized U[:, -1].
# Since the svd method is more precise than other methods. If the calculating
# power allow, this program should use svd method as much as possible.
# Also notice, the tail 3 of j_ori isn't for normalized edgeY vector in
# both 'svd' and 'cross' method. It's for raw edgeY vector.
if robot.support_normal_option == 'svd':
q = q + 0.9 / num * (i+1) * dq + 0.9 / num * (i+1) * dq2
else:
q = q + 0.2 / num * (i+1) * dq + 0.2 / num * (i+1) * dq2
# q = q + dq/max(dq)*0.01 + dq2/max(dq2) *0.01
# q = q + dq + dq2
i: 0
Matrix([[0.200652405858808]])
Matrix([[0.113874846476112]])
Matrix([[0.205565960700685]])
Matrix([[0.0556261287052016]])
Matrix([[0.205937189440296]])
Matrix([[0.0556261287052016]])
Compare
[[ 8.66025400e-01 2.08929909e-17 -5.00000000e-01]
[ 2.01497592e-21 1.00000000e+00 -4.17287452e-17]
[ 5.00000000e-01 -3.61776890e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [ 1.30000000e-01 -9.40619915e-18 2.25166604e-01]
i: 1
Matrix([[0.200464197426834]])
Matrix([[0.113630557404181]])
Matrix([[0.205358968165558]])
Matrix([[0.0556366211930191]])
Matrix([[0.205732193354102]])
Matrix([[0.0557051759365938]])
Compare
[[ 8.66025400e-01 -1.88842018e-17 -5.00000000e-01]
[ 5.01891646e-19 1.00000000e+00 3.79858419e-17]
[ 5.00000000e-01 3.29499332e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 8.56698262e-18 2.25166604e-01]
i: 2
Matrix([[0.199785565147066]])
Matrix([[0.112838490615058]])
Matrix([[0.204665682479924]])
Matrix([[0.0556692671887738]])
Matrix([[0.204959983456642]])
Matrix([[0.0559524087384013]])
Compare
[[ 8.66025400e-01 3.46676086e-17 -5.00000000e-01]
[ 1.23558295e-18 1.00000000e+00 -7.18704038e-17]
[ 5.00000000e-01 -6.24049710e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.62252925e-17 2.25166604e-01]
i: 3
Matrix([[0.198591051556393]])
Matrix([[0.111699293718025]])
Matrix([[0.203593181867003]])
Matrix([[0.0557151078876275]])
Matrix([[0.203485639625737]])
Matrix([[0.0562810757471112]])
Compare
[[ 8.66025400e-01 1.56205193e-17 -5.00000000e-01]
[-8.69233573e-19 1.00000000e+00 -3.35792459e-17]
[ 5.00000000e-01 -2.88602560e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [ 1.30000000e-01 -7.50366657e-18 2.25166604e-01]
i: 4
Matrix([[0.197196260789123]])
Matrix([[0.110688127890502]])
Matrix([[0.202549627925695]])
Matrix([[0.0557605326487339]])
Matrix([[0.201616903223953]])
Matrix([[0.0565494426098182]])
Compare
[[ 8.66025400e-01 -7.96849266e-18 -5.00000000e-01]
[ 1.54410448e-19 1.00000000e+00 1.08808800e-17]
[ 5.00000000e-01 8.02577976e-18 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 2.08670274e-18 2.25166604e-01]
i: 5
Matrix([[0.195978520992071]])
Matrix([[0.110058019702853]])
Matrix([[0.201849886684243]])
Matrix([[0.0557972686342503]])
Matrix([[0.199897177573393]])
Matrix([[0.0567088889879161]])
Compare
[[ 8.66025400e-01 -5.50941382e-17 -5.00000000e-01]
[ 1.03734162e-18 1.00000000e+00 1.12973066e-16]
[ 5.00000000e-01 9.81308049e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 2.55140093e-17 2.25166604e-01]
i: 6
Matrix([[0.195102434476325]])
Matrix([[0.109779513369715]])
Matrix([[0.201524217506006]])
Matrix([[0.0558220574068817]])
Matrix([[0.198620772328339]])
Matrix([[0.0567777478181446]])
Compare
[[ 8.66025400e-01 -4.98682638e-17 -5.00000000e-01]
[ 8.90269652e-19 1.00000000e+00 1.00925728e-16]
[ 5.00000000e-01 8.79179480e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 2.28586665e-17 2.25166604e-01]
i: 7
Matrix([[0.194540065791544]])
Matrix([[0.109725189797687]])
Matrix([[0.201447227105906]])
Matrix([[0.0558371244751695]])
Matrix([[0.197772390582585]])
Matrix([[0.0567862238790339]])
Compare
[[ 8.66025400e-01 -5.88324198e-17 -5.00000000e-01]
[-6.88817889e-19 1.00000000e+00 1.17533907e-16]
[ 5.00000000e-01 1.01707860e-16 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 2.64440436e-17 2.25166604e-01]
i: 8
Matrix([[0.194193100424057]])
Matrix([[0.109771253721704]])
Matrix([[0.201484424087798]])
Matrix([[0.0558463265903783]])
Matrix([[0.197227383912645]])
Matrix([[0.0567615364009154]])
Compare
[[ 8.66025400e-01 -2.78186774e-17 -5.00000000e-01]
[ 3.88832243e-19 1.00000000e+00 5.70632787e-17]
[ 5.00000000e-01 4.92632927e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 1.28084561e-17 2.25166604e-01]
i: 9
Matrix([[0.193971915373273]])
Matrix([[0.109835896331777]])
Matrix([[0.201543007848993]])
Matrix([[0.0558525473874137]])
Matrix([[0.196865465949048]])
Matrix([[0.0567240193961370]])
Compare
[[ 8.66025400e-01 1.49397716e-17 -5.00000000e-01]
[ 7.54498114e-20 1.00000000e+00 -2.79872162e-17]
[ 5.00000000e-01 -2.43343222e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [ 1.30000000e-01 -6.32692376e-18 2.25166604e-01]
i: 10
Matrix([[0.193822549289113]])
Matrix([[0.109883546237539]])
Matrix([[0.201579645395266]])
Matrix([[0.0558572831165495]])
Matrix([[0.196609507011765]])
Matrix([[0.0566865003985952]])
Compare
[[ 8.66025400e-01 1.07947130e-19 -5.00000000e-01]
[-2.75657700e-19 1.00000000e+00 5.27394948e-19]
[ 5.00000000e-01 4.16298299e-19 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 1.08237558e-19 2.25166604e-01]
i: 11
Matrix([[0.193721120920278]])
Matrix([[0.109908832453315]])
Matrix([[0.201590528120759]])
Matrix([[0.0558610046703323]])
Matrix([[0.196428462651993]])
Matrix([[0.0566561822469021]])
Compare
[[ 8.66025400e-01 3.63560368e-17 -5.00000000e-01]
[ 7.74995627e-20 1.00000000e+00 -7.23523453e-17]
[ 5.00000000e-01 -6.23253647e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.62045948e-17 2.25166604e-01]
i: 12
Matrix([[0.193658138092893]])
Matrix([[0.109919088433488]])
Matrix([[0.201588524349653]])
Matrix([[0.0558636413390986]])
Matrix([[0.196313139018673]])
Matrix([[0.0566359134666801]])
Compare
[[ 8.66025400e-01 -6.03511779e-17 -5.00000000e-01]
[ 1.18883651e-19 1.00000000e+00 1.21020475e-16]
[ 5.00000000e-01 1.04909824e-16 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 2.72765543e-17 2.25166604e-01]
i: 13
Matrix([[0.193625328368193]])
Matrix([[0.109922287899392]])
Matrix([[0.201584433638831]])
Matrix([[0.0558651879223760]])
Matrix([[0.196252352027075]])
Matrix([[0.0566250015227241]])
Compare
[[ 8.66025400e-01 2.70085048e-17 -5.00000000e-01]
[-1.31503602e-20 1.00000000e+00 -5.39593163e-17]
[ 5.00000000e-01 -4.66552947e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.21303766e-17 2.25166604e-01]
i: 14
Matrix([[0.193611907101886]])
Matrix([[0.109923037013635]])
Matrix([[0.201582024733209]])
Matrix([[0.0558658957167918]])
Matrix([[0.196227325877905]])
Matrix([[0.0566204650723440]])
Compare
[[ 8.66025400e-01 -6.26047161e-17 -5.00000000e-01]
[-2.33229701e-20 1.00000000e+00 1.25169014e-16]
[ 5.00000000e-01 1.08369388e-16 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 2.81760408e-17 2.25166604e-01]
i: 15
Matrix([[0.193607794162576]])
Matrix([[0.109923161305813]])
Matrix([[0.201581137345032]])
Matrix([[0.0558661458818032]])
Matrix([[0.196219599928836]])
Matrix([[0.0566190679235582]])
Compare
[[ 8.66025400e-01 -7.52298724e-17 -5.00000000e-01]
[-1.98617141e-21 1.00000000e+00 1.50435162e-16]
[ 5.00000000e-01 1.30281771e-16 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 3.38732605e-17 2.25166604e-01]
i: 16
Matrix([[0.193606882734149]])
Matrix([[0.109923171256572]])
Matrix([[0.201580913321427]])
Matrix([[0.0558662064822084]])
Matrix([[0.196217870954272]])
Matrix([[0.0566187578856053]])
Compare
[[ 8.66025400e-01 -3.16182854e-17 -5.00000000e-01]
[ 8.58154627e-22 1.00000000e+00 6.32401072e-17]
[ 5.00000000e-01 5.47669875e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 1.42394168e-17 2.25166604e-01]
i: 17
Matrix([[0.193606741317178]])
Matrix([[0.109923173128898]])
Matrix([[0.201580874558851]])
Matrix([[0.0558662200884570]])
Matrix([[0.196217596162483]])
Matrix([[0.0566187106701023]])
Compare
[[ 8.66025400e-01 -2.67896254e-17 -5.00000000e-01]
[-7.08740581e-22 1.00000000e+00 5.35870702e-17]
[ 5.00000000e-01 4.64057323e-17 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 1.20654904e-17 2.25166604e-01]
i: 18
Matrix([[0.193606728196559]])
Matrix([[0.109923172310622]])
Matrix([[0.201580872004251]])
Matrix([[0.0558662209414493]])
Matrix([[0.196217569850389]])
Matrix([[0.0566187060352217]])
Compare
[[ 8.66025400e-01 4.14219457e-18 -5.00000000e-01]
[-6.16918064e-21 1.00000000e+00 -8.27898568e-18]
[ 5.00000000e-01 -7.16409283e-18 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [ 1.30000000e-01 -1.86266414e-18 2.25166604e-01]
i: 19
Matrix([[0.193606740688201]])
Matrix([[0.109923178705195]])
Matrix([[0.201580883995420]])
Matrix([[0.0558662303576911]])
Matrix([[0.196217574854985]])
Matrix([[0.0566187110900983]])
Compare
[[ 8.66025400e-01 -1.10289229e-16 -5.00000000e-01]
[-1.74908901e-20 1.00000000e+00 2.20540429e-16]
[ 5.00000000e-01 1.90997215e-16 8.66025400e-01]]
[[ 0.8660254 0. -0.5 ]
[ 0. 1. 0. ]
[ 0.5 0. 0.8660254]]
Pos_BodyOnSupport_Target: [1.30000000e-01 4.96592759e-17 2.25166604e-01]
[ ]: