solvers.py 6.92 KB
Newer Older
Galo Maldonado's avatar
Galo Maldonado committed
1
import numpy as np
2
from wrapper import Wrapper
3
import scipy
Galo Maldonado's avatar
update    
Galo Maldonado committed
4
from abstract_solver import AbstractSolver
Galo Maldonado's avatar
Galo Maldonado committed
5
6

class NProjections():
7
    def reset(self,q,v,dt):
8
9
        self.robot.q = q
        self.robot.v = v
10
11
        self.dt = dt 
        self.t = 0.0
Galo Maldonado's avatar
Galo Maldonado committed
12
13
        self.tasks = []
        self.task_weights = []
14
15
16
17
18
19
20
21
22
        self.stack = []

    def __init__(self, name, q, v, dt, robotName, robot):
        self.name = name
        self.robot = robot 
        self.nq = self.robot.nq
        self.nv = self.robot.nv
        self.na = self.nv-6
        self.reset(q,v,dt)
Galo Maldonado's avatar
Galo Maldonado committed
23

Galo's avatar
Galo committed
24
    def null(self, A, eps=1e-12):
Galo Maldonado's avatar
Galo Maldonado committed
25
26
27
28
29
30
31
32
        '''Compute a base of the null space of A.'''
        u, s, vh = np.linalg.svd(A)
        padding = max(0,np.shape(A)[1]-np.shape(s)[0])
        null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
        null_space = scipy.compress(null_mask, vh, axis=0)
        return scipy.transpose(null_space)

    def addTask(self, task, weight):
Galo Maldonado's avatar
Galo Maldonado committed
33
        ''' append a new task and weight to the stack '''
Galo Maldonado's avatar
Galo Maldonado committed
34
35
        self.tasks        += [task]
        self.task_weights += [weight]
36
        
Galo Maldonado's avatar
Galo Maldonado committed
37
38
39
40
41
42
43
44
    def removeTask(self, task_name):
        for (i,t) in enumerate(self.tasks):
            if t.name==task_name:
                del self.tasks[i]
                del self.task_weights[i]
                return True
            raise ValueError("[InvDynForm] ERROR: task %s cannot be removed because it does not exist!" % task_name);

45
46
47
48
    def emptyStack(self):
        self.tasks = []
        self.task_weights = []

49
    def inverseKinematics1st(self,t):
Galo Maldonado's avatar
Galo Maldonado committed
50
51
52
53
54
        ''' 
        Hierarchichal Inverse Kinematics formulation based on nullspace projection
        q_dot
        ERRstack contain a stack of desired velocities in the operational space
        '''
55
        Jstack = []
56
57
        ERRstack = []
        Z = []
58
        if len(self.tasks)==1:
59
60
61
62
63
64
65
66
67
68
69
            if np.size(self.tasks[0]) > 1:
                j = []; e = []; d =[]
                for w in xrange(len(self.tasks[0])):
                    x1, x3 = self.tasks[0][w].kyn_value(t, self.robot.q)
                    j.append(x1); e.append(x3);
                    J = np.vstack(j); E = np.vstack(e);
            else :        
                J, E = self.tasks[0].kyn_value(t, self.robot.q)
            
            Jstack.append(J); Dstack.append(D); ERRstack.append(E);
            q_dot = np.linalg.pinv(Jstack[0])*(ERRstack[0])
70
71
72
73
            return q_dot

        else:
            for i in xrange (len(self.tasks)):
74
                #_Stack jacobians and task functions
75

Galo Maldonado's avatar
Galo Maldonado committed
76
                #if tasks are combined at same hierarchy
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
                if np.size(self.tasks[i]) > 1:
                    j = []
                    e = []
                    for w in xrange(len(self.tasks[i])):
                        x1, x2 = self.tasks[i][w].kin_value(t, self.robot.q)
                        j.append(x1)
                        e.append(x2)
                        J = np.vstack(j)
                        E = np.vstack(e)
                        
                else :        
                    J, E = self.tasks[i].kin_value(t, self.robot.q)
                    
                # stack them
                Jstack.append(J)
                ERRstack.append(E)

            #_Solve HQP
            q_dot = np.linalg.pinv(Jstack[0])*ERRstack[0]
            Z = self.null(Jstack[0]) 
Galo Maldonado's avatar
Galo Maldonado committed
97
            for k in range (1, len(Jstack)):
98
99
                Jplus = Z*np.linalg.pinv(Jstack[k]*Z) 
                x_dot = (ERRstack[k] - Jstack[k]*q_dot) 
