Exemplo n.º 1
0
    def cost(self, x):
        q = a2m(x)
        pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
        pinocchio.updateFramePlacements(self.rmodel, self.rdata)

        refMl = self.refL.inverse() * self.rdata.oMf[self.idL]
        residualL = m2a(pinocchio.log(refMl).vector)
        refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
        residualR = m2a(pinocchio.log(refMr).vector)

        self.residuals = np.concatenate([residualL, residualR])
        return sum(self.residuals**2)
Exemplo n.º 2
0
Arquivo: ik.py Projeto: thomasfla/labs
def ik(robot,oMdes, JOINT_ID,eps = 1e-4):
    q = robot.q0.copy()
    IT_MAX = 500
    DT     = 1e-1
    damp   = 1e-12
    i=0
    success = False
    while True:
        pinocchio.forwardKinematics(robot.model,robot.data,q)
        dMi = oMdes.actInv(robot.data.oMi[JOINT_ID])
        err = pinocchio.log(dMi).vector
        if norm(err) < eps:
            success = True
            break
        if i >= IT_MAX:
            success = False
            break
        J = pinocchio.computeJointJacobian(robot.model,robot.data,q,JOINT_ID)
        v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
        q = pinocchio.integrate(robot.model,q,v*DT)
        i += 1
    if not success:
        #raise( Exception('ik did not converged') )
        q[:] = np.nan
    if ((q < robot.model.lowerPositionLimit).any() or
    (q > robot.model.upperPositionLimit).any()):
        #Violate joint limits
        q[:] = np.nan
    return (q)
 def errorInSE3( M,Mdes):
     '''
     Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference
     between M and Mdes, both element of SE3.
     '''
     error = se3.log(Mdes.inverse()*M)
     return error.vector()
Exemplo n.º 4
0
 def cost(self, x):
     q = a2m(x)
     pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
     pinocchio.updateFramePlacements(self.rmodel, self.rdata)
     M = self.rdata.oMf[self.idEff]
     self.residuals = m2a(pinocchio.log(M.inverse() * self.ref).vector)
     return sum(self.residuals**2)
Exemplo n.º 5
0
    def compute(self, x_des, M_des, q0):

        self.frame_id = self.model.getFrameId('tool0')
        q_i = q0

        H_des = pin.SE3(M_des, x_des)

        for i in range(self.n):

            self.robot.computeAllTerms(q_i, np.zeros(6))
            H = self.robot.framePlacement(q_i, self.frame_id, False)

            dH = H_des.actInv(H)
            error = pin.log(dH).vector

            if (norm(error) < self.eps):
                self.success = True
                break

            # J = self.robot.frameJacobian(q_i, self.frame_id, False)
            J = pin.computeJointJacobian(self.model, self.data, q_i, 6)
            v = -J.T.dot(solve(J.dot(J.T) + self.damp * np.eye(6), error))
            q_i = pin.integrate(self.model, q_i, v * self.DT)

        return q_i
Exemplo n.º 6
0
def errorInSE3(M, Mdes):        
    '''                                                                                                     
    Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference                          
    between M and Mdes, both element of SE3.    
    '''
    error = se3.log(Mdes.inverse()*M)
    return error
 def constraint(self, x):
     q = a2m(x)
     pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
     pinocchio.updateFramePlacements(self.rmodel, self.rdata)
     refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
     self.eq = m2a(pinocchio.log(refMeff).vector)
     return self.eq.tolist()
Exemplo n.º 8
0
 def constraint_leftfoot(self, x, nc=0):
     q, tauq, fr, fl = self.x2qf(x)
     pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
     pinocchio.updateFramePlacements(self.rmodel, self.rdata)
     refMl = self.refL.inverse() * self.rdata.oMf[self.idL]
     self.eq[nc:nc + 6] = m2a(pinocchio.log(refMl).vector)
     return self.eq[nc:nc + 6].tolist()
Exemplo n.º 9
0
 def constraint_rightfoot(self, x, nc=0):
     q = self.vq2q(a2m(x))
     pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
     pinocchio.updateFramePlacements(self.rmodel, self.rdata)
     refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
     self.eq[nc:nc + 6] = m2a(pinocchio.log(refMr).vector)
     return self.eq[nc:nc + 6].tolist()
