2 Create a simulation environment for a N-pendulum.
17 import pinocchio
as pin
18 from numpy.linalg
import inv
19 from pinocchio.utils
import rand
21 from display
import Display
26 Class representing one 3D mesh of the robot, to be attached to a joint. The class contains:
27 * the name of the 3D objects inside Gepetto viewer.
28 * the ID of the joint in the kinematic tree to which the body is attached.
29 * the placement of the body with respect to the joint frame.
30 This class is only used in the list Robot.visuals (see below).
33 def __init__(self, name, jointParent, placement):
38 def place(self, display, oMjoint):
39 oMbody = oMjoint * self.
placementplacement
40 display.place(self.
namename, oMbody,
False)
45 Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
46 The configuration is nq=7. The velocity is the same.
47 The members of the class are:
48 * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
49 * model: the kinematic tree of the robot.
50 * data: the temporary variables to be used by the kinematic algorithms.
51 * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
52 an object Visual (see above).
54 See tp1.py for an example of use.
58 """Create a Pinocchio model of a N-pendulum, with N the argument <nbJoint>."""
61 self.
modelmodel = pin.Model()
63 self.
datadata = self.
modelmodel.createData()
65 self.
q0q0 = np.zeros(self.
modelmodel.nq)
74 def createPendulum(self, nbJoint, rootId=0, prefix="", jointPlacement=None):
75 color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
76 colorred = [1.0, 0.0, 0.0, 1.0]
80 jointPlacement
if jointPlacement
is not None else pin.SE3.Identity()
84 inertia = pin.Inertia(
86 np.array([0.0, 0.0, length / 2]),
87 mass / 5 * np.diagflat([1e-2, length**2, 1e-2]),
90 for i
in range(nbJoint):
92 name = prefix +
"joint" + istr
93 jointName, _bodyName = [name +
"_joint", name +
"_body"]
94 jointId = self.
modelmodel.addJoint(
95 jointId, pin.JointModelRY(), jointPlacement, jointName
97 self.
modelmodel.appendBodyToJoint(jointId, inertia, pin.SE3.Identity())
99 self.
viewerviewer.viewer.gui.addSphere(
100 "world/" + prefix +
"sphere" + istr, 0.15, colorred
105 Visual(
"world/" + prefix +
"sphere" + istr, jointId, pin.SE3.Identity())
108 self.
viewerviewer.viewer.gui.addCapsule(
109 "world/" + prefix +
"arm" + istr, 0.1, 0.8 * length, color
115 "world/" + prefix +
"arm" + istr,
117 pin.SE3(np.eye(3), np.array([0.0, 0.0, length / 2])),
120 jointPlacement = pin.SE3(np.eye(3), np.array([0.0, 0.0, length]))
122 self.
modelmodel.addFrame(
123 pin.Frame(
"tip", jointId, 0, jointPlacement, pin.FrameType.OP_FRAME)
127 pin.forwardKinematics(self.
modelmodel, self.
datadata, q)
128 for visual
in self.
visualsvisuals:
129 visual.place(self.
viewerviewer, self.
datadata.oMi[visual.jointParent])
130 self.
viewerviewer.viewer.gui.refresh()
134 return self.
modelmodel.nq
138 return self.
modelmodel.nv
142 return self.
nqnq + self.
nvnv
152 def reset(self, x0=None):
154 q0 = np.pi * (rand(self.
nqnq) * 2 - 1)
155 v0 = rand(self.
nvnv) * 2 - 1
156 x0 = np.vstack([q0, v0])
157 assert len(x0) == self.
nxnx
160 return self.
obsobs(self.
xx)
163 assert len(u) == self.
nunu
164 _, self.
rr = self.
dynamicsdynamics(self.
xx, u)
165 return self.
obsobs(self.
xx), self.
rr
170 [np.vstack([np.cos(qi), np.sin(qi)])
for qi
in x[: self.
nqnq]]
177 """Return the altitude of pendulum tip"""
178 pin.framesKinematics(self.
modelmodel, self.
datadata, q)
179 return self.
datadata.oMf[1].translation[2, 0]
183 Dynamic function: x,u -> xnext=f(x,y).
184 Put the result in x (the initial value is destroyed).
185 Also compute the cost of making this step.
186 Return x for convenience along with the cost.
190 return (th + np.pi) % (2 * np.pi) - np.pi
193 return np.sum(np.square(x))
196 q = modulePi(x[: self.
nqnq])
198 u = np.clip(np.reshape(np.array(u), [self.
nunu, 1]), -self.
umaxumax, self.
umaxumax)
200 DT = self.
DTDT / self.
NDTNDT
201 for i
in range(self.
NDTNDT):
202 pin.computeAllTerms(self.
modelmodel, self.
datadata, q, v)
204 b = self.
datadata.nle
206 a = inv(M) * (u - self.
KfKf * v - b)
210 cost += (sumsq(q) + 1e-1 * sumsq(v) + 1e-3 * sumsq(u)) * DT
216 x[: self.
nqnq] = modulePi(q)
217 x[self.
nqnq :] = np.clip(v, -self.
vmaxvmax, self.
vmaxvmax)
222 q = self.
xx[: self.
nqnq]
224 time.sleep(self.
DTDT / 10)
def __init__(self, nbJoint=1)
def dynamics(self, x, u, display=False)
def createPendulum(self, nbJoint, rootId=0, prefix="", jointPlacement=None)