1 import pinocchio
as pin
11 parser = argparse.ArgumentParser()
14 help=
"Add a cart at the base of the pendulum to simulate a cart pole system.",
18 "-N", help=
"Number of pendulums compositing the dynamical system.", type=int
20 args = parser.parse_args()
28 geom_model = pin.GeometryModel()
34 cart_length = 5 * cart_radius
36 joint_name =
"joint_cart"
38 geometry_placement = pin.SE3.Identity()
39 geometry_placement.rotation = pin.Quaternion(
40 np.array([0.0, 0.0, 1.0]), np.array([0.0, 1.0, 0.0])
43 joint_id = model.addJoint(
44 parent_id, pin.JointModelPY(), pin.SE3.Identity(), joint_name
47 body_inertia = pin.Inertia.FromCylinder(cart_mass, cart_radius, cart_length)
48 body_placement = geometry_placement
49 model.appendBodyToJoint(
50 joint_id, body_inertia, body_placement
53 shape_cart = fcl.Cylinder(cart_radius, cart_length)
55 geom_cart = pin.GeometryObject(
56 "shape_cart", joint_id, geometry_placement, shape_cart
58 geom_cart.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
59 geom_model.addGeometryObject(geom_cart)
64 shape_base = fcl.Sphere(base_radius)
65 geom_base = pin.GeometryObject(
"base", 0, pin.SE3.Identity(), shape_base)
66 geom_base.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
67 geom_model.addGeometryObject(geom_base)
69 joint_placement = pin.SE3.Identity()
74 joint_name =
"joint_" + str(k + 1)
75 joint_id = model.addJoint(
76 parent_id, pin.JointModelRX(), joint_placement, joint_name
79 body_inertia = pin.Inertia.FromSphere(body_mass, body_radius)
80 body_placement = joint_placement.copy()
81 body_placement.translation[2] = 1.0
82 model.appendBodyToJoint(joint_id, body_inertia, body_placement)
84 geom1_name =
"ball_" + str(k + 1)
85 shape1 = fcl.Sphere(body_radius)
86 geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1)
87 geom1_obj.meshColor = np.ones((4))
88 geom_model.addGeometryObject(geom1_obj)
90 geom2_name =
"bar_" + str(k + 1)
91 shape2 = fcl.Cylinder(body_radius / 4.0, body_placement.translation[2])
92 shape2_placement = body_placement.copy()
93 shape2_placement.translation[2] /= 2.0
95 geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2_placement, shape2)
96 geom2_obj.meshColor = np.array([0.0, 0.0, 0.0, 1.0])
97 geom_model.addGeometryObject(geom2_obj)
100 joint_placement = body_placement.copy()
104 visual_model = geom_model
107 viz = Visualizer(model, geom_model, visual_model)
109 except ImportError
as err:
111 "Error while initializing the viewer. It seems you should install gepetto-viewer"
117 viz.loadViewerModel(
"pinocchio")
118 except AttributeError
as err:
120 "Error while loading the viewer model. It seems you should start gepetto-viewer"
126 q0 = pin.neutral(model)
133 N = math.floor(T / dt)
135 model.lowerPositionLimit.fill(-math.pi)
136 model.upperPositionLimit.fill(+math.pi)
139 model.lowerPositionLimit[0] = model.upperPositionLimit[0] = 0.0
141 data_sim = model.createData()
144 q = pin.randomConfiguration(model)
145 v = np.zeros((model.nv))
146 tau_control = np.zeros((model.nv))
150 tau_control = -damping_value * v
151 a = pin.aba(model, data_sim, q, v, tau_control)
155 q = pin.integrate(model, q, v * dt)
161 dt_sleep =
max(0, dt - (ellapsed))