simulator.py 6.93 KB
Newer Older
Galo's avatar
Galo committed
1
#from wrapper import Wrapper
2
3
4
from viewer_utils import Viewer
import numpy as np 
from pinocchio.utils import zero as mat_zeros
5
import pinocchio as se3
6
EPS = 1e-4
7
8
9
10
11
12
13

class Simulator(object):
    DISPLAYCOM = True
    DISPLAYJOINTFRAMES = False
    COM_SPHERE_RADIUS = 0.01
    COM_SPHERE_COLOR = (1, 0, 0, 1)

Galo Maldonado's avatar
update    
Galo Maldonado committed
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
106
107
108
109
110
111
112
    '''
    name = ''
    q = None;   # current positions
    v = None;   # current velocities

    LCP = None;   # approximate LCP solver using staggered projections
    
    USE_LCP_SOLVER          = True;
    DETECT_CONTACT_POINTS   = True;   
    #'' True: detect collisions between feet and ground, False: collision is specified by the user ''
    GROUND_HEIGHT           = 0.0;
    LOW_PASS_FILTER_INPUT_TORQUES = False;
    
    ENABLE_FORCE_LIMITS = True;
    ENABLE_TORQUE_LIMITS = True;
    ENABLE_JOINT_LIMITS = True;
    
    ACCOUNT_FOR_ROTOR_INERTIAS = False;

    VIEWER_DT = 0.05;
    DISPLAY_COM = True;
    DISPLAY_CAPTURE_POINT = True;
    COM_SPHERE_RADIUS           = 0.01;
    CAPTURE_POINT_SPHERE_RADIUS = 0.01;
    CONTACT_FORCE_ARROW_RADIUS  = 0.01;
    COM_SPHERE_COLOR            = (1, 0, 0, 1); # red, green, blue, alpha
    CAPTURE_POINT_SPHERE_COLOR  = (0, 1, 0, 1);    
    CONTACT_FORCE_ARROW_COLOR   = (1, 0, 0, 1);
    CONTACT_FORCE_ARROW_SCALE   = 1e-3;
    contact_force_arrow_names = [];  # list of names of contact force arrows
    
    SLIP_VEL_THR = 0.1;
    SLIP_ANGVEL_THR = 0.2;
    NORMAL_FORCE_THR = 5.0;
    JOINT_LIMITS_DQ_THR = 1e-1; #1.0;
    TORQUE_VIOLATION_THR = 1.0;
    DQ_MAX = 9.14286;
                       
    ENABLE_WALL_DRILL_CONTACT = False;
    wall_x = 0.5;
    wall_damping = np.array([30, 30, 30, 0.3, 0.3, 0.3]);
    
    k=0;    # number of contact constraints (i.e. size of contact force vector)
    na=0;   # number of actuated DoFs
    nq=0;   # number of position DoFs
    nv=0;   # number of velocity DoFs
    r=[];   # robot
    
    mu=[];          # friction coefficient (force, moment)
    fMin = 0;       # minimum normal force

    dt = 0;     # time step used to compute joint acceleration bounds
    qMin = [];  # joint lower bounds
    qMax = [];  # joint upper bounds
    
    #'' Mapping between y and tau: y = C*tau+c ''
    C = [];
    c = [];
    
    M = [];         # mass matrix
    Md = [];        # rotor inertia
    h = [];         # dynamic drift
    q = [];
    dq = [];

    x_com = [];     # com 3d position
    dx_com = [];    # com 3d velocity
    ddx_com = [];   # com 3d acceleration
    cp = None;      # capture point
#    H_lankle = [];  # left ankle homogeneous matrix
#    H_rankle = [];  # right ankle homogeneous matrix
    J_com = [];     # com Jacobian
    Jc = [];        # contact Jacobian
    
    Minv = [];      # inverse of the mass matrix
    Jc_Minv = [];   # Jc*Minv
    Lambda_c = [];  # task-space mass matrix (Jc*Minv*Jc^T)^-1
    Jc_T_pinv = []; # Lambda_c*Jc_Minv
    Nc_T = [];      # I - Jc^T*Jc_T_pinv
    S_T = [];       # selection matrix
    dJc_v = [];     # product of contact Jacobian time derivative and velocity vector: dJc*v
    
    candidateContactConstraints = [];
    rigidContactConstraints = [];
#    constr_rfoot = None;
#    constr_lfoot = None;
#    constr_rhand = None;
    
#    f_rh = [];      # force right hand
#    w_rf = [];      # wrench right foot
#    w_lf = [];      # wrench left foot
    
    #'' debug variables ''
    x_c = [];       # contact points
    dx_c = [];      # contact points velocities
    x_c_init = [];  # initial position of constrained bodies    
        
    viewer = None;
    '''
Galo Maldonado's avatar
Galo Maldonado committed
113
114
115
116
117
118
119
120
121
122
    def reset(self, t, q, v, dt, nv):
        n = nv
        self.robot.dt = 0.0025 #dt
        self.robot.t = 0. #t
        self.robot.time_step = 0
        self.robot.q = np.matrix.copy(q)
        self.robot.v = np.matrix.copy(v)
        self.robot.vOld = np.matrix.copy(v)
        self.robot.dv = np.asmatrix(np.zeros(n)).T
        self.robot.time_step = 0
