예제 #1
0
    def calc(self, data, x, u=None):
        if u is None:
            u = self.unone
        nq, nv = self.nq, self.nv
        q = a2m(x[:nq])
        v = a2m(x[-nv:])

        pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
        pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)

        data.tauq[:] = self.actuation.calc(data.actuation, x, u)
        self.contact.calc(data.contact, x)

        data.K[:nv, :nv] = data.pinocchio.M
        if hasattr(self.pinocchio, 'armature'):
            data.K[range(nv), range(nv)] += self.pinocchio.armature.flat
        data.K[nv:, :nv] = data.contact.J
        data.K.T[nv:, :nv] = data.contact.J
        data.Kinv = np.linalg.inv(data.K)

        data.r[:nv] = data.tauq - m2a(data.pinocchio.nle)
        data.r[nv:] = -data.contact.a0
        data.af[:] = np.dot(data.Kinv, data.r)
        data.f[:] *= -1.

        # Convert force array to vector of spatial forces.
        self.contact.setForces(data.contact, data.f)

        data.cost = self.costs.calc(data.costs, x, u)
        return data.xout, data.cost
예제 #2
0
    def calc(self, data, x, u=None):
        self.costsData.shareMemory(data)
        if u is None:
            u = self.unone
        q, v = x[:self.state.nq], x[-self.state.nv:]
        self.actuation.calc(self.actuationData, x, u)
        tau = self.actuationData.tau
        # Computing the dynamics using ABA or manually for armature case
        if self.enable_force:
            data.xout[:] = pinocchio.aba(self.state.pinocchio,
                                         self.pinocchioData, q, v, tau)
        else:
            pinocchio.computeAllTerms(self.state.pinocchio, self.pinocchioData,
                                      q, v)
            data.M = self.pinocchioData.M
            if self.armature.size == self.state.nv:
                data.M[range(self.state.nv),
                       range(self.state.nv)] += self.armature
            self.Minv = np.linalg.inv(data.M)
            data.xout = self.Minv * (tau - self.pinocchioData.nle)

        # Computing the cost value and residuals
        pinocchio.forwardKinematics(self.state.pinocchio, self.pinocchioData,
                                    q, v)
        pinocchio.updateFramePlacements(self.state.pinocchio,
                                        self.pinocchioData)
        self.costs.calc(self.costsData, x, u)
        data.cost = self.costsData.cost
예제 #3
0
    def log_state(self, logger, prefix):
        '''Log current state: the values logged are defined in CLAPTRAP_STATE_SUFFIXES
        @param logger Logger object
        @param prefix Prefix to add before each suffix.
        '''
        rpy = se3.rpy.matrixToRpy(
            se3.Quaternion(self.q[6, 0], self.q[3, 0], self.q[4, 0],
                           self.q[5, 0]).matrix())
        logger.set(prefix + "roll", rpy[0, 0])
        logger.set(prefix + "pitch", rpy[1, 0])
        logger.set(prefix + "yaw", rpy[2, 0])
        logger.set_vector(prefix + "omega", self.v[3:6, 0])
        logger.set(
            prefix + "wheelVelocity",
            self.v[self.robot.model.joints[self.robot.model.
                                           getJointId("WheelJoint")].idx_v, 0])

        se3.computeAllTerms(self.robot.model, self.robot.data, self.q, self.v)
        energy = self.robot.data.kinetic_energy + self.robot.data.potential_energy
        logger.set(prefix + "energy", energy)

        logger.set(prefix + "wheelTorque", self.tau[0, 0])

        w_M_base = self.robot.framePosition(
            self.q, self.robot.model.getFrameId("Body"), False)
        logger.set_vector(prefix + "base", w_M_base.translation)
예제 #4
0
    def update_state(self, q, v):
        nf, ny = self.nf, self.ny
        se3.computeAllTerms(self.robot.model, self.robot.data, q, v)
        se3.framesForwardKinematics(self.robot.model, self.robot.data, q) # replace with updateFramePlacements(model, data)
        M = self.robot.data.M        #(7,7)
        Mj_diag = np.matrix(np.diag(np.diag(M[ny:,ny:])))
        Jl,Jr = self.robot.get_Jl_Jr_world(q, False)
        J = np.vstack([Jl[1:3],Jr[1:3]])    # (4, 7)    
        Mlf, Mrf = self.robot.get_Mlf_Mrf(q, False) 
        pyl, pzl = Mlf.translation[1:].A1
        pyr, pzr = Mrf.translation[1:].A1
        com, com_vel = self.robot.get_com_and_derivatives(q, v)
        cy, cz     = com.A1
        X_am  = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)])
#        X_com = np.hstack([np.eye(2)/M[0,0],np.eye(2)/M[0,0]])
        X_com = np.hstack([np.eye(2),np.eye(2)])
        self.X = np.vstack([X_com, X_am])
        self.X_pinv = np.linalg.pinv(self.X)
        
        self.A[  ny:2*ny, 2*ny:2*ny+nf] = self.X
        self.H[  ny:2*ny, 2*ny:2*ny+nf] = self.X
        
        self.XT_Mb_inv = self.X.T * np.linalg.inv(M[:ny,:ny])
        
        Minv = np.linalg.inv(M)
        Upsilon = J*Minv*J.T
        S = matlib.zeros((self.na, self.nv))
        S[:,self.nv-self.na:] = matlib.eye(self.na)
        JSTpinv = np.linalg.pinv(J*S.T)        
        A = J*Minv*S.T*Mj_diag*JSTpinv
        self.K_A = self.K*A
        self.K_Upsilon = self.K*Upsilon
예제 #5
0
    def f(self, x, u, t, jacobian=False):
        nq = self.robot.nq
        nv = self.robot.nv
        model = self.robot.model
        data = self.robot.data
        q = x[:nq]
        v = x[nq:]

        if (nv == 1):
            # for 1 DoF systems pin.aba does not work (I don't know why)
            pin.computeAllTerms(model, data, q, v)
            ddq = (u - data.nle) / data.M[0]
        else:
            ddq = pin.aba(model, data, q, v, u)

        self.dx[:nv] = v
        self.dx[nv:] = ddq

        if (jacobian):
            pin.computeABADerivatives(model, data, q, v, u)
            self.Fx[:nv, :nv] = 0.0
            self.Fx[:nv, nv:] = np.identity(nv)
            self.Fx[nv:, :nv] = data.ddq_dq
            self.Fx[nv:, nv:] = data.ddq_dv
            self.Fu[nv:, :] = data.Minv

            return (np.copy(self.dx), np.copy(self.Fx), np.copy(self.Fu))

        return np.copy(self.dx)
예제 #6
0
    def __call__(self, q, vq):
        robot = self.robot
        NV = robot.model.nv
        NJ = robot.model.njoints
        C = self.C
        YS = self.YS
        Sxv = self.Sxv
        H = hessian(robot, q)

        se3.computeAllTerms(robot.model, robot.data, q, vq)
        J = robot.data.J
        oMi = robot.data.oMi
        v = [(oMi[i] * robot.data.v[i]).vector for i in range(NJ)]
        Y = [(oMi[i] * robot.data.Ycrb[i]).matrix() for i in range(NJ)]

        # --- Precomputations
        # Centroidal matrix
        for j in range(NJ):
            j0, j1 = iv(j)[0], iv(j)[-1] + 1
            YS[:, j0:j1] = Y[j] * J[:, j0:j1]

        # velocity-jacobian cross products.
        for j in range(NJ):
            j0, j1 = iv(j)[0], iv(j)[-1] + 1
            vj = v[j]
            Sxv[:, j0:j1] = adj(vj) * J[:, j0:j1]

        # --- Main loop
        for i in range(NJ - 1, 0, -1):
            i0, i1 = iv(i)[0], iv(i)[-1] + 1
            Yi = Y[i]
            Si = J[:, i0:i1]
            vp = v[robot.model.parents[i]]

            SxvY = Sxv[:, i0:i1].T * Yi
            for j in ancestors(i):
                j0, j1 = iv(j)[0], iv(j)[-1] + 1
                Sj = J[:, j0:j1]

                # COR ===> Si' Yi Sj x vj
                C[i0:i1, j0:j1] = YS[:, i0:i1].T * Sxv[:, j0:j1]
                # CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
                C[i0:i1, j0:j1] -= SxvY * Sj

            vxYS = adjdual(v[i]) * Yi * Si
            YSxv = Yi * Sxv[:, i0:i1]
            Yv = Yi * vp
            for ii in ancestors(i)[:-1]:
                ii0, ii1 = iv(ii)[0], iv(ii)[-1] + 1
                Sii = J[:, ii0:ii1]
                # COR ===> Sii' Yi Si x vi
                C[ii0:ii1, i0:i1] = Sii.T * YSxv
                # CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
                C[ii0:ii1, i0:i1] += Sii.T * vxYS
                # CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
                C[ii0:ii1, i0:i1] += np.matrix(
                    td(H[:, i0:i1, ii0:ii1], Yv, [0, 0])[:, :, 0]).T

        return C