Exemplo n.º 10
0
    def solve(self, M_des, robot, joint_id, q=None):
        """ Inverse kinematics for specified joint (joint_id) and desired pose M_des (array 4x4)
        NOTE The code below is adopted from here (01.03.2020): 
        https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html
        """

        oMdes = pinocchio.SE3(M_des[:3, :3], M_des[:3, 3])

        if np.all(q == None): q = pinocchio.neutral(robot.model)

        i = 0
        iter_resample = 0
        while True:
            # forward
            pinocchio.forwardKinematics(robot.model, robot.data, q)

            # error between desired pose and the current one
            dMi = oMdes.actInv(robot.data.oMi[joint_id])
            err = pinocchio.log(dMi).vector

            # Termination criteria
            if norm(err) < self.inv_kin_sol_params.eps:
                success = True
                break
            if i >= self.inv_kin_sol_params.max_iter:
                if iter_resample <= self.inv_kin_sol_params.max_resample:
                    q = pinocchio.randomConfiguration(robot.model)
                    iter_resample += 1
                    continue
                else:
                    success = False
                    break

            # Jacobian
            J = pinocchio.computeJointJacobian(robot.model, robot.data, q,
                                               joint_id)

            # P controller (?)
            # v* =~ A * e
            v = -J.T.dot(
                solve(
                    J.dot(J.T) + self.inv_kin_sol_params.damp * np.eye(6),
                    err))

            # integrate (in this case only sum)
            q = pinocchio.integrate(robot.model, q,
                                    v * self.inv_kin_sol_params.dt)

            i += 1

        if not success: q = pinocchio.neutral(robot.model)
        q_arr = np.squeeze(np.array(q))
        q_arr = np.mod(np.abs(q_arr), 2 * np.pi) * np.sign(q_arr)
        mask = np.abs(q_arr) > np.pi
        # If angle abs(q) is larger than pi, represent angle as the shorter angle inv_sign(q) * (2*pi - abs(q))
        # This is to avoid conflicting with angle limits.
        q_arr[mask] = (-1) * np.sign(
            q_arr[mask]) * (2 * np.pi - np.abs(q_arr[mask]))
        return success, q_arr
Exemplo n.º 11
0
 def eval_vel(delta_t):
     if dofs == "TRANSLATION":
         #print("delta_t:" , delta_t)
         return (goal - transformation_func()) / delta_t
     elif dofs is None:
         return pinocchio.log(
             transformation_func().inverse() * goal).vector / delta_t
     else:
         raise ValueError("Implementation for %s not available" % dofs)
Exemplo n.º 12
0
 def cost(self, x):
     """
     x --> initial configuration of the robot object
     """
     q = a2m(x)
     pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
     M = self.rdata.oMi[self.idEFF]
     # This is really the only difference between fKinematics_arm and this script.
     self.residuals = m2a(pinocchio.log(M.inverse() * self.ref).vector)
     return sum(self.residuals**2)
Exemplo n.º 13
0
    def inverse_kinematics(self, x, y, z):
        """Calculate the joint values for a desired cartesian position"""
        model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
            "/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf"
        )
        data, collision_data, visual_data = pinocchio.createDatas(
            model, collision_model, visual_model)

        JOINT_ID = 5
        oMdes = pinocchio.SE3(np.eye(3), np.array([x, y, x]))

        q = np.array(abel.sort_joints()[5:])
        eps = 1e-1
        IT_MAX = 1000
        DT = 1e-4
        damp = 1e-4

        i = 0
        while True:
            pinocchio.forwardKinematics(model, data, q)
            dMi = oMdes.actInv(data.oMi[JOINT_ID])
            err = pinocchio.log(dMi).vector
            if norm(err) < eps:
                success = True
                break
            if i >= IT_MAX:
                success = False
                break
            J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
            v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
            q = pinocchio.integrate(model, q, v * DT)
            if not i % 10:
                print('%d: error = %s' % (i, err.T))
            i += 1

        if success:
            print("Convergence achieved!")
        else:
            print(
                "\nWarning: the iterative algorithm has not reached convergence to the desired precision"
            )

        print('\nresult: %s' % q.flatten().tolist())
        print('\nfinal error: %s' % err.T)

        point = JointTrajectoryPoint()
        point.positions = [
            0, 0, 0, 0, 0, q[0], q[1], q[2], q[3], q[4], q[5], q[6], q[7],
            q[8], q[9]
        ]

        abel.move_all_joints(point)

        self.direct_kinematics()
