def run_quadrotor(self,dt,fb,taub):
""" Dynamic/Differential equations for rigid body motion/flight """
# dpos/dt = ve
self.d_pos = self.ve
# q = [ s ] = [ s v1 v2 v3]^T
# [ v ]
# dq/dt = 0.5*[ -v ]
# [ sI3 + skew(v) ] * omegab
# version 1, unfolded - fastest, but still lower than rotm
self.d_q=np.array(
[(-self.q[1]*self.omegab[0]
-self.q[2]*self.omegab[1]-self.q[3]*self.omegab[2]),
(self.q[0]*self.omegab[0]
-self.q[3]*self.omegab[1]+self.q[2]*self.omegab[2]),
(self.q[3]*self.omegab[0]
+self.q[0]*self.omegab[1]-self.q[1]*self.omegab[2]),
(-self.q[2]*self.omegab[0]
+self.q[1]*self.omegab[1]+self.q[0]*self.omegab[2])
])*0.5
# version 2, more compact but slower than version 1
#d_q = 0.5*np.dot(
# np.block([[-self.q[1:3+1]],
# [self.q[0]*np.identity(3)+ut.skew(self.q[1:3+1])]]),
# self.omegab
# )
# dve/dt = 1/m*Rbe*fb + g
self.d_ve = 1/self.mass*utils.quat2rotm(self.q)@fb + np.array([0,0,-envir.g ])
# domegab/dt = I^(-1)*(-skew(omegab)*I*omegabb + taub)
self.d_omegab = (np.dot(
self.invI,
np.dot(
-utils.skew(self.omegab),
np.dot(self.I,self.omegab)
)
+ taub
)
)
# Integrate for over dt
# Simple Euler, the step dt must be small
X = np.concatenate([self.pos, self.q, self.ve, self.omegab])
dX = np.concatenate([ self.d_pos, self.d_q, self.d_ve, self.d_omegab ])
X = X + dt*dX
# unpack the vector state
self.pos = X[1-1:3]
self.q = X[4-1:7] / np.linalg.norm(X[4-1:7]) #update and normalize
self.rotmb2e = utils.quat2rotm(self.q)
self.rpy = utils.rotm2exyz(self.rotmb2e)
self.ve = X[8-1:10]
self.vb =np.transpose(self.rotmb2e)@self.ve
self.omegab = X[11-1:13]
def handle_panda_ikfast(self, req):
"""
ROS service handler
"""
quat = req.quat
pos = req.pos
# Setting the object pose
if self.obj:
obj_quat = req.obj_quat
obj_pos = req.obj_pos
T_obj = np.eye(4)
T_obj[:3, :3] = utils.quat2rotm(obj_quat)
T_obj[:3, 3] = obj_pos
self.obj.SetTransform(T_obj)
rospy.loginfo("Computing IK for ({}, {})".format(quat, pos))
ik_sols = self.get_ik_solutions(quat, pos)
rospy.loginfo("Got {} sets of IK solutions".format(ik_sols.shape))
ik_rank, ik_scores = self.rank_ik_sols(ik_sols)
ik_sols = ik_sols.flatten()
return [ik_sols, ik_rank]
def quadrotor_dt_simple(X, t, velb, omegab):
q = X[3:7] / np.linalg.norm(X[3:7]) #update and normalize
rotmb2e = utils.quat2rotm(q)
d_pos = rotmb2e @ velb
# q = [ s ] = [ s v1 v2 v3]^T
# [ v ]
# dq/dt = 0.5* [ -v^T ]
# [ sI3 + skew(v) ] * omegab
# version 1, unfolded - fastest, but still lower than rotm
d_q = np.array([(-q[1] * omegab[0] - q[2] * omegab[1] - q[3] * omegab[2]),
(q[0] * omegab[0] - q[3] * omegab[1] + q[2] * omegab[2]),
(q[3] * omegab[0] + q[0] * omegab[1] - q[1] * omegab[2]),
(-q[2] * omegab[0] + q[1] * omegab[1] + q[0] * omegab[2])
]) * 0.5
# version 2, more compact but slower than version 1
#d_q = 0.5*np.dot(
# np.block([[-q[1:3+1]],
# [q[0]*np.identity(3)+utils.skew(q[1:3+1])]]),
# omegab
# )
#version 3
#d_q = 0.5 * utils.quaternion_multiply(np.array([0,omegab[0],omegab[1],omegab[2]]),q)
return np.concatenate([d_pos, d_q])
def run_quadrotor(self, dt, fb, taub):
""" Dynamic/Differential equations for rigid body motion/flight """
# ODE Integration
X = np.concatenate([self.pos, self.q, self.ve, self.omegab])
Y = odeint(quadrotor_dt,
X,
np.array([0, dt]),
args=(
self.mass,
self.I,
self.invI,
fb,
taub,
))
Y = Y[1] # X[0]=x(t=t0)
# unpack the vector state
self.pos = Y[1 - 1:3]
self.q = Y[4 - 1:7] / np.linalg.norm(Y[4 - 1:7]) #update and normalize
self.rotmb2e = utils.quat2rotm(self.q)
self.rpy = utils.rotm2rpy(self.rotmb2e)
self.ve = Y[8 - 1:10]
self.vb = np.transpose(self.rotmb2e) @ self.ve
self.omegab = Y[11 - 1:13]
def quadrotor_dt(X, t, mass, I, invI, fb, taub):
q = X[3:7] / np.linalg.norm(X[3:7]) #update and normalize
ve = X[7:10]
omegab = X[10:13]
d_pos = ve
# q = [ s ] = [ s v1 v2 v3]^T
# [ v ]
# dq/dt = 0.5* [ -v^T ]
# [ sI3 + skew(v) ] * omegab
# version 1, unfolded - fastest, but still lower than rotm
d_q = np.array([(-q[1] * omegab[0] - q[2] * omegab[1] - q[3] * omegab[2]),
(q[0] * omegab[0] - q[3] * omegab[1] + q[2] * omegab[2]),
(q[3] * omegab[0] + q[0] * omegab[1] - q[1] * omegab[2]),
(-q[2] * omegab[0] + q[1] * omegab[1] + q[0] * omegab[2])
]) * 0.5
# dve/dt = 1/m*Rbe*fb + g
d_ve = 1 / mass * utils.quat2rotm(q) @ fb + np.array([0, 0, -envir.g])
# domegab/dt = I^(-1)*(-skew(omegab)*I*omegabb + taub)
d_omegab = (np.dot(invI,
np.dot(-utils.skew(omegab), np.dot(I, omegab)) + taub))
return np.concatenate([d_pos, d_q, d_ve, d_omegab])
def __init__(self,pos,q,ve,omegab,mass,I):
self.pos = pos
""" Position vector, float in R3, meters """
self.q = q
""" quaternion, in the form [ scalar vector3 ] """
self.rotmb2e = utils.quat2rotm(q)
""" the rotation matrix """
self.rpy = utils.rotm2exyz(utils.quat2rotm(self.q),1)
self.ve = ve
""" velocity vector in earth frame, float in R3, m/s """
self.vb =np.transpose(self.rotmb2e)@self.ve
""" velocity vector in body frame, float in R3, m/s """
self.omegab = omegab
"""angular velocity vector, float in R3, rad/s"""
self.mass = mass
""" body mass, in kg """
self.I = I
""" Inertia Matrix in the body frame
I = [ Ixx -Ixy -Ixz
-Ixy Iyy -Iyz
-Ixz -Iyz Izz ],
where
Ixx, Iyy, Izz - Moments of Inertia around body'sown 3-axes
Ixx = Integral_Volume (y*y + z*z) dm
Iyy = Integral_Volume (x*x + z*z) dm
Izz = Integral_Volume (x*x + y*y) dm
Ixy, Ixz, Iyz - Products of Inertia
Ixy = Integral_Volume (xy) dm
Ixz = Integral_Volume (xz) dm
Iyz = Integral_Volume (yz) dm
"""
self.invI = np.linalg.inv(self.I)
self.d_pos = np.zeros(3)
self.d_q = np.zeros(4)
self.d_ve = np.zeros(3)
self.d_omegab = np.zeros(3)
def get_ik_solutions(self, quat, pos):
"""
Get the IK solutions for a pose.
@type quat: list
@param quat: quaternion in (qw, qx, qy, qz)
@rtype: numpy.ndarray
@return: list of ik solutions
"""
rotm = utils.quat2rotm(quat)
T = np.eye(4)
T[:3, :3] = rotm
T[:3, 3] = pos
ik_solutions = self.manip.FindIKSolutions(T, IK_CHECK_COLLISION)
return ik_solutions
def run_quadrotor_simple(self, dt, vb, omegab):
""" Dynamic/Differential equations for rigid body motion/flight """
# ODE Integration
X = np.concatenate([self.pos, self.q])
Y = odeint(quadrotor_dt_simple,
X,
np.array([0, dt]),
args=(
vb,
omegab,
))
Y = Y[1] # X[0]=x(t=t0)
# unpack the vector state
self.pos = Y[1 - 1:3]
self.q = Y[4 - 1:7] / np.linalg.norm(Y[4 - 1:7]) #update and normalize
self.rotmb2e = utils.quat2rotm(self.q)
self.rpy = utils.rotm2rpy(self.rotmb2e)
def ros_transform_to_numpy_transform(transform):
"""ROS message transform to numpy matrix
Args:
-transform (ROS message): transformation, orientation is in quaternion
Returns:
- marker_transformation (4x4 numpy float array): rigid body transformation
"""
# Marker quaternion
marker_quaternion = np.array([transform.transform.rotation.w, transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z])
# Marker rotation matrix
marker_orientation_rotm = utils.quat2rotm(marker_quaternion)
# Marker transformation
marker_position = np.array([transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z])
# Marker rigid body transformation
marker_transformation = np.zeros((4, 4))
marker_transformation[:3, :3] = marker_orientation_rotm
marker_transformation[:3, 3] = marker_position
marker_transformation[3, 3] = 1
return marker_transformation