예제 #7
0
파일: dcrba.py 프로젝트: nim65s/pinocchio
    def __call__(self,q,vq):
        robot = self.robot
        NV  = robot.model.nv
        NJ  = robot.model.njoints
        C   = self.C
        YS  = self.YS
        Sxv = self.Sxv
        H = hessian(robot,q)

        pin.computeAllTerms(robot.model,robot.data,q,vq)
        J = robot.data.J
        oMi=robot.data.oMi
        v = [ (oMi[i]*robot.data.v[i]).vector      for i in range(NJ) ]
        Y = [ (oMi[i]*robot.data.Ycrb[i]).matrix() for i in range(NJ) ]

        # --- Precomputations
        # Centroidal matrix
        for j in range(NJ):
            j0,j1 = iv(j)[0],iv(j)[-1]+1
            YS[:,j0:j1] = Y[j]*J[:,j0:j1]

        # velocity-jacobian cross products.
        for j in range(NJ):
            j0,j1 = iv(j)[0],iv(j)[-1]+1
            vj = v[j]
            Sxv[:,j0:j1] = adj(vj)*J[:,j0:j1]

        # --- Main loop
        for i in range(NJ-1,0,-1):
            i0,i1 = iv(i)[0],iv(i)[-1]+1
            Yi    = Y[i]
            Si    = J[:,i0:i1]
            vp = v[robot.model.parents[i]]

            SxvY = Sxv[:,i0:i1].T*Yi
            for j in ancestors(i):
                j0,j1 = iv(j)[0],iv(j)[-1]+1
                Sj      = J[:,j0: j1 ]

                # COR ===> Si' Yi Sj x vj
                C[i0:i1,j0:j1]  = YS[:,i0:i1].T*Sxv[:,j0:j1]
                # CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
                C[i0:i1,j0:j1] -= SxvY*Sj

            vxYS = adjdual(v[i])*Yi*Si
            YSxv = Yi*Sxv[:,i0:i1]
            Yv   = Yi*vp
            for ii in ancestors(i)[:-1]:
                ii0,ii1 = iv(ii)[0],iv(ii)[-1]+1
                Sii = J[:,ii0:ii1]
                # COR ===> Sii' Yi Si x vi
                C[ii0:ii1,i0:i1]  = Sii.T*YSxv
                # CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
                C[ii0:ii1,i0:i1] += Sii.T*vxYS
                # CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
                C[ii0:ii1,i0:i1] += np.matrix(td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0]).T

        return C
예제 #8
0
파일: claptrap.py 프로젝트: Siviuze/CL4P-TP
    def simulate(self, x0, simulation_duration, dt, motor_control_law, output_name = "/tmp/claptrap.csv", verbose = True):
        ''' Run a simulation of given controller motor_control_law, log the results, and return the state.
            @param x0 Initial state (position + velocity)
            @param simulation_duration Length of the simulation
            @param dt Timestep for logger - note that the controller is simulated in a continuous manner
            @param motor_control_law Motor callback law, with signature motor_control_law(t, q, v) -> torque
            @param output_name Optional, name of the output log file.
            @param verbose Optional, whether or not to display a progress bar during simulation.
        '''
        self.controller = motor_control_law

        # Create integrator.
        solver = scipy.integrate.ode(self._dynamics)
        solver.set_integrator('dopri5')
        solver.set_initial_value(x0)

        # Create logger
        logged_values = ["Claptrap.q" + str(i) for i in range(self.robot.model.nq)] + \
                        ["Claptrap.v" + str(i) for i in range(self.robot.model.nv)] + \
                        ["Claptrap.energy"]

        logger = Logger(logged_values)

        if verbose:
            pbar = tqdm.tqdm(total=simulation_duration, bar_format="{percentage:3.0f}%|{bar}| {n:.2f}/{total_fmt} [{elapsed}<{remaining}]")

        t = 0
        result_x = []
        while solver.successful() and t < simulation_duration:
            # Integrate, skip first iteration, we only want to log in this case
            if t > 0:
                solver.integrate(t)
                if verbose:
                    pbar.update(dt)

            result_x.append(solver.y)
            q = solver.y[:self.robot.nq]
            v = solver.y[self.robot.nq:]

            # Log
            for i in range(self.robot.model.nq):
                logger.set("Claptrap.q" + str(i), q[i])
            for i in range(self.robot.model.nv):
                logger.set("Claptrap.v" + str(i), v[i])
            pnc.computeAllTerms(self.robot.model, self.robot.data, q, v)
            energy = self.robot.data.kinetic_energy + self.robot.data.potential_energy
            logger.set("Claptrap.energy", energy)
            logger.set("time", t)
            logger.new_line()

            t += dt


        logger.save(output_name)
        # Return time and x
        return logger.data["time"][:-1], np.array(result_x)
예제 #9
0
 def cost(self, x):
     tauq, fr, fl = self.x2vars(x)
     pinocchio.computeAllTerms(self.rmodel, self.rdata, self.q, self.vq)
     b = self.rdata.nle
     pinocchio.updateFramePlacements(self.rmodel, self.rdata)
     Jr = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR,
                                     LOCAL)
     Jl = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idL,
                                     LOCAL)
     self.residuals = m2a(b - tauq - Jr.T * fr - Jl.T * fl)
     return sum(self.residuals**2)
예제 #10
0
    def test_forwardDynamics_rcoeff(self):
        data_no_q = self.model.createData()

        self.model.gravity = pin.Motion.Zero()
        ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
                                  self.tau0, self.J, self.gamma, r_coeff)
        self.assertLess(np.linalg.norm(ddq), self.tolerance)

        pin.computeAllTerms(self.model, data_no_q, self.q, self.v0)
        ddq_no_q = pin.forwardDynamics(self.model, data_no_q, self.tau0,
                                       self.J, self.gamma, r_coeff)
        self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)

        self.assertApprox(ddq, ddq_no_q)
예제 #11
0
    def updateRobotConfig(self, q, robotName, osimref=True):
        #self.robots[robotName].display(q, robotName)

        se3.computeAllTerms(self.robots[robotName].model,
                            self.robots[robotName].data, q,
                            self.robots[robotName].v)

        #se3.forwardKinematics(self.robots[robotName].model,
        #                      self.robots[robotName].data,
        #                      q)
        #se3.framesKinematics(self.robots[robotName].model,
        #                     self.robots[robotName].data,
        #                     q)
        self.display(q, robotName)
