def _get_C(self, i_X_p, Si, Ic, q, q_dot, n_joints,
gravity=None, f_ext=None):
"""Internal function for calculating the joint space bias matrix."""
v = []
a = []
f = []
C = cs.SX.zeros(n_joints)
for i in range(0, n_joints):
vJ = cs.mtimes(Si[i], q_dot[i])
if i == 0:
v.append(vJ)
if gravity is not None:
ag = np.array([0., 0., 0., gravity[0], gravity[1], gravity[2]])
a.append(cs.mtimes(i_X_p[i], -ag))
else:
a.append(cs.SX([0., 0., 0., 0., 0., 0.]))
else:
v.append(cs.mtimes(i_X_p[i], v[i-1]) + vJ)
a.append(cs.mtimes(i_X_p[i], a[i-1]) + cs.mtimes(plucker.motion_cross_product(v[i]),vJ))
f.append(cs.mtimes(Ic[i], a[i]) + cs.mtimes(plucker.force_cross_product(v[i]), cs.mtimes(Ic[i], v[i])))
if f_ext is not None:
f = self._apply_external_forces(f_ext, f, i_X_0)
for i in range(n_joints-1, -1, -1):
C[i] = cs.mtimes(Si[i].T, f[i])
if i != 0:
f[i-1] = f[i-1] + cs.mtimes(i_X_p[i].T, f[i])
return C
def get_inverse_dynamics_rnea(self, root, tip,
gravity=None, f_ext=None):
"""Returns the inverse dynamics as a casadi function."""
if self.robot_desc is None:
raise ValueError('Robot description not loaded from urdf')
n_joints = self.get_n_joints(root, tip)
q = cs.SX.sym("q", n_joints)
q_dot = cs.SX.sym("q_dot", n_joints)
q_ddot = cs.SX.sym("q_ddot", n_joints)
i_X_p, Si, Ic = self._model_calculation(root, tip, q)
v = []
a = []
f = []
tau = cs.SX.zeros(n_joints)
for i in range(0, n_joints):
vJ = cs.mtimes(Si[i], q_dot[i])
if i == 0:
v.append(vJ)
if gravity is not None:
ag = np.array([0.,
0.,
0.,
gravity[0],
gravity[1],
gravity[2]])
a.append(
cs.mtimes(i_X_p[i], -ag) + cs.mtimes(Si[i], q_ddot[i]))
else:
a.append(cs.mtimes(Si[i], q_ddot[i]))
else:
v.append(cs.mtimes(i_X_p[i], v[i-1]) + vJ)
a.append(
cs.mtimes(i_X_p[i], a[i-1])
+ cs.mtimes(Si[i], q_ddot[i])
+ cs.mtimes(plucker.motion_cross_product(v[i]), vJ))
f.append(
cs.mtimes(Ic[i], a[i])
+ cs.mtimes(
plucker.force_cross_product(v[i]),
cs.mtimes(Ic[i], v[i])))
if f_ext is not None:
f = self._apply_external_forces(f_ext, f, i_X_p)
for i in range(n_joints-1, -1, -1):
tau[i] = cs.mtimes(Si[i].T, f[i])
if i != 0:
f[i-1] = f[i-1] + cs.mtimes(i_X_p[i].T, f[i])
tau = cs.Function("C", [q, q_dot, q_ddot], [tau], self.func_opts)
return tau
def get_forward_dynamics_aba(self, root, tip, gravity=None, f_ext=None):
"""Returns the forward dynamics as a casadi function using the
articulated body algorithm."""
if self.robot_desc is None:
raise ValueError('Robot description not loaded from urdf')
n_joints = self.get_n_joints(root, tip)
q = cs.SX.sym("q", n_joints)
q_dot = cs.SX.sym("q_dot", n_joints)
tau = cs.SX.sym("tau", n_joints)
q_ddot = cs.SX.zeros(n_joints)
i_X_p, Si, Ic = self._model_calculation(root, tip, q)
v = []
c = []
pA = []
IA = []
u = [None] * n_joints
U = [None] * n_joints
d = [None] * n_joints
for i in range(0, n_joints):
vJ = cs.mtimes(Si[i], q_dot[i])
if i == 0:
v.append(vJ)
c.append([0, 0, 0, 0, 0, 0])
else:
v.append(cs.mtimes(i_X_p[i], v[i - 1]) + vJ)
c.append(cs.mtimes(plucker.motion_cross_product(v[i]), vJ))
IA.append(Ic[i])
pA.append(
cs.mtimes(plucker.force_cross_product(v[i]),
cs.mtimes(Ic[i], v[i])))
if f_ext is not None:
pA = self._apply_external_forces(f_ext, pA)
for i in range(n_joints - 1, -1, -1):
U[i] = cs.mtimes(IA[i], Si[i])
d[i] = cs.mtimes(Si[i].T, U[i])
u[i] = tau[i] - cs.mtimes(Si[i].T, pA[i])
if i != 0:
Ia = IA[i] - ((cs.mtimes(U[i], U[i].T) / d[i]))
pa = pA[i] + cs.mtimes(Ia,
c[i]) + (cs.mtimes(U[i], u[i]) / d[i])
IA[i - 1] += cs.mtimes(i_X_p[i].T, cs.mtimes(Ia, i_X_p[i]))
pA[i - 1] += cs.mtimes(i_X_p[i].T, pa)
a = []
for i in range(0, n_joints):
if i == 0:
if gravity is not None:
ag = np.array(
[0., 0., 0., gravity[0], gravity[1], gravity[2]])
a_temp = (cs.mtimes(i_X_p[i], -ag) + c[i])
else:
a_temp = c[i]
else:
a_temp = (cs.mtimes(i_X_p[i], a[i - 1]) + c[i])
q_ddot[i] = (u[i] - cs.mtimes(U[i].T, a_temp)) / d[i]
a.append(a_temp + cs.mtimes(Si[i], q_ddot[i]))
q_ddot = cs.Function("q_ddot", [q, q_dot, tau], [q_ddot],
self.func_opts)
return q_ddot
