Debian Codegraph @

python/examples/operational_space_control/main.py

raw
1 import numpy as np
2 import dartpy as dart
3
4
5 class HelloWorldNode(dart.gui.osg.RealTimeWorldNode):
6 def __init__(self, world, kr5):
7 super(HelloWorldNode, self).__init__(world)
8 self.kr5 = kr5
9 self.dofs = self.kr5.getNumDofs()
10 self.ee = kr5.getBodyNode('palm')
11 self.Kp = np.eye(3) * 50.0
12 self.Kd = np.eye(self.dofs) * 5.0
13 self.ee_offset = [0.05, 0, 0]
14
15 tf = self.ee.getTransform()
16 tf.pretranslate(self.ee_offset)
17 self.target = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), "target", tf)
18
19 def customPreStep(self):
20 M = self.kr5.getMassMatrix()
21
22 J = self.ee.getLinearJacobian()
23 Jt = J.transpose();
24 JJt = np.matmul(J, Jt)
25 kI = 0.0025 * np.eye(3)
26 invJ = np.matmul(Jt, np.linalg.inv(JJt + kI))
27
28 dJ = self.ee.getLinearJacobianDeriv()
29 dJt = dJ.transpose()
30 dJdJt = np.matmul(dJ, dJt)
31 invdJ = np.matmul(dJt, np.linalg.inv(dJdJt + kI))
32
33 e = self.target.getTransform().translation() - self.ee.getTransform().translation()
34 de = -self.ee.getLinearVelocity()
35
36 cg = self.kr5.getCoriolisAndGravityForces()
37
38 tmp1 = np.matmul(np.matmul(invJ, self.Kp), de)
39 tmp2 = np.matmul(np.matmul(invdJ, self.Kp), e)
40
41 forces1 = np.matmul(M, tmp1 + tmp2)
42 forces2 = cg
43 forces3 = np.matmul(np.matmul(np.matmul(self.Kd, invJ), self.Kp), e)
44
45 forces = forces1 + forces2 + forces3
46
47 self.kr5.setForces(forces)
48
49
50 def main():
51 world = dart.simulation.World()
52
53 urdfParser = dart.utils.DartLoader()
54 kr5 = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf")
55 ground = urdfParser.parseSkeleton("dart://sample/urdf/KR5/ground.urdf")
56 world.addSkeleton(kr5)
57 world.addSkeleton(ground)
58 world.setGravity([0, -9.81, 0])
59
60 node = HelloWorldNode(world, kr5)
61
62 # Create world node and add it to viewer
63 viewer = dart.gui.osg.Viewer()
64 viewer.addWorldNode(node)
65
66 # Grid settings
67 grid = dart.gui.osg.GridVisual()
68 grid.setPlaneType(dart.gui.osg.GridVisual.PlaneType.ZX)
69 grid.setOffset([0, -0.55, 0])
70 viewer.addAttachment(grid)
71
72 viewer.setUpViewInWindow(0, 0, 640, 480)
73 viewer.setCameraHomePosition([2.0, 1.0, 2.0],
74 [0.00, 0.00, 0.00],
75 [-0.24, 0.94, -0.25])
76 viewer.run()
77
78
79 if __name__ == "__main__":
80 main()