예제 #12
0
def c_control(dt, robot, sample, t_simu):
    eigenpy.switchToNumpyMatrix()

    qa = robot.q  # actuation joint position
    qa_dot = robot.v  # actuation joint velocity

    # Method : Joint Control A q_ddot = torque
    Kp = 400.  # Convergence gain
    ID_EE = robot.model.getFrameId(
        "LWrMot3")  # Control target (End effector) ID

    # Compute/update all joints and frames
    se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)

    # Get kinematics information
    oMi = robot.framePlacement(qa, ID_EE)  ## EE's placement
    v_frame = robot.frameVelocity(qa, qa_dot, ID_EE)  # EE's velocity
    J = robot.computeFrameJacobian(qa,
                                   ID_EE)  ## EE's Jacobian w.r.t Local Frame

    # Get dynamics information
    M = robot.mass(qa)  # Mass Matrix
    NLE = robot.nle(qa, qa_dot)  #gravity and Coriolis
    Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose())  # Lambda Matrix

    # Update Target oMi position
    wMl = copy.deepcopy(sample.pos)
    wMl.rotation = copy.deepcopy(oMi.rotation)
    v_frame_ref = se3.Motion()  # target velocity (v, w)
    v_frame_ref.setZero()

    # Get placement Error
    p_error = se3.log(sample.pos.inverse() * oMi)
    v_error = v_frame - wMl.actInv(v_frame_ref)

    # Task Force
    p_vec = p_error.vector
    v_vec = v_error.vector

    for i in range(0, 3):  # for position only control
        p_vec[i + 3] = 0.0
        v_vec[i + 3] = 0.0

    f_star = Lam * (-Kp * p_vec - 2.0 * np.sqrt(Kp) * v_vec)

    # Torque from Joint error
    torque = J.transpose() * f_star + NLE
    torque[0] = 0.0

    return torque
예제 #13
0
 def update(self, q):
     se3.computeAllTerms(self.model, self.data, self.q, self.v)
     #se3.forwardKinematics(self.model, self.data, q, self.v, self.a)
     #- se3::forwardKinematics
     #- se3::crba
     #- se3::nonLinearEffects
     #- se3::computeJacobians
     #- se3::centerOfMass
     #- se3::jacobianCenterOfMass
     #- se3::kineticEnergy
     #- se3::potentialEnergy
     se3.framesKinematics(self.model, self.data, q)
     se3.computeJacobians(self.model, self.data, q)
     se3.rnea(self.model, self.data, q, self.v, self.a)
     self.biais(self.q, self.v)
     self.q = q.copy()
예제 #14
0
def compute_closed_loop_transition_matrix(gains_array, robot, q, v, update=True):
    gains = Gains(gains_array)
    
    ny=3
    H = matlib.zeros((3*nf+2*ny, 3*nf+2*ny))
    H[:ny, ny:2*ny] = matlib.eye(ny)
    
    H_f = matlib.zeros((3*nf,3*nf))
    H_f[  :nf,     nf:2*nf] = matlib.eye(nf)
    H_f[nf:2*nf, 2*nf:3*nf] = matlib.eye(nf)
    H_f[2*nf:,   2*nf:3*nf] = -gains.kd_bar*matlib.eye(nf)
    
    if(update):
        se3.computeAllTerms(robot.model, robot.data, q, v)
        se3.framesForwardKinematics(robot.model, robot.data, q)
        
    M = robot.data.M        #(7,7)
    Jl,Jr = robot.get_Jl_Jr_world(q, False)
    J = np.vstack([Jl[1:3],Jr[1:3]])    # (4, 7)    
    Mlf, Mrf = robot.get_Mlf_Mrf(q, False) 
    pyl, pzl = Mlf.translation[1:].A1
    pyr, pzr = Mrf.translation[1:].A1
    com, com_vel = robot.get_com_and_derivatives(q, v)
    cy, cz     = com.A1
    X_am  = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)])
    X_com = np.hstack([np.eye(2),np.eye(2)])
    X = np.vstack([X_com, X_am])
    X_pinv = np.linalg.pinv(X)
    
    Minv = np.linalg.inv(M)
    Upsilon = J*Minv*J.T
    S = matlib.zeros((na,nv))
    S[:,nv-na:] = matlib.eye(na)
    JSTpinv = np.linalg.pinv(J*S.T)        
    A = J*Minv*S.T*Mj_diag*JSTpinv
    
    # compute closed-loop transition matrix
    K1 = gains.kp_bar*K*A*gains.Kf
    K2 = K*Upsilon + gains.kp_bar*matlib.eye(nf)
    H_f[2*nf:,   1*nf:2*nf] = - K2
    H_f[2*nf:,   0*nf:1*nf] = - K1
    
    H[2*ny:,     2*ny:]        = H_f
    H[  ny:2*ny, 2*ny:2*ny+nf] = X
    H[ -nf:,         :ny]      = -K1*X_pinv*gains.Kp_com
    H[ -nf:,       ny:2*ny]    = -K1*X_pinv*gains.Kd_com
    return H
예제 #15
0
    def calc(self, data, x, u=None):
        '''
        M(vnext-v) - J^T f = 0
        J vnext = 0

        [MJ^T][vnext] = [Mv]
        [J   ][ -f   ]   [0 ]

        [vnext] = K^-1[Mv], with K = [MJ^T;J0]
        [ -f   ]       [0 ]
        '''
        nq, nv = self.nq, self.nv
        q = a2m(x[:nq])
        v = a2m(x[-nv:])

        pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
        pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)

        self.impulse.calc(data.impulse, x)

        data.K[:nv, :nv] = data.pinocchio.M
        if hasattr(self.pinocchio, 'armature'):
            data.K[range(nv), range(nv)] += self.pinocchio.armature.flat
        data.K[nv:, :nv] = data.impulse.J
        data.K.T[nv:, :nv] = data.impulse.J
        data.Kinv = inv(data.K)
        data.r[:nv] = (data.K[:nv, :nv] * v).flat
        data.r[nv:] = 0

        data.af[:] = np.dot(data.Kinv, data.r)
        data.f[:] *= -1.
        # Convert force array to vector of spatial forces.
        self.impulse.setForces(data.impulse, data.f)

        data.xnext[:nq] = q.flat
        data.xnext[nq:] = data.vnext

        if isinstance(self.costs, CostModelImpactBase):
            self.costs.setImpactData(data.costs, data.vnext)
        if isinstance(self.costs, CostModelSum):
            for cmodel, cdata in zip(self.costs.costs.values(),
                                     data.costs.costs.values()):
                if isinstance(cmodel.cost, CostModelImpactBase):
                    cmodel.cost.setImpactData(cdata, data.vnext)

        data.cost = self.costs.calc(data.costs, x, u=None)
        return data.xnext, data.cost
    def test_numdiff(self):
        rmodel,rdata = self.rmodel,self.rdata
        q,vq,aq,dq= self.q,self.vq,self.aq,self.dq

        #### Compute d/dq VCOM with the algo.
        pin.computeAllTerms(rmodel,rdata,q,vq)
        pin.computeForwardKinematicsDerivatives(rmodel,rdata,q,vq,aq)
        dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel,rdata)
        
        #### Approximate d/dq VCOM by finite diff.
        def calc_vc(q,vq):
            """ Compute COM velocity """
            pin.centerOfMass(rmodel,rdata,q,vq)
            return rdata.vcom[0]
        dvc_dqn = df_dq(rmodel,lambda _q: calc_vc(_q,vq),q)
        
        self.assertTrue(np.allclose(dvc_dq,dvc_dqn,atol=np.sqrt(self.precision)))
예제 #17
0
    def test_numdiff(self):
        rmodel, rdata = self.rmodel, self.rdata
        rdata_fd = self.rdata_fd
        q, vq = self.q, self.vq

        #### Compute d/dq VCOM with the algo.
        pin.computeAllTerms(rmodel, rdata, q, vq)
        dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel, rdata)

        #### Approximate d/dq VCOM by finite diff.
        def calc_vc(q, vq):
            """ Compute COM velocity """
            pin.centerOfMass(rmodel, rdata_fd, q, vq)
            return rdata_fd.vcom[0].copy()

        dvc_dqn = df_dq(rmodel, lambda _q: calc_vc(_q, vq), q)

        self.assertTrue(
            np.allclose(dvc_dq, dvc_dqn, atol=np.sqrt(self.precision)))