Exemplo n.º 14
0
    def optimize_initial_position(self, init_state):
        # Optimize the initial configuration
        q = se3.neutral(self.robot.model)

        plan_joint_init_pos = self.planner_setting.get(
            PlannerVectorParam_KinematicDefaultJointPositions)
        if len(plan_joint_init_pos) != self.robot.num_ctrl_joints:
            raise ValueError(
                'Number of joints in config file not same as required for robot\n'
                + 'Got %d joints but robot expects %d joints.' %
                (len(plan_joint_init_pos), self.robot.num_ctrl_joints))

        q[7:] = np.matrix(plan_joint_init_pos).T
        q[2] = self.robot.floor_height + 0.32  #was 0.32
        #print q[2]
        dq = np.matrix(np.zeros(self.robot.robot.nv)).T

        com_ref = init_state.com
        lmom_ref = np.zeros(3)
        amom_ref = np.zeros(3)
        endeff_pos_ref = np.array(
            [init_state.effPosition(i) for i in range(init_state.effNum())])
        endeff_vel_ref = np.matrix(np.zeros((init_state.effNum(), 3)))
        endeff_contact = np.ones(init_state.effNum())
        quad_goal = se3.Quaternion(
            se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
        q[3:7] = quad_goal.coeffs()

        for iters in range(self.max_iterations):
            # Adding small P controller for the base orientation to always start with flat
            # oriented base.
            quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
                                    float(q[5]))
            amom_ref = 1e-1 * se3.log((quad_goal * quad_q.inverse()).matrix())

            res = self.inv_kin.compute(q, dq, com_ref, lmom_ref, amom_ref,
                                       endeff_pos_ref, endeff_vel_ref,
                                       endeff_contact, None)
            q = se3.integrate(self.robot.model, q, res)

            if np.linalg.norm(res) < 1e-3:
                print('Found initial configuration after {} iterations'.format(
                    iters + 1))
                break

        if iters == self.max_iterations - 1:
            print('Failed to converge for initial setup.')

        print("initial configuration: \n", q)
        q[2] = 0.20

        self.q_init = q.copy()
        self.dq_init = dq.copy()
Exemplo n.º 15
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
Exemplo n.º 16
0
    def compute(self, time, q, v):
        oMi = self.robot.framePlacement(q, self.id)
        v_frame = self.robot.frameVelocity(q, v, self.id)
        self.drift = self.robot.frameClassicAcceleration(self.id)

        self.J = self.robot.getFrameJacobian(self.id)
        self.p_error = se3.log(self.M_ref.inverse() * oMi)
        self.p_error_vec = self.p_error.vector
        self.p_ref = np.hstack(
            (self.M_ref.translation, self.M_ref.rotation.flatten()))
        self.p = np.hstack((oMi.translation, oMi.rotation.flatten()))

        self.wMl.setIdentity()
        self.wMl.rotation = oMi.rotation

        if self.local:
            self.v_error = v_frame - self.wMl.actInv(self.v_ref)
            self.a_des = -self.Kp * self.p_error_vec - self.Kd * self.v_error.vector + self.wMl.actInv(
                self.a_ref).vector
        else:
            self.p_error_vec = np.dot(self.wMl.toActionMatrix(),
                                      self.p_error.vector)
            self.v_error = self.wMl.act(v_frame) - self.v_ref
            self.drift = self.wMl.act(self.drift)
            self.a_des = -self.Kp * self.p_error_vec - self.Kd * self.v_error.vector + self.a_ref.vector
            self.J_rotated = np.dot(self.wMl.toActionMatrix(), self.J)
            self.J = self.J_rotated

        self.v_error_vec = self.v_error.vector
        self.v_ref_vec = self.v_ref.vector
        self.v = v_frame.vector

        idx = 0
        for i in range(6):
            if np.abs(self.mask[i] - 1.0) < 1e-5:
                self.constraint.getMatrix()[idx, :] = self.J[i, :]
                self.constraint.getVector()[idx] = (self.a_des -
                                                    self.drift.vector)[i]
                self.a_des_masked[i] = self.a_des[i]
                self.drift_masked[i] = self.drift.vector[i]
                self.p_error_masked_vec[i] = self.p_error_vec[i]
                self.v_error_masked_vec[i] = self.v_error_vec[i]

                idx += 1

        return self.constraint
