Inverse Kinematics

This example demonstrates how to solve inverse kinematics of Atlas. It solves the problem using Sequential Quadratic Programming from the scipy.optimize package.

Screenshot

../_images/inverse_kinematics.png

Code

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
class MyWorld(pydart.World):

    def __init__(self, ):
        pydart.World.__init__(self, 0.001)
        self.set_gravity([0.0, 0.0, -9.81])
        print('pydart create_world OK')

        filename = "./data/sdf/atlas/atlas_v3_no_head.sdf"
        self.robot = self.add_skeleton(filename)
        self.robot.set_root_joint_to_trans_and_euler()
        print('pydart add_skeleton OK')

        self.theta = 0.0 * np.pi
        self.update_target()
        self.solve()

        print("click step to rotate the target")

    def update_target(self, ):
        th, r = self.theta - 0.5 * np.pi, 0.6
        x, y = r * np.cos(th) + 0.4, r * np.sin(th)
        self.target = np.array([x, y, 0.3])

    def set_params(self, x):
        q = self.robot.positions()
        q[6:] = x
        self.robot.set_positions(q)

    def f(self, x):
        self.set_params(x)

        lhs = self.robot.body("l_hand").to_world([0.0, 0.0, 0.0])
        rhs = self.target
        return 0.5 * np.linalg.norm(lhs - rhs) ** 2

    def g(self, x):
        self.set_params(x)

        lhs = self.robot.body("l_hand").to_world([0.0, 0.0, 0.0])
        rhs = self.target
        J = self.robot.body("l_hand").linear_jacobian()
        g = (lhs - rhs).dot(J)[6:]

        DEBUG = True
        if DEBUG:  # Debug by comparing with the numerical computation
            from pydart2.utils.misc import grad
            lhs = g
            rhs = grad(self.f, x, 1e-5)
            print(lhs)
            print(rhs)
            print("OK" if np.allclose(lhs, rhs) else "NG!!!!")

        return g

    def solve(self, ):
        res = minimize(self.f,
                       x0=self.robot.positions()[6:],
                       jac=self.g,
                       method="SLSQP")
        print(">>> theta = %.4f" % self.theta)
        print(res)

    def step(self, ):
        super(MyWorld, self).step()
        self.theta = (self.theta + pydart.utils.misc.deg2rad(10)) % np.pi
        self.update_target()
        self.solve()

    def render_with_ri(self, ri):
        ri.set_color(1.0, 0.0, 0.0)
        ri.render_sphere(self.target, 0.05)


if __name__ == '__main__':
    print('Example: inverse kinematics')

    pydart.init()
    print('pydart initialization OK')

    world = MyWorld()

    win = pydart.gui.pyqt5.window.PyQt5Window(world)
    win.scene.set_camera(1)  # Z-up Camera
    win.run()