예제 #18
0
    def dynamics(self,x,u,display=False):
        '''
        Dynamic function: x,u -> xnext=f(x,y).
        Put the result in x (the initial value is destroyed). 
        Also compute the cost of making this step.
        Return x for convenience along with the cost.
        '''

        modulePi = (lambda th: (th+np.pi)%(2*np.pi)-np.pi) if self.modulo else (lambda th: th)
        sumsq    = lambda x : np.sum(np.square(x))

        cost = 0.0
        q = modulePi(x[:self.nq])
        v = x[self.nq:]
        u = np.clip(np.reshape(np.matrix(u),[self.nu,1]),-self.umax,self.umax)


        DT = self.DT/self.NDT
        for i in range(self.NDT):

            tau = u-self.Kf*v

            if self.armature is None:
                a = se3.aba(self.model,self.data,q,v,tau)
            else:
                se3.computeAllTerms(self.model,self.data,q,v)
                M   = self.data.M 
                #M.flat[::self.nv+1] += self.armature
                b   = self.data.nle
                a   = inv(M+self.armature*eye(self.nv))*(tau-b)

            v    += a*DT  # TODO
            q    += v*DT
            cost += (sumsq(q) + 1e-1*sumsq(v) + 1e-3*sumsq(u))*DT

            if display:
                self.display(q)
                time.sleep(self.DT/20)

        x[:self.nq] = modulePi(q)
        x[self.nq:] = np.clip(v,-self.vmax,self.vmax)

        return x,-cost
예제 #19
0
    def computeProblemData(self, time, q, v):
        se3.computeAllTerms(self.model, self.data, q, v)

        if self.freeflyer:
            M_a = self.data.M[self.u:, :]
            h_a = self.robot.nle(q, v)[self.u:]
            M_u = self.data.M[:self.u, :]
            h_u = self.robot.nle(q, v)[:self.u]

            # self.baseDynamics.setMatrix(np.hstack((M_u, -J_u)))
            # self.baseDynamics.setVector(-h_u)
        else:
            self.M_a = self.data.M
            self.h_a = self.robot.nle(q, v)
            for i in range(0, len(self.taskMotions)):
                c = self.taskMotions[i].task.compute(time, q, v)

        self.solutionDecoded = False
        return self.hqpData
예제 #20
0
    def step(self, q, v, tauq, dt=None):
        if dt is None: dt = self.dt
        self.tauq = tauq.copy()
        robot = self.robot
        NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK

        if self.first_iter:
            self.compute_f_df(q, v)
            self.lpf = FirstOrderLowPassFilter(
                dt, self.joint_torques_cut_frequency, tauq.A1)
            self.first_iter = False

        # filter desired joint torques
        if (self.joint_torques_cut_frequency > 0):
            self.tauq = np.matrix(self.lpf.filter_data(self.tauq.A1)).T

        # add Coulomb friction to joint torques
        for i in range(3, 7):
            if v[i] < 0:
                self.tauq[i] += self.tauc[i - 4]
            elif v[i] > 0:
                self.tauq[i] -= self.tauc[i - 4]

        #~ dv  = se3.aba(robot.model,robot.data,q,v,tauq,ForceDict(self.forces,NB))
        #~ #simulazione mano! (Forces are directly in the world frame, and aba wants them in the end effector frame)
        se3.forwardKinematics(robot.model, robot.data, q, v)
        se3.computeAllTerms(robot.model, robot.data, q, v)
        se3.updateFramePlacements(robot.model, robot.data)
        se3.computeJointJacobians(robot.model, robot.data, q)
        M = robot.data.M  #(7,7)
        h = robot.data.nle  #(7,1)
        Jl, Jr = robot.get_Jl_Jr_world(q)
        Jc = np.vstack([Jl[1:3], Jr[1:3]])  # (4, 7)
        dv = np.linalg.inv(M) * (self.tauq - h + Jc.T * self.f
                                 )  #use last forces
        v_mean = v + 0.5 * dt * dv
        v += dv * dt
        q = se3.integrate(robot.model, q, v_mean * dt)
        # WARNING: using the previous dv to compute df is an approximation!
        self.compute_f_df(q, v, dv, False)
        self.dv = dv
        self.t += dt
        return q, v
예제 #21
0
 def calc(self, data, x, u=None):
     if u is None:
         u = self.unone
     q, v = x[:self.state.nq], x[-self.state.nv:]
     # Computing the dynamics using ABA or manually for armature case
     if self.forceAba:
         data.xout = pinocchio.aba(self.state.pinocchio, data.pinocchio, q,
                                   v, u)
     else:
         pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q,
                                   v)
         data.M = data.pinocchio.M
         if self.armature.size == self.state.nv:
             data.M[range(self.state.nv),
                    range(self.state.nv)] += self.armature
         data.Minv = np.linalg.inv(data.M)
         data.xout = data.Minv * (u - data.pinocchio.nle)
     # Computing the cost value and residuals
     pinocchio.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)
     pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)
     self.costs.calc(data.costs, x, u)
     data.cost = data.costs.cost
예제 #22
0
    def test_forwardDynamics_default(self):
        data_no_q = self.model.createData()

        self.model.gravity = pin.Motion.Zero()
        ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
                                  self.tau0, self.J, self.gamma)
        self.assertLess(np.linalg.norm(ddq), self.tolerance)

        KKT_inverse = pin.getKKTContactDynamicMatrixInverse(
            self.model, self.data, self.J)
        M = pin.crba(self.model, self.model.createData(), self.q)

        self.assertApprox(
            M,
            np.linalg.inv(KKT_inverse)[:self.model.nv, :self.model.nv])

        pin.computeAllTerms(self.model, data_no_q, self.q, self.v0)
        ddq_no_q = pin.forwardDynamics(self.model, data_no_q, self.tau0,
                                       self.J, self.gamma)
        self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)

        self.assertApprox(ddq, ddq_no_q)
예제 #23
0
 def calc(self, data, x, u=None):
     if u is None:
         u = self.unone
     nq, nv = self.nq, self.nv
     q = a2m(x[:nq])
     v = a2m(x[-nv:])
     tauq = a2m(u)
     # --- Dynamics
     if self.forceAba:
         data.xout[:] = pinocchio.aba(self.pinocchio, data.pinocchio, q, v,
                                      tauq).flat
     else:
         pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
         data.M = data.pinocchio.M
         if hasattr(self.pinocchio, 'armature'):
             data.M[range(nv), range(nv)] += self.pinocchio.armature.flat
         data.Minv = np.linalg.inv(data.M)
         data.xout[:] = data.Minv * (tauq - data.pinocchio.nle).flat
     # --- Cost
     pinocchio.forwardKinematics(self.pinocchio, data.pinocchio, q, v)
     pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
     data.cost = self.costs.calc(data.costs, x, u)
     return data.xout, data.cost
예제 #24
0
    def dynamics(self,x,u,display=False):
        '''
        Dynamic function: x,u -> xnext=f(x,y).
        Put the result in x (the initial value is destroyed). 
        Also compute the cost of making this step.
        Return x for convenience along with the cost.
        '''

        modulePi = lambda th: (th+np.pi)%(2*np.pi)-np.pi
        sumsq    = lambda x : np.sum(np.square(x))

        cost = 0.0
        q = modulePi(x[:self.nq])
        v = x[self.nq:]
        u = np.clip(np.reshape(np.matrix(u),[self.nu,1]),-self.umax,self.umax)


        DT = self.DT/self.NDT
        for i in range(self.NDT):
            se3.computeAllTerms(self.model,self.data,q,v)
            M   = self.data.M
            b   = self.data.nle
            #tau = u-self.Kf*v
            a   = inv(M)*(u-self.Kf*v-b)

            v    += a*DT
            q    += v*DT
            cost += (sumsq(q) + 1e-1*sumsq(v) + 1e-3*sumsq(u))*DT

            if display:
                self.display(q)
                time.sleep(1e-4)

        x[:self.nq] = modulePi(q)
        x[self.nq:] = np.clip(v,-self.vmax,self.vmax)
        
        return x,-cost
예제 #25
0
    def dynamics(self, x, u, display=False):
        '''
        Dynamic function: x,u -> xnext=f(x,y).
        Put the result in x (the initial value is destroyed). 
        Also compute the cost of making this step.
        Return x for convenience along with the cost.
        '''

        modulePi = lambda th: (th + np.pi) % (2 * np.pi) - np.pi
        sumsq = lambda x: np.sum(np.square(x))

        cost = 0.0
        q = modulePi(x[:self.nq])
        v = x[self.nq:]
        u = np.clip(np.reshape(np.matrix(u), [self.nu, 1]), -self.umax,
                    self.umax)

        DT = self.DT / self.NDT
        for i in range(self.NDT):
            pin.computeAllTerms(self.model, self.data, q, v)
            M = self.data.M
            b = self.data.nle
            #tau = u-self.Kf*v
            a = inv(M) * (u - self.Kf * v - b)

            v += a * DT
            q += v * DT
            cost += (sumsq(q) + 1e-1 * sumsq(v) + 1e-3 * sumsq(u)) * DT

            if display:
                self.display(q)
                time.sleep(1e-4)

        x[:self.nq] = modulePi(q)
        x[self.nq:] = np.clip(v, -self.vmax, self.vmax)

        return x, -cost
