3 import pinocchio
as pin
14 base_sizes = (0.4, 0.2, 0.05)
16 base = pin.JointModelPX()
17 base_id = model.addJoint(0, base, pin.SE3.Identity(),
"base")
20 pole = pin.JointModelRUBY()
22 pole = pin.JointModelRY()
23 pole_id = model.addJoint(1, pole, pin.SE3.Identity(),
"pole")
25 base_inertia = pin.Inertia.FromBox(m1, *base_sizes)
26 pole_inertia = pin.Inertia(
28 np.array([0.0, 0.0, length / 2]),
29 m2 / 5 * np.diagflat([1e-2, length**2, 1e-2]),
32 base_body_pl = pin.SE3.Identity()
33 pole_body_pl = pin.SE3.Identity()
34 pole_body_pl.translation = np.array([0.0, 0.0, length / 2])
36 model.appendBodyToJoint(base_id, base_inertia, base_body_pl)
37 model.appendBodyToJoint(pole_id, pole_inertia, pole_body_pl)
40 collision_model = pin.GeometryModel()
41 shape_base = fcl.Box(*base_sizes)
43 shape_pole = fcl.Capsule(radius, length)
44 RED_COLOR = np.array([1, 0.0, 0.0, 1.0])
45 WHITE_COLOR = np.array([1, 1.0, 1.0, 1.0])
46 geom_base = pin.GeometryObject(
"link_base", base_id, shape_base, base_body_pl)
47 geom_base.meshColor = WHITE_COLOR
48 geom_pole = pin.GeometryObject(
"link_pole", pole_id, shape_pole, pole_body_pl)
49 geom_pole.meshColor = RED_COLOR
51 collision_model.addGeometryObject(geom_base)
52 collision_model.addGeometryObject(geom_pole)
53 visual_model = collision_model
54 return model, collision_model, visual_model
58 """Take a Pinocchio model, turn it into a Casadi model
59 and define the appropriate graphs.
62 def __init__(self, model: pin.Model, timestep=0.05):
71 """Create the acceleration expression and acceleration function."""
75 q = casadi.SX.sym(
"q", nq)
76 v = casadi.SX.sym(
"v", nv)
77 u = casadi.SX.sym(
"u", nu)
78 dq_ = casadi.SX.sym(
"dq_", nv)
88 self.
acc_func = casadi.Function(
"acc", [q, v, u], [a], [
"q",
"v",
"u"], [
"a"])
91 """Create the map `(q,v) -> (qnext, vnext)` using semi-implicit Euler integration."""
99 q_dq = cpin.integrate(self.
cmodel, q, dq_)
106 qnext = cpin.integrate(self.
cmodel, self.
q_dq, dt * vnext)
112 [
"q",
"dq_",
"v",
"u"],
117 q=q, dq_=casadi.SX.zeros(nv), v=v, u=u
133 state = casadi.SX.sym(
"x", nq + nv)
135 q, v = casadi.vertsplit(state, nq)
143 next_state = casadi.SX.sym(
"xnext", nq + nv)
144 qn, vn = casadi.vertsplit(next_state, nq)
145 dqn_ = casadi.SX.sym(
"dqn_", nv)
146 qn_dq = cpin.integrate(model, qn, dqn_)
148 res_q = cpin.difference(model, q_dq, qn_dq) - dt * vnext
149 res_v = (vn - v) - dt * a
151 residual = casadi.vertcat(res_q, res_v)
154 [state, u, next_state, dq_, dqn_],
156 [
"x",
"u",
"xnext",
"dq_",
"dqn_"],
167 xnext = np.concatenate((qnext, vnext))
183 super().
__init__(model=model, timestep=timestep)
188 model = cartpole.model
192 q0 = np.array([0.0, 0.95, 0.01])
193 q0 = pin.normalize(model, q0)
194 v = np.zeros(model.nv)
196 a0 = cartpole.acc_func(q0, v, u)
200 x0 = np.append(q0, v)
201 xnext = cartpole.forward(x0, u)
205 states_ = [x0.copy()]
206 for t
in range(nsteps):
208 xnext = cartpole.forward(states_[t], u).ravel()
209 states_.append(xnext)
214 states_ = np.stack(states_).T
217 from pinocchio
import visualize
219 viz = visualize.MeshcatVisualizer(
221 collision_model=cartpole.collision_model,
222 visual_model=cartpole.visual_model,
226 viz.loadViewerModel(
"pinocchio")
228 qs_ = states_[: model.nq, :]
229 viz.play(q_trajectory=qs_, dt=dt)