aro_lab/inverse_geometry.py at main · rrwwee/aro_lab · GitHub

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
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Wed Sep  6 15:32:51 2023

@author: stonneau
"""

import pinocchio as pin
import numpy as np
from numpy.linalg import pinv,inv,norm,svd,eig
import time
from tools import collision, getcubeplacement, setcubeplacement, projecttojointlimits
from config import LEFT_HOOK, RIGHT_HOOK, LEFT_HAND, RIGHT_HAND, EPSILON
from config import CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET

from tools import setcubeplacement

def computeqgrasppose(robot, qcurrent, cube, cubetarget, viz=None, max_attempts: int = 1000, viz_sleep: bool = True):
    '''Return a collision free configuration grasping a cube at a specific location and a success flag.

    Args:
        robot: RobotWrapper
        qcurrent: initial joint configuration to seed the IK
        cube: cube body
        cubetarget: desired cube SE3 placement
        viz: optional visualizer
        max_attempts: maximum number of IK iteration steps to attempt (default 1000)
    '''
    q = qcurrent.copy()
    # Ensure the cube is placed at the requested target before solving IK
    setcubeplacement(robot, cube, cubetarget)

    lhand_id = robot.model.getFrameId(LEFT_HAND)
    rhand_id = robot.model.getFrameId(RIGHT_HAND)

    oMlhook = getcubeplacement(cube, LEFT_HOOK)
    oMrhook = getcubeplacement(cube, RIGHT_HOOK)

    success = False

    for i in range(max_attempts):
        pin.framesForwardKinematics(robot.model, robot.data, q)
        pin.computeJointJacobians(robot.model, robot.data, q)

        oMlhand = robot.data.oMf[lhand_id]
        oMrhand = robot.data.oMf[rhand_id]

        lhandMlhook = oMlhand.inverse()*oMlhook
        rhandMrhook = oMrhand.inverse()*oMrhook

        nu_L = pin.log(lhandMlhook).vector
        nu_R = pin.log(rhandMrhook).vector

        J_L = pin.computeFrameJacobian(robot.model, robot.data, q, lhand_id, pin.LOCAL)
        J_R = pin.computeFrameJacobian(robot.model, robot.data, q, rhand_id, pin.LOCAL)

        JL_p = pinv(J_L)
        v1 = JL_p @ nu_L
        N1 = np.eye(robot.nv) - JL_p @ J_L

        JRN = J_R @ N1
        JRN_p = pinv(JRN)

        v2 = JRN_p @ (nu_R - J_R @ v1)
        vq = v1 + N1 @ v2

        N2 = N1 - JRN_p @ JRN @ N1
        v3 = pin.difference(robot.model, q, robot.q0)

        vq += N2 @ v3

        q = pin.integrate(robot.model,q, vq)

        lerror = norm(oMlhand.translation - oMlhook.translation)
        rerror = norm(oMrhand.translation - oMrhook.translation)

        if viz:
            # allow callers to request that we display without sleeping so the
            # planner can validate IK quickly without incurring a sleep cost.
            viz.display(q)
            if viz_sleep:
                time.sleep(0.1)

        if lerror < EPSILON and rerror < EPSILON and not collision(robot, q):
            success = True
            break

    return q, success


if __name__ == "__main__":
    from tools import setupwithmeshcat
    from setup_meshcat import updatevisuals
    robot, cube, viz = setupwithmeshcat()

    q = robot.q0.copy()

    q0,successinit = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT, viz)
    qe,successend = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT_TARGET,  viz)

    updatevisuals(viz, robot, cube, q0)