예제 #26
0
 def test_forwardDynamics_no_q(self):
     data5 = self.data
     data6 = self.model.createData()
     data9_deprecated = self.model.createData()
     pin.computeAllTerms(self.model, data5, self.q, self.v0)
     pin.computeAllTerms(self.model, data6, self.q, self.v0)
     pin.computeAllTerms(self.model, data9_deprecated, self.q, self.v0)
     ddq5 = pin.forwardDynamics(self.model, data5, self.tau, self.J,
                                self.gamma)
     ddq6 = pin.forwardDynamics(self.model, data6, self.tau, self.J,
                                self.gamma, r_coeff)
     with warnings.catch_warnings(record=True) as warning_list:
         ddq9_deprecated = pin.forwardDynamics(self.model, data9_deprecated,
                                               self.q, self.v, self.tau,
                                               self.J, self.gamma, r_coeff,
                                               False)
         self.assertTrue(
             any(item.category == pin.DeprecatedWarning
                 for item in warning_list))
     self.assertTrue((ddq5 == ddq6).all())
     self.assertTrue((ddq5 == ddq9_deprecated).all())
예제 #27
0
def update_quantities(robot: jiminy.Model,
                      position: np.ndarray,
                      velocity: Optional[np.ndarray] = None,
                      acceleration: Optional[np.ndarray] = None,
                      update_physics: bool = True,
                      update_com: bool = True,
                      update_energy: bool = True,
                      update_jacobian: bool = False,
                      update_collisions: bool = True,
                      use_theoretical_model: bool = True) -> None:
    """Compute all quantities using position, velocity and acceleration
    configurations.

    Run multiple algorithms to compute all quantities which can be known with
    the model position, velocity and acceleration.

    This includes:
    - body spatial transforms,
    - body spatial velocities,
    - body spatial drifts,
    - body transform acceleration,
    - body transform jacobians,
    - center-of-mass position,
    - center-of-mass velocity,
    - center-of-mass drift,
    - center-of-mass acceleration,
    - center-of-mass jacobian,
    - articular inertia matrix,
    - non-linear effects (Coriolis + gravity)
    - collisions and distances

    .. note::
        Computation results are stored internally in the robot, and can
        be retrieved with associated getters.

    .. warning::
        This function modifies the internal robot data.

    .. warning::
        It does not called overloaded pinocchio methods provided by
        `jiminy_py.core` but the original pinocchio methods instead. As a
        result, it does not take into account the rotor inertias / armatures.
        One is responsible of calling the appropriate methods manually instead
        of this one if necessary. This behavior is expected to change in the
        future.

    :param robot: Jiminy robot.
    :param position: Robot position vector.
    :param velocity: Robot velocity vector.
    :param acceleration: Robot acceleration vector.
    :param update_physics: Whether or not to compute the non-linear effects and
                           internal/external forces.
                           Optional: True by default.
    :param update_com: Whether or not to compute the COM of the robot AND each
                       link individually. The global COM is the first index.
                       Optional: False by default.
    :param update_energy: Whether or not to compute the energy of the robot.
                          Optional: False by default
    :param update_jacobian: Whether or not to compute the jacobians.
                            Optional: False by default.
    :param use_theoretical_model: Whether the state corresponds to the
                                  theoretical model when updating and fetching
                                  the robot's state.
                                  Optional: True by default.
    """
    if use_theoretical_model:
        pnc_model = robot.pinocchio_model_th
        pnc_data = robot.pinocchio_data_th
    else:
        pnc_model = robot.pinocchio_model
        pnc_data = robot.pinocchio_data

    if (update_physics and update_com and
            update_energy and update_jacobian and
            velocity is not None and acceleration is None):
        pin.computeAllTerms(pnc_model, pnc_data, position, velocity)
    else:
        if velocity is None:
            pin.forwardKinematics(pnc_model, pnc_data, position)
        elif acceleration is None:
            pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
        else:
            pin.forwardKinematics(
                pnc_model, pnc_data, position, velocity, acceleration)

        if update_com:
            if velocity is None:
                pin.centerOfMass(pnc_model, pnc_data, position)
            elif acceleration is None:
                pin.centerOfMass(pnc_model, pnc_data, position, velocity)
            else:
                pin.centerOfMass(
                    pnc_model, pnc_data, position, velocity, acceleration)

        if update_jacobian:
            if update_com:
                pin.jacobianCenterOfMass(pnc_model, pnc_data)
            pin.computeJointJacobians(pnc_model, pnc_data)

        if update_physics:
            if velocity is not None:
                pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
            pin.crba(pnc_model, pnc_data, position)

        if update_energy:
            if velocity is not None:
                pin.computeKineticEnergy(pnc_model, pnc_data)
            pin.computePotentialEnergy(pnc_model, pnc_data)

    pin.updateFramePlacements(pnc_model, pnc_data)

    if update_collisions:
        pin.updateGeometryPlacements(
            pnc_model, pnc_data, robot.collision_model, robot.collision_data)
        pin.computeCollisions(
            robot.collision_model, robot.collision_data,
            stop_at_first_collision=False)
        pin.computeDistances(robot.collision_model, robot.collision_data)
        for dist_req in robot.collision_data.distanceResults:
            if np.linalg.norm(dist_req.normal) < 1e-6:
                pin.computeDistances(
                    robot.collision_model, robot.collision_data)
                break
예제 #28
0
파일: dcrba.py 프로젝트: nim65s/pinocchio
    
    # --- HESSIAN ---
    # --- HESSIAN ---
    # --- HESSIAN ---
    H = hessian(robot,q)

    # Compute the Hessian matrix H using RNEA, so that acor = v*H*v
    vq1 = vq*0
    Htrue = np.zeros([6,robot.model.nv,robot.model.nv])
    for joint_i in range(1,robot.model.njoints):
        for joint_j in ancestors(joint_i)[:-1]:
            for i in iv(joint_i):
                for j in iv(joint_j):
                    vq1 *= 0
                    vq1[i] = vq1[j] = 1.0
                    pin.computeAllTerms(robot.model,robot.data,q,vq1)
                    Htrue[:,i,j] = (robot.data.oMi[joint_i]*robot.data.a[joint_i]).vector.T
    
    print('Check hessian = \t\t', norm(H-Htrue))    

    # --- dCRBA ---
    # --- dCRBA ---
    # --- dCRBA ---

    dcrba = DCRBA(robot)
    dcrba.pre(q)
    Mp = dcrba()

    # --- Validate dM/dq by finite diff
    dM = np.zeros([robot.model.nv,]*3)
    eps = 1e-6
    def step(self, u, dt=None):
        if dt is None:
            dt = self.dt

        # compute all quantities needed for simulation
        se3.computeAllTerms(self.model, self.data, self.q, self.v)
        se3.updateFramePlacements(self.model, self.data)
        M = self.data.M  # mass matrix
        h = self.data.nle  # nonlinear effects (gravity, Coriolis, centrifugal)

        if (self.simulate_coulomb_friction
                and self.simulation_type == 'timestepping'):
            # minimize kinetic energy using time stepping
            from quadprog import solve_qp
            '''
            Solve a strictly convex quadratic program
            
            Minimize     1/2 x^T G x - a^T x
            Subject to   C.T x >= b
            
            Input Parameters:
            G : array, shape=(n, n)
            a : array, shape=(n,)
            C : array, shape=(n, m) matrix defining the constraints
            b : array, shape=(m), default=None, vector defining the constraints
            meq : int, default=0
                the first meq constraints are treated as equality constraints,
                all further as inequality constraints
            Output: a tuple, where the first element is the optimal x.
            '''
            # M (v' - v) = dt*S^T*(tau - tau_c) - dt*h + dt*J^T*f
            # M v' = M*v + dt*(S^T*tau - h + J^T*f) - dt*S^T*tau_c
            # M v' = b + B*tau_c
            # v' = Minv*(b + B*tau_c)
            b = M.dot(self.v) + dt * (self.S.T.dot(u) - h)
            B = -dt * self.S.T
            # Minimize kinetic energy:
            # min v'.T * M * v'
            # min  (b+B*tau_c​).T*Minv*(b+B*tau_c​)
            # min tau_c.T * B.T*Minv*B* tau_C + 2*b.T*Minv*B*tau_c
            Minv = np.linalg.inv(M)
            G = B.T.dot(Minv.dot(B))
            a = -b.T.dot(Minv.dot(B))
            C = np.vstack((np.eye(self.na), -np.eye(self.na)))
            c = np.concatenate((-self.tau_coulomb_max, -self.tau_coulomb_max))
            solution = solve_qp(G, a, C.T, c, 0)
            self.tau_c = solution[0]
            self.v = Minv.dot(b + B.dot(self.tau_c))
            self.q = se3.integrate(self.model, self.q, self.v * dt)

        elif (self.simulation_type == 'euler'
              or self.simulate_coulomb_friction == False):
            self.tau_c = self.tau_coulomb_max * np.sign(self.v[-self.na:])
            self.dv = np.linalg.solve(M, self.S.T.dot(u - self.tau_c) - h)
            v_mean = self.v + 0.5 * dt * self.dv
            self.v += self.dv * dt
            self.q = se3.integrate(self.model, self.q, v_mean * dt)
        else:
            print("[ERROR] Unknown simulation type:", self.simulation_type)

        self.t += dt
        return self.q, self.v