Exemplo n.º 17
0
 def calc(self, data, x):
     # We suppose forwardKinematics(q,v,a), computeJointJacobian and updateFramePlacement already
     # computed.
     assert (self.ref is not None or self.gains[0] == 0.)
     data.J[:, :] = pinocchio.getFrameJacobian(
         self.pinocchio, data.pinocchio, self.frame,
         pinocchio.ReferenceFrame.LOCAL)
     data.a0[:] = pinocchio.getFrameAcceleration(self.pinocchio,
                                                 data.pinocchio,
                                                 self.frame).vector.flat
     if self.gains[0] != 0.:
         data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame]
         data.a0[:] += self.gains[0] * m2a(pinocchio.log(data.rMf).vector)
     if self.gains[1] != 0.:
         data.a0[:] += self.gains[1] * m2a(
             pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
                                        self.frame).vector)
Exemplo n.º 18
0
 def logarithmicMap(self, quat_wxyz):
     w, x, y, z = quat_wxyz
     return pin.log(pin.Quaternion(w, x, y, z).matrix())
for i in range(N):
    M = se3.exp(nu)*M
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)

# Integrate a velocity of the body that is constant in the world frame.
for i in range(N):
    Mc = se3.SE3(M.rotation,zero(3))
    M  = M*se3.exp(Mc.actInv(nu))
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)

# Integrate a constant "log" velocity in body frame.
ME = se3.SE3( se3.Quaternion(0.7, -0.6,  0.1,  0.4).normalized().matrix(), 
              np.matrix([1,-1,2],np.double).T )
nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
for i in range(N):
    M = M*se3.exp(nu)
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
time.sleep(1)

# Integrate a constant "log" velocity in reference frame.
ME = se3.SE3( se3.Quaternion(0.3, -0.2,  0.6,  0.5).normalized().matrix(), 
              np.matrix([-1,1,0.6],np.double).T )
nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
for i in range(N):
    M = M*se3.exp(nu)
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
time.sleep(1)
Exemplo n.º 20
0
    model, collision_model, visual_model)

JOINT_ID = 5
oMdes = pinocchio.SE3(np.eye(3), np.array([-0.19, 0.50, -0.38]))

q = pinocchio.neutral(model)
eps = 1e-2
IT_MAX = 1000
DT = 1e-4
damp = 1e-12

i = 0
while True:
    pinocchio.forwardKinematics(model, data, q)
    dMi = oMdes.actInv(data.oMi[JOINT_ID])
    err = pinocchio.log(dMi).vector
    if norm(err) < eps:
        success = True
        break
    if i >= IT_MAX:
        success = False
        break
    J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
    v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
    q = pinocchio.integrate(model, q, v * DT)
    if not i % 10:
        print('%d: error = %s' % (i, err.T))
    i += 1

if success:
    print("Convergence achieved!")
Exemplo n.º 21
0
def cost(q):
    '''Compute score from a configuration'''
    q = np.matrix(q).T
    M = robot.placement(q, 6)
    return np.linalg.norm(pio.log(M.inverse() * Mtarget).vector)
Exemplo n.º 22
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
Exemplo n.º 23
0
 def residual(self, q):
     '''Compute score from a configuration'''
     pin.forwardKinematics(self.rmodel, self.rdata, q)
     M = pin.updateFramePlacement(self.rmodel, self.rdata, self.frameIndex)
     self.deltaM = self.Mtarget.inverse() * M
     return pin.log(self.deltaM).vector
