simulation-pendulum.py
Go to the documentation of this file.
1 import pinocchio as pin
2 import hppfcl as fcl
3 import numpy as np
4 import math
5 import time
6 import sys
7 
8 # Parse input arguments
9 import argparse
10 parser = argparse.ArgumentParser()
11 parser.add_argument("--with-cart", help="Add a cart at the base of the pendulum to simulate a cart pole system.",
12  action="store_true")
13 parser.add_argument("-N", help="Number of pendulums compositing the dynamical system.",
14  type=int)
15 args = parser.parse_args()
16 
17 if args.N:
18  N = args.N
19 else:
20  N = 1 # number of pendulums
21 
22 model = pin.Model()
23 geom_model = pin.GeometryModel()
24 
25 parent_id = 0
26 
27 if args.with_cart:
28  cart_radius = 0.1
29  cart_length = 5 * cart_radius
30  cart_mass = 2.
31  joint_name = "joint_cart"
32 
33  geometry_placement = pin.SE3.Identity()
34  geometry_placement.rotation = pin.Quaternion(np.array([0.,0.,1.]),np.array([0.,1.,0.])).toRotationMatrix()
35 
36  joint_id = model.addJoint(parent_id, pin.JointModelPY(), pin.SE3.Identity(), joint_name)
37 
38  body_inertia = pin.Inertia.FromCylinder(cart_mass,cart_radius,cart_length)
39  body_placement = geometry_placement
40  model.appendBodyToJoint(joint_id,body_inertia,body_placement) # We need to rotate the inertia as it is expressed in the LOCAL frame of the geometry
41 
42  shape_cart = fcl.Cylinder(cart_radius, cart_length)
43 
44  geom_cart = pin.GeometryObject("shape_cart", joint_id, shape_cart, geometry_placement)
45  geom_cart.meshColor = np.array([1.,0.1,0.1,1.])
46  geom_model.addGeometryObject(geom_cart)
47 
48  parent_id = joint_id
49 else:
50  base_radius = 0.2
51  shape_base = fcl.Sphere(base_radius)
52  geom_base = pin.GeometryObject("base", 0, shape_base, pin.SE3.Identity())
53  geom_base.meshColor = np.array([1.,0.1,0.1,1.])
54  geom_model.addGeometryObject(geom_base)
55 
56 joint_placement = pin.SE3.Identity()
57 body_mass = 1.
58 body_radius = 0.1
59 
60 for k in range(N):
61  joint_name = "joint_" + str(k+1)
62  joint_id = model.addJoint(parent_id,pin.JointModelRX(),joint_placement,joint_name)
63 
64  body_inertia = pin.Inertia.FromSphere(body_mass,body_radius)
65  body_placement = joint_placement.copy()
66  body_placement.translation[2] = 1.
67  model.appendBodyToJoint(joint_id,body_inertia,body_placement)
68 
69  geom1_name = "ball_" + str(k+1)
70  shape1 = fcl.Sphere(body_radius)
71  geom1_obj = pin.GeometryObject(geom1_name, joint_id, shape1, body_placement)
72  geom1_obj.meshColor = np.ones((4))
73  geom_model.addGeometryObject(geom1_obj)
74 
75  geom2_name = "bar_" + str(k+1)
76  shape2 = fcl.Cylinder(body_radius/4.,body_placement.translation[2])
77  shape2_placement = body_placement.copy()
78  shape2_placement.translation[2] /= 2.
79 
80  geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2, shape2_placement)
81  geom2_obj.meshColor = np.array([0.,0.,0.,1.])
82  geom_model.addGeometryObject(geom2_obj)
83 
84  parent_id = joint_id
85  joint_placement = body_placement.copy()
86 
87 from pinocchio.visualize import MeshcatVisualizer as Visualizer
88 
89 visual_model = geom_model
90 viz = Visualizer(model, geom_model, visual_model)
91 
92 # Initialize the viewer.
93 try:
94  viz.initViewer()
95 except ImportError as err:
96  print("Error while initializing the viewer. It seems you should install gepetto-viewer")
97  print(err)
98  sys.exit(0)
99 
100 try:
101  viz.loadViewerModel("pinocchio")
102 except AttributeError as err:
103  print("Error while loading the viewer model. It seems you should start gepetto-viewer")
104  print(err)
105  sys.exit(0)
106 
107 # Display a robot configuration.
108 q0 = pin.neutral(model)
109 viz.display(q0)
110 
111 # Play a bit with the simulation
112 dt = 0.01
113 T = 5
114 
115 N = math.floor(T/dt)
116 
117 model.lowerPositionLimit.fill(-math.pi)
118 model.upperPositionLimit.fill(+math.pi)
119 
120 if args.with_cart:
121  model.lowerPositionLimit[0] = model.upperPositionLimit[0] = 0.
122 
123 data_sim = model.createData()
124 
125 t = 0.
126 q = pin.randomConfiguration(model)
127 v = np.zeros((model.nv))
128 tau_control = np.zeros((model.nv))
129 damping_value = 0.1
130 for k in range(N):
131 
132  tic = time.time()
133  tau_control = -damping_value * v # small damping
134  a = pin.aba(model,data_sim,q,v,tau_control) # Forward dynamics
135 
136  # Semi-explicit integration
137  v += a*dt
138  q = pin.integrate(model,q,v*dt) # Configuration integration
139 
140  viz.display(q)
141  toc = time.time()
142  ellapsed = toc - tic
143 
144  dt_sleep = max(0,dt - (ellapsed))
145  time.sleep(dt_sleep)
146  t += dt
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:24


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04