예제 #30
0
    # --- HESSIAN ---
    # --- HESSIAN ---
    # --- HESSIAN ---
    H = hessian(robot, q)

    # Compute the Hessian matrix H using RNEA, so that acor = v*H*v
    vq1 = vq * 0
    Htrue = np.zeros([6, robot.model.nv, robot.model.nv])
    for joint_i in range(1, robot.model.njoints):
        for joint_j in ancestors(joint_i)[:-1]:
            for i in iv(joint_i):
                for j in iv(joint_j):
                    vq1 *= 0
                    vq1[i] = vq1[j] = 1.0
                    se3.computeAllTerms(robot.model, robot.data, q, vq1)
                    Htrue[:, i, j] = (robot.data.oMi[joint_i] *
                                      robot.data.a[joint_i]).vector.T

    print('Check hessian = \t\t', norm(H - Htrue))

    # --- dCRBA ---
    # --- dCRBA ---
    # --- dCRBA ---

    dcrba = DCRBA(robot)
    dcrba.pre(q)
    Mp = dcrba()

    # --- Validate dM/dq by finite diff
    dM = np.zeros([
예제 #31
0
    u = pin.utils.zero(m.nv)

    # Random states and control
    q = pin.utils.rand(m.nq)
    if use_free_jnt:
        q[3:7] = q[3:7]/np.linalg.norm(q[3:7])  # normalize quaternion if free joint
    v = pin.utils.rand(m.nv)
    u = pin.utils.rand(m.nv)

    # Set the external forces
    fext = pin.StdVec_Force()
    for k in range(m.njoints):
        fext.append(pin.Force.Zero())

    # Evaluate all terms and forward dynamics
    pin.computeAllTerms(m, d, q, v)
    a = pin.aba(m, d, q, v, u, fext)

    # Print state and controls
    print("pos =", q)
    print("vel =", v)
    print("tau =", u)
    print("acc =", a)
    print("nle =", d.nle)

    # Evaluate the ABA derivatives
    pin.computeABADerivatives(m, d, q, v, u, fext)
    print("\nABA derivatives:")
    print("ddq_dq:\n", d.ddq_dq)
    print("ddq_dv:\n", d.ddq_dv)
    print("M:\n", d.M)
예제 #32
0
def c_control(dt, robot, sample, t_simu):
    eigenpy.switchToNumpyMatrix()

    qa = np.matrix(np.zeros((7, 1)))
    qa_dot = np.matrix(np.zeros((7, 1)))

    for i in range(7):
        qa[i, 0] = robot.q[i]
        qa_dot[i, 0] = robot.v[i]

    # Method : Joint Control A q_ddot = torque
    Kp = 400.  # Convergence gain
    ID_EE = robot.model.getFrameId(
        "LWrMot3")  # Control target (End effector) ID

    # Compute/update all joints and frames
    se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)

    # Get kinematics information
    oMi = robot.framePlacement(qa, ID_EE)  ## EE's placement
    v_frame = robot.frameVelocity(qa, qa_dot, ID_EE)  # EE's velocity
    J = robot.computeFrameJacobian(
        qa, ID_EE)[:3, :]  ## EE's Jacobian w.r.t Local Frame

    # Get dynamics information
    M = robot.mass(qa)  # Mass Matrix
    NLE = robot.nle(qa, qa_dot)  #gravity and Coriolis
    Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose())  # Lambda Matrix

    # Update Target oMi position
    wMl = copy.deepcopy(sample.pos)
    wMl.rotation = copy.deepcopy(oMi.rotation)
    v_frame_ref = se3.Motion()  # target velocity (v, w)
    v_frame_ref.setZero()

    # Get placement Error
    p_error = se3.log(sample.pos.inverse() * oMi)
    v_error = v_frame - wMl.actInv(v_frame_ref)

    # Task Force
    p_vec = p_error.linear
    v_vec = v_error.linear

    xddotstar = (-Kp * p_vec - 2 * np.sqrt(Kp) * v_vec)
    f_star = Lam * xddotstar

    # Torque from Joint error
    torque = J.transpose() * f_star + NLE
    torque[0] = 0.0

    # Selection Matrix
    Id7 = np.identity(7)
    fault_joint_num = 0
    S = np.delete(Id7, (fault_joint_num),
                  axis=0)  #delete fault joint corresponding row
    # Selection Matrix Inverse - dynamically consistent inverse
    Minv = np.linalg.inv(M)
    ST = S.transpose()
    SbarT = np.linalg.pinv(S * Minv * ST) * S * Minv
    # Jacobian Matrix Inverse - dynamically consistent inverse
    JbarT = Lam * J * np.linalg.inv(M)
    #Weighting matrix
    W = S * Minv * ST
    Winv = np.linalg.inv(W)
    #Weighted pseudo inverse of JbarT*ST
    JtildeT = Winv * (JbarT * ST).transpose() * np.linalg.pinv(
        JbarT * ST * Winv * (JbarT * ST).transpose())
    #Null-space projection matrix
    Id6 = np.identity(6)
    NtildeT = Id6 - JtildeT * JbarT * ST
    null_torque = 0.0 * qa_dot  # joint damping

    #Torque
    torque = ST * (JtildeT * f_star + NtildeT * SbarT * null_torque +
                   SbarT * NLE)

    return torque
