def __init__(self, urdf_path, root, tip, get_motor_dynamics=True):
'''Takes the minimum info required to start the dynamics matrix calculations.
Motor inertias and SEA dampings can be given manually, or will be automatically parsed'''
# Create a parser
self.robot_parser = self._load_urdf(urdf_path)
# Store inputs
self.root = root
self.tip = tip
# Retrieve basic info
self.num_joints = self.get_joints_n()
self._define_symbolic_vars()
self.M, self.Cq, self.G = self._get_motion_equation_matrix()
self.M_inv = cs.pinv(self.M)
self.upper_q, self.lower_q, self.max_effort, self.max_velocity = self.get_limits()
self.ee_pos = self._get_forward_kinematics()
# Fix boundaries if not given
self.upper_u, self.lower_u = self._fix_boundaries(self.max_effort)
self.upper_qd, self.lower_qd = self._fix_boundaries(self.max_velocity)
# SEA Stuff
self.sea = self.get_joints_plugin(urdf_path, 'spring_k', 0, diag=True).any() # True if at least one joint is sea
if self.sea:
self.get_seaplugin_values(urdf_path, get_motor_dynamics)
def pinv(A):
"""
Returns the Moore-Penrose pseudoinverse of the matrix A.
See: https://numpy.org/doc/stable/reference/generated/numpy.linalg.pinv.html
"""
if not is_casadi_type(A):
return _onp.linalg.pinv(A)
else:
return _cas.pinv(A)
def pinv(self, J):
if self.options["pinv_method"] == "standard":
pJ = cs.pinv(J)
elif self.options["pinv_method"] == "damped":
dmpng_fctr = self.options["damping_factor"]
if J.size2() >= J.size1():
inner = cs.mtimes(J, J.T)
inner += dmpng_fctr * cs.DM.eye(J.size1())
pJ = cs.solve(inner, J).T
else:
inner = cs.mtimes(J.T, J)
inner += dmpng_fctr * cs.DM.eye(J.size2())
pJ = cs.solve(inner, J.T)
return pJ
def get_least_squares_n_vec(parent, variables, parameters, architecture):
children = architecture.kites_map[parent]
matrix = []
rhs = []
for kite in children:
qk = variables['xd']['q' + str(kite) + str(parent)]
newline = cas.horzcat(qk[[1]], qk[[2]], 1)
matrix = cas.vertcat(matrix, newline)
rhs = cas.vertcat(rhs, -qk[[0]])
coords = cas.mtimes(cas.pinv(matrix), rhs)
n_vec = cas.vertcat(1, coords[[0]], coords[[1]])
return n_vec
def get_least_squares_nhat(parent, variables, parameters, architecture):
children = architecture.kites_map[parent]
matrix = []
rhs = []
for kite in children:
qk = variables['xd']['q' + str(kite) + str(parent)]
newline = cas.horzcat(qk[[1]], qk[[2]], 1)
matrix = cas.vertcat(matrix, newline)
rhs = cas.vertcat(rhs, qk[[0]])
coords = cas.mtimes(cas.pinv(matrix), rhs)
nvec = cas.vertcat(1., coords[[0]], coords[[1]])
scale = 1.
factor = scale * get_factor_var(parent, variables, parameters)
nhat = nvec * factor
return nhat
def nullspace(self, var):
"""Returns I-pinv(J)*J where J is dexpression/dvar."""
J = self.jacobian(var)
return cs.MX.eye(var.size()[0]) - cs.mtimes(cs.pinv(J), J)
def _get_diff_eq(self, cost_func):
'''Function that returns the RhS of the differential equations. See the papers for additional info'''
RHS = []
# RHS that's in common for all cases
rhs1 = self.q_dot
rhs2 = self.M_inv @ (-self.Cq - self.G - self.Fd @ self.q_dot - self.Ff @ cs.sign(self.q_dot))
# Adjusting RHS for SEA with known inertia. Check the paper for more info.
if self.sea and self.SEAdynamics:
rhs2 -= self.M_inv @ self.tau_sea
rhs3 = self.theta_dot
rhs4 = cs.pinv(self.B) @ (-self.FDsea @ self.theta_dot + self.u + self.tau_sea - self.FMusea @ cs.sign(self.theta_dot))
RHS = [rhs1, rhs2, rhs3, rhs4]
# Adjusting the lenght of the variables
self.x = cs.vertcat(self.q, self.q_dot, self.theta, self.theta_dot)
self.num_state_var = self.num_joints * 2
self.lower_q = self.lower_q + self.lower_theta
self.lower_qd = self.lower_qd + self.lower_thetad
self.upper_q = self.upper_q + self.upper_theta
self.upper_qd = self.upper_qd + self.upper_thetad
# Adjusting RHS for SEA modeling, with motor inertia unknown
elif self.sea and not self.SEAdynamics:
rhs2 += -self.M_inv @ self.K @ (self.q - self.u)
# self.upper_u, self.lower_u = self.upper_q, self.lower_q
RHS = [rhs1, rhs2]
# State variable
self.x = cs.vertcat(self.q, self.q_dot)
self.num_state_var = self.num_joints
# Adjusting RHS for classic robot modeling, without any SEA
else:
rhs2 += self.M_inv @ self.u
RHS = [rhs1, rhs2]
# State variable
self.x = cs.vertcat(self.q, self.q_dot)
self.num_state_var = self.num_joints
# Evaluating q_ddot, in order to handle it when given as an input of cost function or constraints
self.q_ddot_val = self._get_joints_accelerations(rhs2)
# The differentiation of J will be the cost function given by the user, with our symbolic
# variables as inputs
if self.sea and self.SEAdynamics:
q_ddot_J = self.q_ddot_val(self.q, self.q_dot, self.theta, self.theta_dot, self.u)
else:
q_ddot_J = self.q_ddot_val(self.q,self.q_dot, self.u)
if self.sea and not self.SEAdynamics:
u_J = self.u - self.q # the instantaneous spent energy is prop to |u-q|
else:
u_J = self.u
J_dot = cost_func( self.q - self.traj,
self.q_dot - self.traj_dot,
q_ddot_J,
self.ee_pos(self.q),
u_J,
self.t
)
# Setting the relationship
self.x_dot = cs.vertcat(*RHS)
# Defining the differential equation
f = cs.Function('f', [self.x, self.u, self.t], # inputs
[self.x_dot, J_dot]) # outputs
return f