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:]
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
def test_exp(self):
m = se3.Motion.Random()
self.assertApprox(se3.exp(m), se3.exp6FromMotion(m))
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):
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.
'''
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