예제 #33
0
def update_quantities(jiminy_model,
                      position,
                      velocity=None,
                      acceleration=None,
                      update_physics=True,
                      update_com=False,
                      update_energy=False,
                      update_jacobian=False,
                      use_theoretical_model=True):
    """
    @brief Compute all quantities using position, velocity and acceleration
           configurations.

    @details Run multiple algorithms to compute all quantities
             which can be known with the model position, velocity
             and acceleration configuration.

             This includes:
             - body spatial transforms,
             - body spatial velocities,
             - body spatial drifts,
             - body transform acceleration,
             - body transform jacobians,
             - center-of-mass position,
             - center-of-mass velocity,
             - center-of-mass drift,
             - center-of-mass acceleration,
             (- center-of-mass jacobian : No Python binding available so far),
             - articular inertia matrix,
             - non-linear effects (Coriolis + gravity)

             Computation results are stored in internal
             data and can be retrieved with associated getters.

    @note This function modifies internal data.

    @param jiminy_model    The Jiminy model
    @param position         Joint position vector
    @param velocity         Joint velocity vector
    @param acceleration     Joint acceleration vector

    @pre model_.nq == positionIn.size()
    @pre model_.nv == velocityIn.size()
    @pre model_.nv == accelerationIn.size()
    """
    if use_theoretical_model:
        pnc_model = jiminy_model.pinocchio_model_th
        pnc_data = jiminy_model.pinocchio_data_th
    else:
        pnc_model = jiminy_model.pinocchio_model
        pnc_data = jiminy_model.pinocchio_data

    if (update_physics and update_com and \
        update_energy and update_jacobian and \
        velocity is not None):
        pnc.computeAllTerms(pnc_model, pnc_data, position, velocity)
    else:
        if update_physics:
            if velocity is not None:
                pnc.nonLinearEffects(pnc_model, pnc_data, position, velocity)
            pnc.crba(pnc_model, pnc_data, position)

        if update_jacobian:
            # if update_com:
            #     pnc.getJacobianComFromCrba(pnc_model, pnc_data)
            pnc.computeJointJacobians(pnc_model, pnc_data)

        if update_com:
            if velocity is None:
                pnc.centerOfMass(pnc_model, pnc_data, position)
            elif acceleration is None:
                pnc.centerOfMass(pnc_model, pnc_data, position, velocity)
            else:
                pnc.centerOfMass(pnc_model, pnc_data, position, velocity,
                                 acceleration)
        else:
            if velocity is None:
                pnc.forwardKinematics(pnc_model, pnc_data, position)
            elif acceleration is None:
                pnc.forwardKinematics(pnc_model, pnc_data, position, velocity)
            else:
                pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
                                      acceleration)
            pnc.framesForwardKinematics(pnc_model, pnc_data, position)

        if update_energy:
            if velocity is not None:
                pnc.kineticEnergy(pnc_model, pnc_data, position, velocity,
                                  False)
            pnc.potentialEnergy(pnc_model, pnc_data, position, False)
    def solve(self, t, q, v, f_meas, df_meas=None):
        robot = self.robot
        NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK
        w_post = self.w_post
        Kp_post, Kd_post = self.Kp_post, self.Kd_post

        se3.computeAllTerms(robot.model, robot.data, q, v)
        se3.updateFramePlacements(robot.model, robot.data)
        se3.rnea(robot.model, robot.data, q, v, 0 * v)
        M = robot.data.M  #(7,7)
        h = robot.data.nle  #(7,1)
        Jl, Jr = robot.get_Jl_Jr_world(q, False)
        Mlf, Mrf = robot.get_Mlf_Mrf(q, False)

        #4th order CoM via feet acceleration task **********************
        '''com_s_des -> ddf_des -> feet_a_des'''

        # Estimate center of mass and derivatives
        if self.estimator is None:
            #take the measurement state as estimate, assume jerk is measured by df
            com_mes, com_v_mes, com_a_mes, com_j_mes = robot.get_com_and_derivatives(
                q, v, f_meas, df_meas)
            am_est = robot.get_angularMomentum(q, v)
            com_est, com_v_est, com_a_est, com_j_est, f_est, df_est = com_mes, com_v_mes, com_a_mes, com_j_mes, f_meas, df_meas
        else:
            com_mes, com_v_mes, com_a_mes = robot.get_com_and_derivatives(
                q, v, f_meas)
            am = robot.get_angularMomentum(q, v)
            p = np.hstack((Mlf.translation[1:].A1, Mrf.translation[1:].A1))
            self.estimator.predict_update(com_mes.A1, com_v_mes.A1,
                                          np.array([am]), f_meas.A1, p,
                                          self.data.ddf_des.A1)

            (com_est, com_v_est, am_est, f_est,
             df_est) = self.estimator.get_state(True)
            dummy1, dummy2, com_a_est, com_j_est = robot.get_com_and_derivatives(
                q, v, f_est, df_est)

            # DEBUG
#            com_mes, com_v_mes, com_a_mes, com_j_mes = robot.get_com_and_derivatives(q, v, f_meas, df_meas)
#            am_est = robot.get_angularMomentum(q, v)
#            com_est, com_v_est, com_a_est, com_j_est = com_mes, com_v_mes, com_a_mes, com_j_mes
#            f_est, df_est = f_meas, df_meas
#            f_est = f_meas

# Formulate dynamic constrains  (ne sert a rien...)************* todo remove and make the problem smaller
#      |M   -Jc.T  -S.T| * [dv,f,tau].T =  -h
        Jc = np.vstack([Jl[1:3], Jr[1:3]])  # (4, 7)
        driftLF, driftRF = robot.get_driftLF_driftRF_world(q, v, False)
        dJcdq = np.vstack([driftLF.vector[1:3], driftRF.vector[1:3]])

        S = np.hstack([matlib.zeros([4, 3]), matlib.eye(4)])  # (4,7)
        Aec = np.vstack([
            np.hstack([M, -Jc.T, -S.T]),
            np.hstack(
                [matlib.zeros([4, 7]),
                 matlib.eye(4),
                 matlib.zeros([4, 4])])
        ])
        #        bec = np.vstack([-h, f_est])
        #        bec = np.vstack([-h, f_meas])
        # use f+dt/2*df instead of f
        bec = np.vstack([-h, f_meas + 0.5 * self.dt * df_est])

        # CoM TASK COMPUTATIONS
        com_s_des, Xc, Nc = self._compute_com_task(t, com_est, com_v_est,
                                                   com_a_est, com_j_est)

        # ANGULAR MOMENTUM TASK COMPUTATIONS
        dddam_des, Xl, Nl = self._compute_ang_mom_task(q, v, am_est, com_est,
                                                       com_v_est, com_a_est,
                                                       f_est, df_est)

        u_des = np.vstack([com_s_des, dddam_des])  # CoM + Angular Momentum
        X = np.vstack([Xc, Xl])  # CoM + Angular Momentum
        N = np.vstack([Nc, Nl])  # CoM + Angular Momentum

        b_momentum = u_des - N - X * self.Kspring * dJcdq
        A_momentum = np.hstack([
            X * self.Kspring * Jc,
            matlib.zeros([X.shape[0], 4 + 4])
        ])  # zeros for columns corresponding to forces and torques

        #posture task  *************************************************
        post_p_err = matlib.zeros(7).T
        post_p_err[:2] = q[:2] - self.post_p_ref[:2]  # Y-Z error
        post_p_err[2] = np.arcsin(q[3]) - np.arcsin(
            self.post_p_ref[3])  # Angular error
        post_p_err[3:] = q[4:] - self.post_p_ref[4:]  # Actuation error
        post_v_err = v - self.post_v_ref
        post_a_des = self.post_a_ref - Kp_post * post_p_err - Kd_post * post_v_err
        b_post = w_post * post_a_des

        #stack all tasks
        A = np.vstack([A_momentum, self.A_post])
        b = np.vstack([b_momentum, b_post])

        # Friction cone inequality constraints Aic * x >= bic
        dt_f = 1.1 * self.dt

        #        Aic = -dt_f*B_f*self.Kspring*Jc
        #        viab_marg = -B_f*df_est + np.sqrt(-2*self.ddf_max*(B_f*f_est-b_f))
        #        if (viab_marg<=0.0).any():
        #            print "%.3f Warning: negative viability margins!"%(t), viab_marg.T
        #
        #        margin = b_f - B_f*(f_est+dt_f*df_est)
        #        if (margin<=0.0).any():
        #            margin[margin<=0.0] = 0.0
        #            print "%.3f Warning: negative predicted margins!"%(t), margin.T
        #        bic = -np.sqrt(2*self.ddf_max*margin) + B_f*df_est + dt_f*B_f*self.Kspring*dJcdq

        b1 = np.matlib.ones(self.b_f.shape[0]).T
        B_ddf_max = np.abs(self.B_f * np.matlib.ones((4, 1)) * self.ddf_max)
        pos = self.B_f * f_est
        vel = self.B_f * df_est
        pos_min = -1e100 * b1
        pos_max = self.b_f.copy()
        vel_max = 1e100 * b1

        #        viabViol = areStatesViable(pos, vel, pos_min, pos_max, vel_max, B_ddf_max);
        #        if(np.sum(viabViol)>0):
        #            print "Some constraints are not viable", np.where(viabViol)[0]

        ind_q = (pos >= pos_max).A1
        if (np.sum(ind_q) > 0):
            new_pos_max = compute_min_q_upper_bound(pos[ind_q], vel[ind_q],
                                                    B_ddf_max[ind_q])
            #            print "TSID-FLEX WARNING: some forces violate friction cones:", np.where(ind_q)[0], (pos[ind_q]-pos_max[ind_q]).T,
            #            print " old UB", pos_max[ind_q].T, "new UB", new_pos_max.T
            #            print "TSID-FLEX WARNING: update B_f_max", np.where(ind_q)[0], "from", pos_max[ind_q].T, "to", new_pos_max.T
            pos_max[ind_q] = new_pos_max + 1e-6
