def get_robot_parameters(urdf_file_path):
l_robot = get_robot(urdf_file_path)
thr_map = sp.Matrix.zeros(6, 6)
F_c_summ = sp.Matrix.zeros(6, 1)
ftz = sp.Matrix(sp.symarray('ftz', len(
l_robot["robot"]["joint"]))) # Along which axis???
for n, thruster in enumerate(l_robot["robot"]["joint"]):
Tct = thruster["Tct"]
Q, _ = np.linalg.qr(Tct[0:3, 0:3])
Tct[0:3, 0:3] = Q
Tct = sp.Matrix(Tct)
# print(" ")
# print(thruster["@name"])
# pprint(Tct)
# print(" ")
R = Tct[0:3, 0:3]
p = Tct[0:3, 3:]
so3_ct = sophus.So3(R)
se3_ct = sophus.Se3(so3_ct, p)
F_t = sp.Matrix([sp.Matrix([0, 0, 0]), sp.Matrix([0, 0, ftz[n, 0]])])
# (3.95) Express wrench in another basis.
# MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL Kevin M. Lynch and Frank C. Park May 3, 2017
# F_t = [mtx; mty; mtz; ftx; fty; ftz] where m - moments and f - forces.
T_adj_tc = sophus.Se3.Adj(se3_ct.inverse()).T
F_c = T_adj_tc * F_t
F_c_summ += F_c
th_map_col = T_adj_tc * sp.Matrix(
[sp.Matrix([0, 0, 0]), sp.Matrix([0, 0, 1])])
# Here we apply force along z+ direction and transform it.
# th_map_col = th_map_col/th_map_col.norm()
thr_map[:, n] = th_map_col
m = l_robot["robot"]["link"][0]["inertial"]["mass"]["@value"]
I_c = l_robot["robot"]["link"][0]["inertial"]["inertia"]
Ixx_c = I_c["@ixx"]
Iyy_c = I_c["@iyy"]
Izz_c = I_c["@izz"]
Ixy_c = I_c["@ixy"]
Ixz_c = I_c["@ixz"]
Iyz_c = I_c["@iyz"]
I_c = sp.Matrix([[Ixx_c, Ixy_c, Ixz_c], [Ixy_c, Iyy_c, Iyz_c],
[Ixz_c, Iyz_c, Izz_c]])
l_robot["F_c_summ"] = F_c_summ
l_robot["thr_map"] = thr_map
l_robot["ftz"] = ftz
l_robot["I_c"] = I_c
l_robot["m"] = m
return l_robot
def test_inverse(self):
T = sympy.Matrix([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3],
[0, 0, 0, 1]])
se3 = sophus.Se3(sophus.So3(T[0:3, 0:3]), T[0:3, 3])
self.assertEqual(
se3.inverse().matrix(),
sympy.Matrix([[1, 0, 0, 0], [0, 0, 1.0, -3.0], [0, -1.0, 0, 0],
[0, 0, 0, 1]]))
def test_adjoint(self):
T = sympy.Matrix([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3],
[0, 0, 0, 1]])
se3 = sophus.Se3(sophus.So3(T[0:3, 0:3]), T[0:3, 3])
self.assertEqual(
se3.Adj(),
sympy.Matrix([[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]]))
def setUp(self):
upsilon0, upsilon1, upsilon2, omega0, omega1, omega2 = sympy.symbols(
'upsilon[0], upsilon[1], upsilon[2], omega[0], omega[1], omega[2]',
real=True)
x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True)
p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True)
t0, t1, t2 = sympy.symbols('t[0] t[1] t[2]', real=True)
v = sophus.Vector3(v0, v1, v2)
self.upsilon_omega = sophus.Vector6(
upsilon0, upsilon1, upsilon2, omega0, omega1, omega2)
self.t = sophus.Vector3(t0, t1, t2)
self.a = Se3(sophus.So3(sophus.Quaternion(x, v)), self.t)
self.p = sophus.Vector3(p0, p1, p2)
def inverse(self):
""" Returns group inverse."""
return sophus.So3(self.q.conj())
"lambda": sp.lambdify((F_c, ), ftz_from_F_c_result, 'numpy'),
"s_lambda": sp.lambdify((F_c, ), ftz_from_F_c_result, 'sympy'),
"result": ftz_from_F_c_result,
"expr": ftz_from_F_c_expr,
"J_wrt_F_c": ftz_from_F_c_result.jacobian(F_c)
}
w_c = sp.Matrix(sp.symarray('w_c', 3))
v_c = sp.Matrix(sp.symarray('v_c', 3))
V_c = sp.Matrix([w_c, v_c])
t_sc = sp.Matrix(sp.symarray('t_sc', 3))
q_s_cog = sp.Matrix(sp.symbols('q_sc_w q_sc_x q_sc_y q_sc_z')) # world to COG
q_sc = sophus.Quaternion(q_s_cog[0], q_s_cog[1:4, :])
T_sc = sophus.Se3(sophus.So3(q_sc), t_sc)
t_st = sp.Matrix(sp.symarray('t_st', 3))
q_s_target = sp.Matrix(
sp.symbols('q_st_w q_st_x q_st_y q_st_z')) # world to target
q_st = sophus.Quaternion(q_s_target[0], q_s_target[1:4, :])
T_st = sophus.Se3(sophus.So3(q_st), t_st)
def get_get_ftz_from_V_c_dV_c(urdf_file_path):
l_robot = get_robot_parameters(urdf_file_path)
G_c = sp.Matrix.zeros(6, 6)
G_c[0:3, 0:3] = l_robot["I_c"]
G_c[3:, 3:] = l_robot["m"] * sp.Matrix.eye(3)
dV_c = sp.Matrix(