4 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
6 from test_case
import PinocchioTestCase
as TestCase
8 import pinocchio
as pin
18 self.
cR0 = SX.sym(
"R0", 3, 3)
19 self.
cR1 = SX.sym(
"R1", 3, 3)
20 self.
dv0 = SX.sym(
"v0", 3)
21 self.
dv1 = SX.sym(
"v1", 3)
23 self.
cv2 = SX.sym(
"v2", 3)
24 self.
cdv2 = SX.sym(
"dv", 3)
30 self.
cM0 = SX.sym(
"M0", 4, 4)
31 self.
cM1 = SX.sym(
"M1", 4, 4)
32 self.
cdw0 = SX.sym(
"dm0", 6)
33 self.
cdw1 = SX.sym(
"dm1", 6)
34 self.
cw2 = SX.sym(
"w2", 6)
35 self.
cdw2 = SX.sym(
"dw2", 6)
40 """Test the exp map and its derivative."""
42 exp_expr = self.
cR0_i @ cpin.exp3(self.
cv2 + dw)
43 repl_dargs =
lambda e: casadi.substitute(
44 e, casadi.vertcat(self.
dv0, dw), np.zeros(6)
47 exp_eval = casadi.Function(
"exp", [self.
cR0, self.
cv2], [repl_dargs(exp_expr)])
49 diff_expr = cpin.log3(exp_eval(self.
cR0, self.
cv2).T @ exp_expr)
50 Jexp_expr = casadi.jacobian(diff_expr, casadi.vertcat(self.
dv0, dw))
51 Jexp_eval = casadi.Function(
52 "exp", [self.
cR0, self.
cv2], [repl_dargs(Jexp_expr)]
56 w1 = np.random.randn(3)
57 w2 = np.array([np.pi, 0, 0])
58 w3 = np.array([-np.pi, 0, 0])
63 self.assertApprox(exp_eval(R0, np.zeros(3)).full(), R0)
64 self.assertApprox(exp_eval(R0, w1).full(), R1)
65 self.assertApprox(exp_eval(R0, -w1).full(), R1.T)
66 self.assertApprox(exp_eval(R1, -w1).full(), R0)
67 self.assertApprox(exp_eval(R0, w2).full(), R2)
80 self.assertApprox(Jexp_eval(R0, w0).full(), np.hstack([R0.T, J0]))
81 self.assertApprox(Jexp_eval(R0, w1).full(), np.hstack([R1.T, J1]))
82 self.assertApprox(Jexp_eval(R0, w2).full(), np.hstack([R2.T, J2]))
83 self.assertApprox(Jexp_eval(R0, w3).full(), np.hstack([R3.T, J3]))
84 self.assertApprox(Jexp_eval(R1, w0).full(), np.hstack([R0.T, J0]))
85 self.assertApprox(Jexp_eval(R1, w2).full(), np.hstack([R2.T, J2]))
88 log_expr = cpin.log3(self.
cR0_i.T @ self.
cR1_i)
89 log_eval = casadi.Function(
92 [casadi.substitute(log_expr, self.
v_all, np.zeros(6))],
95 Jlog_expr = casadi.jacobian(log_expr, self.
v_all)
96 Jlog_eval = casadi.Function(
99 [casadi.substitute(Jlog_expr, self.
v_all, np.zeros(6))],
103 vr = np.random.randn(3)
105 R2 = pin.exp3(np.array([np.pi, 0, 0]))
106 v3 = np.array([0, np.pi, 0])
109 self.assertApprox(log_eval(R0, R0).full().squeeze(), np.zeros(3))
110 self.assertApprox(log_eval(R0, R1).full().squeeze(), vr)
111 self.assertApprox(log_eval(R1, R1).full().squeeze(), np.zeros(3))
112 self.assertApprox(log_eval(R0, R2).full().squeeze(), np.array([np.pi, 0, 0]))
113 self.assertApprox(log_eval(R0, R3).full().squeeze(), v3)
116 jac_identity = np.hstack([-J0, J0])
117 self.assertApprox(Jlog_eval(R0, R0).full(), jac_identity)
120 self.assertApprox(Jlog_eval(R0, R1).full(), np.hstack([-R1.T @ J1, J1]))
123 self.assertApprox(Jlog_eval(R0, R2).full(), np.hstack([-R2.T @ J2, J2]))
126 cquat = SX.sym(
"quat", 4)
127 cdv = SX.sym(
"dv", 3)
128 SO3 = cpin.liegroups.SO3()
129 repl_dargs =
lambda e: casadi.substitute(e, cdv, np.zeros(3))
131 cquat_i = SO3.integrate(cquat, cdv)
133 clog = cpin.log3(cquat_i)
134 cJlog = casadi.jacobian(clog, cdv)
135 clog_eval = casadi.Function(
"log", [cquat], [repl_dargs(clog)])
136 cJlog_eval = casadi.Function(
"Jlog", [cquat], [repl_dargs(cJlog)])
138 q0 = np.array([0.0, 0.0, 0.0, 1.0])
139 q1 = np.array([0.0, 1.0, 0.0, 0.0])
140 q2 = np.array([0.0, 0.0, 1.0, 0.0])
141 q3 = np.array([1.0, 0.0, 0.0, 0.0])
143 self.assertApprox(clog_eval(q0).full().squeeze(), np.zeros(3))
144 self.assertApprox(cJlog_eval(q0).full().squeeze(), np.eye(3))
146 clog_fun = casadi.dot(clog, clog)
147 cJlog_fun = casadi.jacobian(clog_fun, cdv)
148 clog_fun_eval = casadi.Function(
"normlog", [cquat], [repl_dargs(clog_fun)])
149 cJlog_fun_eval = casadi.Function(
"Jnormlog", [cquat], [repl_dargs(cJlog_fun)])
151 self.assertApprox(clog_fun_eval(q0).full().squeeze(), np.zeros(1))
152 self.assertApprox(cJlog_fun_eval(q0).full(), np.zeros(3))
155 cJlog_fun_eval(q1).full(), 2 * np.pi * np.array([0.0, 1.0, 0.0])
158 cJlog_fun_eval(q2).full(), 2 * np.pi * np.array([0.0, 0.0, 1.0])
161 cJlog_fun_eval(q3).full(), 2 * np.pi * np.array([1.0, 0.0, 0.0])
163 print(
"log3 quat done")
166 exp_expr = cpin.exp6(self.
cw2 + self.
cdw2)
167 repl_dargs =
lambda e: casadi.substitute(e, self.
cdw2, np.zeros(6))
169 exp_eval = casadi.Function(
"exp6", [self.
cw2], [repl_dargs(exp_expr.np)])
171 diff_expr = cpin.log6(cpin.SE3(exp_eval(self.
cw2)).actInv(exp_expr)).np
172 Jexp_expr = casadi.jacobian(diff_expr, self.
cdw2)
174 Jexp_eval = casadi.Function(
"exp6", [self.
cw2], [repl_dargs(Jexp_expr)])
177 w1 = np.array([0.0, 0.0, 0.0, np.pi, 0.0, 0.0])
178 w2 = np.random.randn(6)
179 w3 = np.array([0.0, 0.0, 0.0, np.pi / 2, 0.0, 0.0])
184 self.assertApprox(exp_eval(w0).full(), M0.np)
185 self.assertApprox(exp_eval(w1).full(), M1.np)
186 self.assertApprox(exp_eval(w2).full(), M2.np)
187 self.assertApprox(exp_eval(w3).full(), M3.np)
189 np.set_printoptions(precision=3)
191 self.assertApprox(Jexp_eval(w0).full(), J0)
193 self.assertApprox(Jexp_eval(w1).full(), J1)
195 self.assertApprox(Jexp_eval(w2).full(), J2)
198 log_expr = cpin.log6(self.
cM0_i.actInv(self.
cM1_i))
200 repl_dargs =
lambda e: casadi.substitute(
201 e, casadi.vertcat(self.
cdw0, self.
cdw1), np.zeros(12)
203 log_eval = casadi.Function(
204 "log6", [self.
cM0, self.
cM1], [repl_dargs(log_expr.np)]
207 Jlog_expr = casadi.jacobian(log_expr.np, casadi.vertcat(self.
cdw0, self.
cdw1))
208 Jlog_eval = casadi.Function(
209 "Jlog6", [self.
cM0, self.
cM1], [repl_dargs(Jlog_expr)]
213 M0 = pin.exp6(np.zeros(6))
214 w1 = np.array([0, 0, 0, np.pi, 0.0, 0.0])
216 w2 = np.random.randn(6)
219 self.assertApprox(log_eval(M0.np, M0.np).full(), w0)
220 self.assertApprox(log_eval(M1.np, M1.np).full(), w0)
221 self.assertApprox(log_eval(M0.np, M1.np).full(), w1)
222 self.assertApprox(log_eval(M0.np, M2.np).full(), w2)
227 self.assertApprox(Jlog_eval(M0.np, M0.np).full(), np.hstack([-J0, J0]))
229 Jlog_eval(M0.np, M1.np).full(), np.hstack([-M1.dualAction.T @ J1, J1])
232 Jlog_eval(M0.np, M2.np).full(), np.hstack([-M2.dualAction.T @ J2, J2])
236 cq0 = SX.sym(
"q0", 7)
237 cv0 = SX.sym(
"q0", 6)
238 repl_dargs =
lambda e: casadi.substitute(e, cv0, np.zeros(6))
239 SE3 = cpin.liegroups.SE3()
241 cq0_i = SE3.integrate(cq0, cv0)
242 clog = cpin.log6_quat(cq0_i).vector
243 clog_eval = casadi.Function(
"log", [cq0], [repl_dargs(clog)])
245 cJlog = casadi.jacobian(clog, cv0)
246 cJlog_eval = casadi.Function(
"Jlog", [cq0], [repl_dargs(cJlog)])
248 q0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
249 q1 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0])
250 q2 = np.array([0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0])
251 q3 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
252 self.assertApprox(clog_eval(q0).full().squeeze(), np.zeros(6))
253 self.assertApprox(cJlog_eval(q0).full(), np.eye(6))
255 print(
"log6 with quats")
256 print(clog_eval(q1).full())
257 print(cJlog_eval(q1).full())
259 clog_fun = casadi.dot(clog, clog)
260 clog_fun = casadi.dot(clog, clog)
261 cJlog_fun = casadi.jacobian(clog_fun, cv0)
262 clog_fun_eval = casadi.Function(
"normlog", [cq0], [repl_dargs(clog_fun)])
263 cJlog_fun_eval = casadi.Function(
"Jnormlog", [cq0], [repl_dargs(cJlog_fun)])
265 print(clog_fun_eval(q0).full().squeeze())
266 print(cJlog_fun_eval(q0).full())
268 print(clog_fun_eval(q1).full().squeeze())
269 print(cJlog_fun_eval(q1).full())
271 print(clog_fun_eval(q2).full().squeeze())
272 print(cJlog_fun_eval(q2).full())
274 print(clog_fun_eval(q3).full().squeeze())
275 print(cJlog_fun_eval(q3).full())
278 if __name__ ==
"__main__":