def robotint(q,dq):
     M = se3.SE3(se3.Quaternion(q[6,0],q[3,0],q[4,0],q[5,0]).matrix(),q[:3])
     dM = se3.exp(dq[:6])
     M = M*dM
     q[:3] = M.translation
     q[3:7] = se3.Quaternion(M.rotation).coeffs()
     q[7:] += dq[6:]
Exemplo n.º 2
0
    def integrate_dv_R3SO3(self, q, v, dv, dt):
        b_v = v[0:3]
        b_w = v[3:6]
        b_acc = dv[0:3] + np.cross(b_w, b_v)

        p_int = q[:3]
        oRb_int = pin.Quaternion(q[3:7].reshape((4, 1))).toRotationMatrix()
        v_int = oRb_int @ v[:3]

        p_int = p_int + v_int * dt + 0.5 * oRb_int @ b_acc * dt**2
        v_int = v_int + oRb_int @ b_acc * dt
        oRb_int = oRb_int @ pin.exp(b_w * dt)

        q[:3] = p_int
        q[3:7] = pin.Quaternion(oRb_int).coeffs()
        q[7:] += v[6:] * dt
        v += dt * dv
        v[:3] = oRb_int.T @ v_int
        return q, v
Exemplo n.º 3
0
 def test_exp(self):
     m = se3.Motion.Random()
     self.assertApprox(se3.exp(m), se3.exp6FromMotion(m))
Exemplo n.º 4
0
 def test_exp(self):
     m = se3.Motion.Random()
     self.assertApprox(se3.exp(m), se3.exp6FromMotion(m))
from pinocchio.utils import *
norm = npl.norm
import time

N = 1000
M = se3.SE3.Identity()

# Integrate a constant body velocity.
v = zero(3)
v[2] = 1.0 / N
w = zero(3)
w[1] = 1.0 / N
nu = se3.Motion(v, w)

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)
import pinocchio as se3
from pinocchio.utils import *
norm = npl.norm
import time

N = 1000
M = se3.SE3.Identity()

# Integrate a constant body velocity.
v = zero(3); v[2] = 1.0 / N
w = zero(3); w[1] = 1.0 / N
nu = se3.Motion( v, w)

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):
Exemplo n.º 7
0
qdot[3] = 10
for i in range(100):
    q[3] += qdot[3]*dt
    robot.Mrh(q)

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.
  '''
Exemplo n.º 8
0
    Deltat = 0

    v_direct_lst = []
    v_preint_lst = []
    for i in range(N_SIMU):
        v_direct_lst.append(v_int.copy())
        v_preint_lst.append(x_imu_int[1].copy())

        b_a = np.random.random(3) - 0.5
        b_w = np.random.random(3) - 0.5
        b_proper_acc = b_a - oRb_int.T @ gravity

        # direct preint
        p_int = p_int + v_int * dt + 0.5 * oRb_int @ b_a * dt**2
        v_int = v_int + oRb_int @ b_a * dt
        oRb_int = oRb_int @ pin.exp(b_w * dt)

        # preint
        Deltat += dt
        deltak = compute_current_delta_IMU(b_proper_acc, b_w, dt)
        DeltaIMU = compose_delta_IMU(DeltaIMU, deltak, dt)
        x_imu_int = state_plus_delta_IMU(x_imu_ori, DeltaIMU, Deltat)

        # pin
        b_nu_int += pin.Motion(b_dnu * dt)

    import matplotlib.pyplot as plt

    v_direct_arr = np.array(v_direct_lst)
    v_preint_arr = np.array(v_preint_lst)
    v_err = v_direct_arr - v_preint_arr