#            pos_max[ind_q] = pos[ind_q] + 0.1

        B_ddf_stop = compute_min_ddq_stop(pos, vel, pos_min, pos_max)
        ind = (B_ddf_stop > B_ddf_max).A1
        if (np.sum(ind) > 0):
            self.ind = ind
            #            print "TSID-FLEX WARNING: update B_ddf_max", np.where(ind)[0], "from", B_ddf_max[ind].T, "to", B_ddf_stop[ind].T
            B_ddf_max[ind] = B_ddf_stop[ind]

        (B_ddf_LB,
         B_ddf_UB) = computeAccLimits(pos,
                                      vel,
                                      pos_min,
                                      pos_max,
                                      vel_max,
                                      B_ddf_max,
                                      dt_f,
                                      verbose=True,
                                      IMPOSE_ACCELERATION_BOUNDS=False)

        # B * ddf <= B_ddf_max
        # B * K * ddp <= B_ddf_max
        # B * K * J * dv <= B_ddf_max - B * K * dJdq
        # -B * K * J * dv >= -B_ddf_max + B * K * dJdq
        Aic = -self.B_f * self.Kspring * Jc
        bic = -B_ddf_UB + self.B_f * self.Kspring * dJcdq

        Aic = np.hstack([Aic, matlib.zeros((Aic.shape[0], 8))])

        #stack equality and inequality constrains
        Ac = np.vstack([Aec, Aic])
        bc = np.vstack([bec, bic])
        #        Ac=Aec
        #        bc=bec
        #formulate the least squares as a quadratic problem *************
        H = (A.T * A).T + self.HESSIAN_REGULARIZATION * np.eye(A.shape[1])
        g = (A.T * b).T
        y_QP = solve_qp(H.A, g.A1, Ac.A.T, bc.A1, bec.shape[0])[0]

        # compute active inequalities
        margins = Aic * np.matrix(y_QP).T - bic
        if (margins <= 1e-5).any():
            #            print "%.4f Active inequalities:"%(t), np.where(margins<=1e-5)[0] #margins.T
            self.ind = (margins <= 1e-5).A1

        # Solve with inverse *******************************************
        if 0:
            Mu = robot.data.M[:3]
            JuT = Jc.T[:3]
            hu = h[:3]
            A_ = np.vstack([Mu, Jc, w_post * Jpost])
            b_ = np.vstack(
                [JuT * fc - hu, feet_a_des - dJcdq, w_post * post_a_des])
            #~ A_ = np.vstack([ Mu,Jc])
            #~ b_ = np.vstack([ JuT*fc - hu, feet_a_des - dJcdq ])
            dv_PINV = np.linalg.pinv(A_) * b_
            tau_PINV = S * (
                M * dv_PINV + h - Jc.T * fc
            )  #get tau_des from dv_des and measured contact forces:
            f_PINV = fc
            y_PINV = np.vstack([dv_PINV, f_PINV, tau_PINV]).A1
            assert isapprox(y_PINV, y_QP)

        dv = np.matrix(y_QP[:7]).T
        f = np.matrix(y_QP[7:7 + 4]).T
        tau = np.matrix(y_QP[7 + 4:]).T

        # compute ddf_des for next loop EKF computations
        feet_a_des = Jc * dv + dJcdq
        self.data.ddf_des = self.Kspring * feet_a_des

        # store data
        self.data.lf_a_des = feet_a_des[:2]
        self.data.rf_a_des = feet_a_des[2:]

        self.data.com_p_mes = com_mes.A1
        self.data.com_v_mes = com_v_mes.A1
        self.data.com_a_mes = com_a_mes.A1
        #~ self.data.com_j_mes  = com_j_mes.A1 # should not be able to measure jerk

        self.data.com_p_est = com_est.A1
        self.data.com_v_est = com_v_est.A1
        self.data.com_a_est = com_a_est.A1
        self.data.com_j_est = com_j_est.A1
        self.data.com_s_des = com_s_des.A1
        self.data.com_s_exp = (X * self.data.ddf_des).A1

        self.data.lkf = f[:2].A1
        self.data.rkf = f[2:].A1
        self.data.tau = tau
        self.data.dv = dv
        self.data.f = f

        self.data.B_ddf_max = -B_ddf_max
        self.data.B_ddf_UB = -B_ddf_UB
        self.data.B_ddf_des = -self.B_f * self.data.ddf_des
        self.data.B_df = -vel
        (B_df_min, B_df_max) = computeVelViabBounds(pos, pos_min, pos_max,
                                                    B_ddf_max)
        self.data.B_df_max = -B_df_max
        self.data.B_f = pos_max - pos

        DEBUG_VIAB = 0
        if DEBUG_VIAB:
            dt = self.dt
            pos_next = pos + dt * vel + 0.5 * dt * dt * self.data.B_ddf_des
            vel_next = vel + dt * self.data.B_ddf_des
            max_pos_next = pos + dt * vel + 0.5 * dt * dt * B_ddf_UB
            max_vel_next = vel + dt * B_ddf_UB
            viabViol = areStatesViable(max_pos_next, max_vel_next, pos_min,
                                       pos_max, vel_max, B_ddf_max)
            if (np.sum(viabViol) > 0):
                print "%.4f ERROR Some constraints will not be viable at next time step" % (
                    t), np.where(viabViol)[0]
#            if(t>0.16):
#                print "    %.4f Current    value of B_f "%(t), pos.T
#                print "    %.4f Future     value of B_f "%(t), pos_next.T
#                print "    %.4f Max future value of B_f "%(t), max_pos_next.T
#                print "    %.4f Current    value of B_df"%(t), vel.T
#                print "    %.4f Future     value of B_df"%(t), vel_next.T
#                print "    %.4f Max future value of B_df"%(t), max_vel_next.T

        if (np.sum(ind) > 0):
            B_ddf_max = np.abs(self.B_f * np.matlib.ones(
                (4, 1)) * self.ddf_max)


#            print "%.4f WARNING: increase max constr acc"%(t), np.where(ind)[0], 'from', B_ddf_max[ind].T, 'to', B_ddf_stop[ind].T, \
#                    'B_f', (pos_max[ind]-pos[ind]).T
#            print "%.4f WARNING: increase max constr acc"%(t), np.where(ind)[0], 'from', B_ddf_max[ind].T, 'to', B_ddf_stop[ind].T, \
#                    'B_ddf_UB', B_ddf_UB[ind].T, 'B_ddf_des', self.data.B_ddf_des[ind].T, 'B_df', vel[ind].T, 'B_f', (pos_max[ind]-pos[ind]).T
#        elif(np.sum(self.ind)>0):
#            ind = self.ind
#            print "%.4f DEBUG:              "%(t), np.where(ind)[0], 'B_ddf_max', B_ddf_max[ind].T, 'B_ddf_stop', B_ddf_stop[ind].T, \
#                    'B_ddf_UB', B_ddf_UB[ind].T, 'B_ddf_des', self.data.B_ddf_des[ind].T, 'B_df', vel[ind].T, 'B_f', (pos_max[ind]-pos[ind]).T

        if w_post == 0 and 0:
            #Test that feet acceleration are satisfied
            try:
                assert (isapprox(feet_a_des, Jc * dv + dJcdq))
            except AssertionError:
                print(
                    "Solution violate the desired feet acceleration and so the CoM snap {}"
                ).format(np.linalg.norm(feet_a_des - (Jc * dv + dJcdq)))
            #external forces should be zero
            assert (isapprox((M * dv + h - Jc.T * fc)[:3], np.zeros([3, 1])))
        return np.vstack([zero(3), tau])