Galo's avatar
Galo committed
123
    
Galo Maldonado's avatar
Galo Maldonado committed
124
125
126
    def __init__(self, name, robot=None):
        if robot is None:
            pass
127
        self.name = name
Galo's avatar
Galo committed
128
        self.robot = robot
Galo Maldonado's avatar
Galo Maldonado committed
129
130
131
132
        self.reset(0, robot.q, robot.v, robot.dt, robot.nv)
        self.viewer = Viewer(self.robot.name, self.robot)
        self.updateRobotConfig(self.robot.q0)
        self.viewer.display(self.robot.q0,robot.name)
Galo Maldonado's avatar
update    
Galo Maldonado committed
133
134
        #self.fMin = 0.001 
        #self.mu = np.array([ 0.3,  0.1])
Galo Maldonado's avatar
Galo Maldonado committed
135
136
        #self.nq = self.robot.nq
        #self.nv = self.robot.nv
Galo Maldonado's avatar
update    
Galo Maldonado committed
137
        #self.na = self.nv        
Galo Maldonado's avatar
Galo Maldonado committed
138
139
140

        #if(self.DISPLAYCOM):
        #    self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, mat_zeros(3), mat_zeros(3), self.COM_SPHERE_COLOR, 'OFF')
Galo's avatar
Galo committed
141
    
Galo Maldonado's avatar
Galo Maldonado committed
142
143
144
145
146
147
    def updateRobotConfig(self, q): 
        se3.computeAllTerms(self.robot.model,
                            self.robot.data,
                            q,
                            self.robot.v)
        se3.framesKinematics(self.robot.model, self.robot.data, q)
148
        se3.computeJacobians(self.robot.model, self.robot.data, q)
Galo Maldonado's avatar
Galo Maldonado committed
149

150
151
152
153
154
155
156
157
158
159
    ''' ********** SET ROBOT STATE ******* '''
    def setPositions(self, q):
        self.q = np.matrix.copy(q);
        self.viewer.updateRobotConfig(q)
        return self.q
    
    def setVelocities(self, v):
        self.v = np.matrix.copy(v);
        return self.v;
    
160
161
162
    def increment(self, q, v, dt, t, updateViewer=True):
        self.t = t
        self.time_step +=1 
163
164
        self.robot.v = v.copy()*dt
        self.q = se3.integrate(self.robot.model, q.copy(), self.robot.v)
165
        self.viewer.updateRobotConfig(self.q, self.robotName )
Galo Maldonado's avatar
Galo Maldonado committed
166
        
167
    def increment2(self, q, dv, dt, t, updateViewer=True):
Galo Maldonado's avatar
Galo Maldonado committed
168
169
170
171
172
        self.robot.t = t
        self.robot.time_step +=1 
        self.robot.dv = dv.copy()
        if(abs(np.linalg.norm(self.robot.q[3:7])-1.0) > EPS):
            print "SIMULATOR ERROR Time %.3f "%t, 
Galo's avatar
Galo committed
173
            "norm of quaternion is not 1=%f" % np.linalg.norm(self.robot.q[3:7])
Galo Maldonado's avatar
Galo Maldonado committed
174
        self.robot.q  = se3.integrate(self.robot.model, q.copy(), self.robot.v*dt)
175
        self.robot.v += dv.copy()*dt 
Galo Maldonado's avatar
Galo Maldonado committed
176
        self.updateRobotConfig(self.robot.q)
Galo Maldonado's avatar
Galo Maldonado committed
177
178
        if updateViewer is True:
            self.viewer.display(self.robot.q.copy(), self.robot.name)
Galo Maldonado's avatar
Galo Maldonado committed
179
        
Galo Maldonado's avatar
Galo Maldonado committed
180
181
182
183
184
185
186
187
188
    def integrateAcc(self, t, dt, dv, f, tau, updateViewer=True):
        self.t = t;
        self.time_step += 1;
        self.dv = np.matrix.copy(dv);
        
        if(abs(norm(self.q[3:7])-1.0) > EPS):
            print "SIMULATOR ERROR Time %.3f "%t, "norm of quaternion is not 1=%f" % norm(self.q[3:7]);
            
        ''' Integrate velocity and acceleration '''
189
        self.q  = se3.integrate(self.robot.model, self.q, dt*self.v);
Galo Maldonado's avatar
Galo Maldonado committed
190
        self.v += dt*self.dv;
191
        self.viewer.updateRobotConfig(self.q, self.robotName ) 
Galo Maldonado's avatar
Galo Maldonado committed
192
    
193
194
195
196
197
198
199
    ''' ---------- VIEWER ------------ '''
    def updateComPositionInViewer(self, com):
        assert np.asarray(com).squeeze().shape[0]==3, "com should be a 3x1 matrix"
        com = np.asarray(com).squeeze();
        if(self.time_step%int(self.VIEWER_DT/self.dt)==0):
            if(self.DISPLAY_COM):
                self.viewer.updateObjectConfig('com', (com[0], com[1], com[2], 0.,0.,0.,1.));
Galo Maldonado's avatar
Galo Maldonado committed
200
201