Exemplo n.º 24
0
    M = se3.exp(nu) * M
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)

# Integrate a velocity of the body that is constant in the world frame.
for i in range(N):
    Mc = se3.SE3(M.rotation, zero(3))
    M = M * se3.exp(Mc.actInv(nu))
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)

# Integrate a constant "log" velocity in body frame.
ME = se3.SE3(
    se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(),
    np.matrix([1, -1, 2], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
    M = M * se3.exp(nu)
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
time.sleep(1)

# Integrate a constant "log" velocity in reference frame.
ME = se3.SE3(
    se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(),
    np.matrix([-1, 1, 0.6], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
    M = M * se3.exp(nu)
    viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
Exemplo n.º 25
0
    inv = np.linalg.inv

    #WBIC from https://arxiv.org/pdf/1909.06586.pdf
    M = robot.mass(q)

    #base orientation (3d)
    Kp1 = Kp_base_orientation
    Kd1 = Kd_base_orientation
    J1 = J_base_orientation
    M1 = robot.framePlacement(q, BASE_ID).rotation

    dx1 = robot.frameVelocity(q, dq, BASE_ID).angular
    M1_des = base_orientation_ref
    dx1_des = base_angularvelocity_ref
    ddx1_des = base_angularacceleration_ref
    e1 = pin.log(M1_des.T * M1)
    ddx1_cmd = ddx1_des + Kp1 * e1 + Kd1 * (dx1_des - dx1)
    dJ1dq = 0  #robot.frameAcceleration(q,dq,0*dq,BASE_ID).angular # TODO check this !

    #base position (3d)
    Kp2 = Kp_base_position
    Kd2 = Kd_base_position
    J2 = J_base_position
    x2 = robot.framePlacement(q, BASE_ID).translation
    dx2 = robot.frameVelocity(q, dq, BASE_ID).linear
    x2_des = base_position_ref
    dx2_des = base_linearvelocity_ref
    ddx2_des = base_linearacceleration_ref
    e2 = x2_des - x2
    ddx2_cmd = ddx2_des + Kp2 * e2 + Kd2 * (dx2_des - dx2)
    dJ2dq = 0  #robot.frameAcceleration(q,dq,0*dq,BASE_ID).linear # TODO check this !
Exemplo n.º 26
0
def errorInSE3(M, Mdes):
    error = se3.log(Mdes.inverse() * M)
    return error
Exemplo n.º 27
0
    def optimize(self,
                 init_state,
                 contact_sequence,
                 dynamic_sequence,
                 plotting=False):
        self.init_state = init_state
        self.contact_sequence = contact_sequence
        self.dynamic_sequence = dynamic_sequence
        self.q_via = None

        # Create array with centroidal and endeffector informations.
        self.fill_data_from_dynamics()
        self.fill_endeffector_trajectory()

        # Run the optimization for the initial configuration only once.
        if self.q_init is None:
            self.optimize_initial_position(init_state)

        # Get the desired joint trajectory
        # print "num_joint_via:",self.planner_setting.get(PlannerIntParam_NumJointViapoints)
        # print "joint_via:",self.planner_setting.get(PlannerCVectorParam_JointViapoints)

        # TODO: this is for jump, should go to config file
        # q_jump = [1., 0.1, -0.2 ,0.1, -0.2 ,-0.1, 0.2 ,-0.1, 0.2]
        # q_via = np.matrix([.75, np.pi/2, -np.pi, np.pi/2, -np.pi, -np.pi/2, np.pi, -np.pi/2, np.pi]).T
        # q_max = np.matrix([1.35, .7*np.pi/2, -.7*np.pi, .7*np.pi/2, -.7*np.pi, -.7*np.pi/2, .7*np.pi, -.7*np.pi/2, .7*np.pi]).T
        # q_via0 = np.vstack((q_via.T, q_jump))
        # self.q_via = np.vstack((q_via0, q_max.T))
        joint_traj_gen = JointTrajectoryGenerator()
        joint_traj_gen.num_time_steps = self.num_time_steps
        joint_traj_gen.q_init = self.q_init[7:]
        self.joint_des = np.zeros((len(self.q_init[7:]), self.num_time_steps),
                                  float)
        if self.q_via is None:
            for i in range(self.num_time_steps):
                self.joint_des[:, i] = self.q_init[7:].T
        else:
            joint_traj_gen.joint_traj(self.q_via)
            for it in range(self.num_time_steps):
                self.joint_des[:, it] = joint_traj_gen.eval_traj(it)

        # Compute inverse kinematics over the full trajectory.
        self.inv_kin.is_init_time = 0
        q, dq = self.q_init.copy(), self.dq_init.copy()
        for it in range(self.num_time_steps):
            quad_goal = se3.Quaternion(
                se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
            quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
                                    float(q[5]))
            amom_ref = (self.reg_orientation * se3.log(
                (quad_goal * quad_q.inverse()).matrix()).T +
                        self.amom_dyn[it]).reshape(-1)

            joint_regularization_ref = self.reg_joint_position * (
                np.matrix(self.joint_des[:, it]).T - q[7:])
            # joint_regularization_ref = self.reg_joint_position * (self.q_init[7 : ] - q[7 : ])

            # Fill the kinematics results for it.
            self.inv_kin.forward_robot(q, dq)
            self.fill_kinematic_result(it, q, dq)

            dq = self.inv_kin.compute(q, dq, self.com_dyn[it],
                                      self.lmom_dyn[it], amom_ref,
                                      self.endeff_pos_ref[it],
                                      self.endeff_vel_ref[it],
                                      self.endeff_contact[it],
                                      joint_regularization_ref)

            # Integrate to the next state.
            q = se3.integrate(self.robot.model, q, dq * self.dt)
Exemplo n.º 28
0
 def calc(self, data, x, u):
     data.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[
         self.Mref.frame]
     data.r = pinocchio.log(data.rMf).vector
     self.activation.calc(data.activation, data.r)
     data.cost = data.activation.a
Exemplo n.º 29
0
        robot.model, robot.data, q, IDX_TOOL)  # 6D jacobian in local frame
    o_Jtool3 = oRtool * tool_Jtool[:3, :]  # 3D jacobian in world frame
    o_TG = oMtool.translation - oMgoal.translation  # vector from tool to goal, in world frame

    vq = -pinv(o_Jtool3) * o_TG

    q = pio.integrate(robot.model, q, vq * DT)
    robot.display(q)
    time.sleep(1e-3)

    herr.append(o_TG)

q = q0.copy()
herr = []
for i in range(1000):  # Integrate over 2 second of robot life
    pio.framesForwardKinematics(robot.model, robot.data,
                                q)  # Compute frame placements
    oMtool = robot.data.oMf[
        IDX_TOOL]  # Placement from world frame o to frame f oMtool
    tool_nu = pio.log(oMtool.inverse() *
                      oMgoal).vector  # 6D error between the two frame
    tool_Jtool = pio.computeFrameJacobian(
        robot.model, robot.data, q, IDX_TOOL)  # Get corresponding jacobian
    vq = pinv(tool_Jtool) * tool_nu

    q = pio.integrate(robot.model, q, vq * DT)
    robot.display(q)
    time.sleep(1e-3)

    herr.append(tool_nu)
Exemplo n.º 30
0
def residualrMi(q):
    pinocchio.forwardKinematics(rmodel, rdata, q)
    return pinocchio.log(Mref.inverse() * rdata.oMi[jid]).vector
Exemplo n.º 31
0
    def optimize(self,
                 init_state,
                 contact_sequence,
                 dynamic_sequence,
                 plotting=False):
        self.init_state = init_state
        self.contact_sequence = contact_sequence
        self.dynamic_sequence = dynamic_sequence

        # Create array with centroidal and endeffector informations.
        self.fill_data_from_dynamics()
        self.fill_endeffector_trajectory()

        # Run the optimization for the initial configuration only once.
        if self.q_init is None:
            self.optimize_initial_position(init_state)

        # Get the desired joint trajectory
        n_via_joint = self.planner_setting.get(
            PlannerIntParam_NumJointViapoints)
        via_joint = self.planner_setting.get(
            PlannerCVectorParam_JointViapoints)

        self.joint_des = np.zeros((len(self.q_init[7:]), self.num_time_steps),
                                  float)
        if n_via_joint == 0:
            for i in range(self.num_time_steps):
                self.joint_des[:, i] = self.q_init[7:].T
        else:
            joint_traj_gen = TrajectoryInterpolator()
            joint_traj_gen.num_time_steps = self.num_time_steps
            joint_traj_gen.init = self.q_init[7:]
            joint_traj_gen.end = self.q_init[7:]
            joint_traj_gen.generate_trajectory(n_via_joint, via_joint, self.dt)
            for it in range(self.num_time_steps):
                self.joint_des[:, it] = joint_traj_gen.evaluate_trajecory(it)

        # Get the desired base viapoints
        n_via_base = self.planner_setting.get(PlannerIntParam_NumBaseViapoints)
        via_base = self.planner_setting.get(PlannerCVectorParam_BaseViapoints)

        # Generate smooth base trajectory for regularization
        self.base_des = np.zeros((3, self.num_time_steps), float)
        if n_via_base == 0:
            for it in range(self.num_time_steps):
                self.base_des[:, it] = np.array([0., 0., 0.]).reshape(-1)
        else:
            base_traj_gen = TrajectoryInterpolator()
            base_traj_gen.num_time_steps = self.num_time_steps
            base_traj_gen.init = np.array([0.0, 0.0, 0.0])
            base_traj_gen.end = np.array([0.0, 0.0, 0.0])
            base_traj_gen.generate_trajectory(n_via_base, via_base, self.dt)
            for it in range(self.num_time_steps):
                self.base_des[:, it] = base_traj_gen.evaluate_trajecory(it)

        # Compute inverse kinematics over the full trajectory.
        self.inv_kin.is_init_time = 0
        q, dq = self.q_init.copy(), self.dq_init.copy()
        for it in range(self.num_time_steps):
            quad_goal = se3.Quaternion(
                se3.rpy.rpyToMatrix(np.matrix(self.base_des[:, it]).T))
            quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
                                    float(q[5]))
            amom_ref = self.reg_orientation * se3.log(
                (quad_goal * quad_q.inverse()).matrix()) + self.amom_dyn[it]

            joint_regularization_ref = self.reg_joint_position * (
                self.joint_des[:, it] - q[7:])

            # Fill the kinematics results for it.
            self.inv_kin.forward_robot(q, dq)
            self.fill_kinematic_result(it, q, dq)

            dq = self.inv_kin.compute(q, dq, self.com_dyn[it],
                                      self.lmom_dyn[it], amom_ref,
                                      self.endeff_pos_ref[it],
                                      self.endeff_vel_ref[it],
                                      self.endeff_contact[it],
                                      joint_regularization_ref)

            # Integrate to the next state.
            q = se3.integrate(self.robot.model, q, dq * self.dt)
Exemplo n.º 32
0
 def calc(self, data, x, u):
     data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame]
     data.residuals[:] = m2a(pinocchio.log(data.rMf).vector)
     data.cost = sum(self.activation.calc(data.activation, data.residuals))
     return data.cost
Exemplo n.º 33
0
N = 1000
v = zero(3); v[2] = 1.0 / N
w = zero(3); w[1] = 1.0 / N
nu = se3.Motion( v, w )

M = se3.SE3.Identity()
updateJointConfiguration(M,'HeadRollLink')

for i in range(N):
   M = M*se3.exp(nu)
   updateJointConfiguration(M,'HeadRollLink')

N = 1000
Mdes = se3.SE3.Random()
nu = se3.log(M.inverse()*Mdes)

for i in range(N):
    M = M*se3.exp(nu.vector()/N)
    updateJointConfiguration(M,'HeadRollLink')
 
def errorInSE3( M,Mdes):
  '''
    Compute a 6-dim error vector (6x1 np.maptrix) caracterizing the difference
    between M and Mdes, both element of SE3.
  '''
  error = se3.log(M.inverse()*Mdes)
  return error.vector()

N = 1000
Mdes = se3.SE3.Random()