Galo Maldonado's avatar
Galo Maldonado committed
100
                q_dot += Jplus*x_dot
101
102
                Z = self.null(Jstack[k]*Z*Z.T) 

Galo Maldonado's avatar
Galo Maldonado committed
103
104
        return q_dot
    
105
    def inverseKinematics2nd(self, t):
Galo Maldonado's avatar
Galo Maldonado committed
106
107
108
109
110
        ''' 
        Hierarchichal Inverse Kinematics formulation based on nullspace projection
        for q_dot_dot
        ERRstack contain the desired accelerations in the operation space
        '''
111
112
113
114
115
        Jstack = []
        ERRstack = []
        Dstack = []
        Z = []
        if len(self.tasks)==1:
116
117
118
119
120
121
122
123
124
125
126
127
            # if tasks are combined at same hierarchy
            if np.size(self.tasks[0]) > 1:
                j = []; e = []; d =[]
                for w in xrange(len(self.tasks[0])):
                    x1, x2, x3 = self.tasks[0][w].dyn_value(t, self.robot.q, self.robot.v)
                    j.append(x1); d.append(x2); e.append(x3);
                    J = np.vstack(j); D = np.vstack(d); E = np.vstack(e);
            else :        
                J, D, E = self.tasks[0].dyn_value(t, self.robot.q, self.robot.v)
            
            Jstack.append(J); Dstack.append(D); ERRstack.append(E);
            q_dot_dot = np.linalg.pinv(Jstack[0])*(ERRstack[0] - Dstack[0])
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
            return q_dot_dot

        else:
            for i in xrange (len(self.tasks)):
                #_Stack jacobians and task functions

                # if tasks are combined at same hierarchy
                if np.size(self.tasks[i]) > 1:
                    j = []; e = []; d =[]
                    for w in xrange(len(self.tasks[i])):
                        x1, x2, x3 = self.tasks[i][w].dyn_value(t, self.robot.q, self.robot.v)
                        j.append(x1); d.append(x2); e.append(x3);
                        J = np.vstack(j); D = np.vstack(d); E = np.vstack(e);
                        
                else :        
                    J,D, E = self.tasks[i].dyn_value(t, self.robot.q, self.robot.v)
                # stack them
                Jstack.append(J); Dstack.append(D); ERRstack.append(E);

            #_Solve HQP
            q_dot_dot = np.linalg.pinv(Jstack[0])*(ERRstack[0]-Dstack[0])
            Z = self.null(Jstack[0]) 
Galo Maldonado's avatar
Galo Maldonado committed
150
            for k in range (1, len(Jstack)):
151
152
153
154
155
156
                Jplus = Z*np.linalg.pinv(Jstack[k]*Z) 
                x_dot_dot = (ERRstack[k] - Jstack[k]*q_dot_dot) 
                q_dot_dot += Jplus*(x_dot_dot-Dstack[k])
                Z = self.null(Jstack[k]*Z*Z.T) 

        return q_dot_dot
Galo Maldonado's avatar
Galo Maldonado committed
157

158

Galo Maldonado's avatar
Galo Maldonado committed
159
160
161
162
163
164
165
166
    def inverseDynamics(self, robot, Jstack, drift, ERRstack):
        ''' 
        Hierarchichal Inverse Dynamics formulation based on nullspace projection
        '''
        #coriolis and centrifugal terms + gravity vector
        b = 1
        tau += np.linalg.pinv(Jstack[0]/robot.M) * ( ERRstack+((J/robot.M)*b)-dift )
        Z += self.null(Jstack[k]/robot.M) * Z[k-1]
167
        return Z
Galo Maldonado's avatar
Galo Maldonado committed
168

Galo Maldonado's avatar
update    
Galo Maldonado committed
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199



class StandardQpSolver (AbstractSolver):
    """
    Nonrobust inverse dynamics solver for the problem:
    min ||D*x - d||^2
    s.t.  lbA <= A*x <= ubA 
    """

    def __init__(self, n, m_in, solver='slsqp', accuracy=1e-6, maxIter=100, verb=0):
        AbstractSolver.__init__(self, n, m_in, solver, accuracy, maxIter, verb);
        self.name = "Classic TSID";
        
    def f_cost(self,x):
        e = np.dot(self.D, x) - self.d;
        return 0.5*np.dot(e.T,e);
    
    def f_cost_grad(self,x):
        return np.dot(self.H,x) - self.dD;
        
    def f_cost_hess(self,x):
        return self.H;

    def get_linear_inequality_matrix(self):
        return self.A;
          
    def get_linear_inequality_vectors(self):
        return (self.lbA, self.ubA);