def UniThrustControlAngVel(ep,ev,evDot,n,G_all,parameters):
u,Td,nTd,Tdnorm,uDot,TdDot,nTdDot,TdnormDot = UniThrustControlDot(ep,ev,evDot,G_all,parameters)
# gradV = LyapunovGrad(block,ep,ev);
gradV,Vgrad_grad_ep,Vgrad_grad_ev = LyapunovGrad2_3D(ep,ev,parameters)
# gains for angular control
ktt2 = parameters.ktt2
ktt = parameters.ktt
# desired angular velocity
wd = ktt2/ktt*skew(n).dot(nTd) + skew(nTd).dot(TdDot)/numpy.linalg.norm(Td) + skew(n).dot(gradV)*(numpy.linalg.norm(Td))*1/ktt;
return wd
def UniThrustControlAngVel(ep, ev, evDot, n, G_all, parameters):
u, Td, nTd, Tdnorm, uDot, TdDot, nTdDot, TdnormDot = UniThrustControlDot(
ep, ev, evDot, G_all, parameters)
# gradV = LyapunovGrad(block,ep,ev);
gradV, Vgrad_grad_ep, Vgrad_grad_ev = LyapunovGrad2_3D(ep, ev, parameters)
# gains for angular control
ktt2 = parameters.ktt2
ktt = parameters.ktt
# desired angular velocity
wd = ktt2 / ktt * skew(n).dot(nTd) + skew(nTd).dot(
TdDot) / numpy.linalg.norm(Td) + skew(n).dot(gradV) * (
numpy.linalg.norm(Td)) * 1 / ktt
return wd
def sys_dynamics(states, U, parameters):
# U = Full actuation vehicles
# acceleration due to gravity (m/s^2)
g = parameters.g
# mass of vehicles (kg)
m = parameters.m
# third canonical basis vector
e3 = numpy.array([0.0, 0.0, 1.0])
# states
# transported mass: position and velocity
x = states[0:3]
v = states[3:6]
# thrust unit vector
R = states[6:15]
R = numpy.reshape(R, (3, 3))
n = R.dot(e3)
# ----------------------------------------------#
# ----------------------------------------------#
# rearrage to proper order
# [roll,pitch,throttle,yaw] in sticks
# in model order is [throttle,roll,pitch,yaw]
U_new = numpy.zeros(4)
U_new[0] = U[2]
U_new[1] = U[0]
U_new[2] = U[1]
U_new[3] = U[3]
U = U_new
# ----------------------------------------------#
# This model would require more work
# U[0:1] : as desired full actuation
# Td = U[0:3];
# Tddot = .... some derivator
# Tdnorm = numpy.linalg.norm(Td);
# nTd = Td/Tdnorm;
# w_feedforward = skew(nTd).dot(TdDot)/numpy.linalg.norm(Td)
# w = ktt*skew(n).dot(nTd) + wd_feedforward
# ----------------------------------------------#
# current euler angles
ee = GetEulerAngles(R)
# current psi
psi = ee[2]
# degrees per second
MAX_PSI_SPEED_Deg = parameters.MAX_PSI_SPEED_Deg
MAX_PSI_SPEED_Rad = MAX_PSI_SPEED_Deg * math.pi / 180.0
MAX_ANGLE_DEG = parameters.MAX_ANGLE_DEG
MAX_ANGLE_RAD = MAX_ANGLE_DEG * math.pi / 180.0
# desired roll and pitch
# U[1:3] : desired roll and pitch
roll_des = (U[1] - 1500.0) * MAX_ANGLE_RAD / 500.0
# ATTENTTION TO PITCH: WHEN STICK IS FORWARD PITCH,
# pwm GOES TO 1000, AND PITCH IS POSITIVE
pitch_des = -(U[2] - 1500) * MAX_ANGLE_RAD / 500.0
# desired euler angles
ee_des = numpy.array([roll_des, pitch_des, psi])
# gain of inner loop for attitude control
ktt = parameters.ktt_inner_loop
nTd = GetRotFromEulerAngles(ee_des).dot(e3)
w = ktt * skew(n).dot(nTd)
RT = numpy.transpose(R)
w = RT.dot(w)
# U[3]: yaw angular speed
w[2] = -(U[3] - 1500.0) * MAX_PSI_SPEED_Rad / 500.0
# Throttle neutral
Throttle_neutral = parameters.Throttle_neutral
K_Throttle = m * g / Throttle_neutral
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
# Throttle = U[0]
# this increases the actual throtle
Throttle = U[0] / numpy.dot(n, e3)
pDot = v
# acceleration of quad
vDot = K_Throttle * (Throttle * n) / m - g * e3
RDot = R.dot(skew(w))
RDot = numpy.reshape(RDot, 9)
# collecting derivatives
derivatives = numpy.concatenate([pDot, vDot, RDot])
return derivatives
def backstep_ctrl( p, v, r, omega, gstar, d_gstar, dd_gstar):
# evaluate controller for double integrator
u, u_p, u_v, u_p_p, u_v_v, u_p_v, Vpv, d_Vpv, Vpv_p, Vpv_v, Vpv_v_p, Vpv_v_v = _bounded_ctrl(p, v)
kr = .5
wr = 250.
womega = 20.
komega = .5
Ul = u + gstar
Tl = dot(Ul, r)
# Tl = Ul[2] # for testing
# Lie derivative of v
d_v = u - (eye(3) - outer(r, r)).dot(Ul)
rstar = Ul/norm(Ul)
# Lie derivative of Ul
d_Ul = diagonal(u_p)*v + diagonal(u_v)*d_v + d_gstar
# turn 3rd-order tensors into vectors (assuming u is component wise -> tensors are diagonal)
u_p_p_vec = sum(u_p_p, (1, 2))
u_p_v_vec = sum(u_p_v, (1, 2))
u_v_v_vec = sum(u_v_v, (1, 2))
# Lie derivative of partial derivatives of u assuming u is component wise
d_u_p = diagflat(u_p_p_vec*v + u_p_v_vec*d_v)
d_u_v = diagflat(u_p_v_vec*v + u_v_v_vec*d_v)
d_r = skew(omega).dot(r)
dd_v = u_p.dot(v) + u_v.dot(d_v) + (outer(d_r, r) + outer(r, d_r)).dot(Ul) - (eye(3) - outer(r, r)).dot(
d_Ul)
dd_Ul = d_u_p.dot(v) + u_p.dot(d_v) + d_u_v.dot(d_v) + u_v.dot(dd_v) + dd_gstar
d_rstar = (eye(3) - outer(rstar, rstar)).dot(d_Ul/norm(Ul))
dd_rstar = -(outer(d_rstar, rstar) + outer(rstar, d_rstar)).dot(d_Ul)/norm(Ul) + (
eye(3) - outer(rstar, rstar)).dot(dd_Ul/norm(Ul) - d_Ul*inner(d_Ul, Ul)/norm(Ul)**3)
omegastar = kr*skew(r).dot(rstar) + skew(rstar).dot(d_rstar) - 1./wr*norm(Ul)*skew(r).dot(Vpv_v)
d_omegastar = kr*skew(d_r).dot(rstar) + kr*skew(r).dot(d_rstar) + skew(rstar).dot(dd_rstar) - inner(Ul, d_Ul)/(
wr*norm(Ul))*skew(r).dot(Vpv_v) - norm(Ul)/wr*skew(d_r).dot(Vpv_v) - norm(Ul)/wr*skew(r).dot(
Vpv_v_p.dot(v) + Vpv_v_v.dot(d_v))
# this is the actual control input
tau = wr/womega*rstar + skew(d_r).dot(omega - omegastar) + komega*skew(r).dot(omega - omegastar) - skew(r).dot(
d_omegastar)
tau = (eye(3) - outer(r,r)).dot(tau)
# Lyapunov function
V = Vpv + wr*(1. - inner(r, rstar)) + .5*womega*norm(skew(r).dot(omega - omegastar))**2
d_V = Vpv_p.dot(v) + Vpv_v.dot(u) - wr*kr*norm(skew(r).dot(rstar))**2 - womega*komega*norm(
skew(r).dot(omega - omegastar))**2
rospy.logwarn('Tl: '+str(Tl)+' Ul:'+str(Ul))
rospy.logwarn('rl. '+str(r))
return Tl, tau, d_V, V
def controller(states,states_load,states_d,parameters):
# mass of vehicles (kg)
m = parameters.m
ml = .136
# is there some onboard thrust control based on the vertical acceleration?
ml = .01
# acceleration due to gravity (m/s^2)
g = parameters.g
rospy.logwarn('g: '+str(g))
# Angular velocities multiplication factor
# Dw = parameters.Dw
# third canonical basis vector
e3 = numpy.array([0.0,0.0,1.0])
#--------------------------------------#
# transported mass: position and velocity
p = states[0:3];
v = states[3:6];
# thrust unit vector and its angular velocity
R = states[6:15];
R = numpy.reshape(R,(3,3))
r3 = R.dot(e3)
# load
pl = states_load[0:3]
vl = states_load[3:6]
Lmeas = norm(p-pl)
rospy.logwarn('rope length: '+str(Lmeas))
L = 0.66
#rospy.logwarn('param Throttle_neutral: '+str(parameters.Throttle_neutral))
rl = (p-pl)/Lmeas
omegal = skew(rl).dot(v-vl)/Lmeas
omegal = zeros(3)
#--------------------------------------#
# desired quad trajectory
pd = states_d[0:3] - L*e3;
vd = states_d[3:6];
ad = states_d[6:9];
jd = states_d[9:12];
sd = states_d[12:15];
# rospy.logwarn(numpy.concatenate((v,vl,vd)))
#--------------------------------------#
# position error and velocity error
ep = pl - pd
ev = vl - vd
rospy.logwarn('ep: '+str(ep))
#--------------------------------------#
gstar = g*e3 + ad
d_gstar = jd
dd_gstar = sd
#rospy.logwarn('rl: '+str(rl))
#rospy.logwarn('omegal: '+str(omegal))
#rospy.logwarn('ev: '+str(ev))
# temp override
#rl = array([0.,0.,1.])
#omegal = zeros(3)
#L = 1
#--------------------------------------#
# rospy.logwarn('ep='+str(ep)+' ev='+str(ev)+' rl='+str(rl)+' omegal='+str(omegal))
Tl, tau, _, _ = backstep_ctrl(ep, ev, rl, omegal, gstar, d_gstar, dd_gstar)
rospy.logwarn('g: '+str(g))
U = (eye(3)-outer(rl,rl)).dot(tau*m*L)+rl*(
# -m/L*inner(v-vl,v-vl)
+Tl*(m+ml))
U = R.T.dot(U)
n = rl
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
# Full_actuation = m*(ad + u + g*e3 + self.d_est)
Full_actuation = U
# -----------------------------------------------------------------------------#
U = numpy.zeros(4)
Throttle = numpy.dot(Full_actuation,r3)
# this decreases the throtle, which will be increased
Throttle = Throttle*numpy.dot(n,r3)
U[0] = Throttle
#--------------------------------------#
# current euler angles
euler = GetEulerAngles(R)
# current phi and theta
phi = euler[0];
theta = euler[1];
# current psi
psi = euler[2];
#--------------------------------------#
# Rz(psi)*Ry(theta_des)*Rx(phi_des) = r3_des
# desired roll and pitch angles
r3_des = Full_actuation/numpy.linalg.norm(Full_actuation)
r3_des_rot = Rz(-psi).dot(r3_des)
sin_phi = -r3_des_rot[1]
sin_phi = bound(sin_phi,1,-1)
phi = numpy.arcsin(sin_phi)
U[1] = phi
sin_theta = r3_des_rot[0]/c(phi)
sin_theta = bound(sin_theta,1,-1)
cos_theta = r3_des_rot[2]/c(phi)
cos_theta = bound(cos_theta,1,-1)
pitch = numpy.arctan2(sin_theta,cos_theta)
U[2] = pitch
#--------------------------------------#
# yaw control: gain
k_yaw = parameters.k_yaw;
# desired yaw: psi_star
psi_star = parameters.psi_star;
psi_star_dot = 0;
psi_dot = psi_star_dot - k_yaw*s(psi - psi_star);
U[3] = 1/c(phi)*(c(theta)*psi_dot - s(phi)*U[2]);
U = Cmd_Converter(U,parameters)
return U
def sys_dynamics(states, U, parameters):
# U = Full actuation vehicles
# acceleration due to gravity (m/s^2)
g = parameters.g
# mass of vehicles (kg)
m = parameters.m
# third canonical basis vector
e3 = numpy.array([0.0, 0.0, 1.0])
# states
# transported mass: position and velocity
x = states[0:3]
v = states[3:6]
# thrust unit vector
R = states[6:15]
R = numpy.reshape(R, (3, 3))
n = R.dot(e3)
#------------------------------------------------#
#------------------------------------------------#
# rearrage to proper order
# [roll,pitch,throttle,yaw] in sticks
# in model order is [throttle,roll,pitch,yaw]
U_new = numpy.zeros(4)
U_new[0] = U[2]
U_new[1] = U[0]
U_new[2] = U[1]
U_new[3] = U[3]
U = U_new
#------------------------------------------------#
#------------------------------------------------#
T = U[0]
w = U[1:4]
# Throttle neutral
Throttle_neutral = parameters.Throttle_neutral
ACRO_RP_P = parameters.ACRO_RP_P
K_Throttle = m * g / Throttle_neutral
Max = ACRO_RP_P * 4500 / 100 * math.pi / 180
w[0] = (w[0] - 1500) / 500 * Max
w[1] = -(w[1] - 1500) / 500 * Max
w[2] = -(w[2] - 1500) / 500 * Max
pDot = v
# acceleration of quad
vDot = K_Throttle * (T * n) / m - g * e3
RDot = R.dot(skew(w))
RDot = numpy.reshape(RDot, 9)
# collecting derivatives
derivatives = numpy.concatenate([pDot, vDot, RDot])
return derivatives
def sys_dynamics(states , U , parameters):
# U = Full actuation vehicles
# acceleration due to gravity (m/s^2)
g = parameters.g
# mass of vehicles (kg)
m = parameters.m
# third canonical basis vector
e3 = numpy.array([0.0,0.0,1.0])
# states
# transported mass: position and velocity
x = states[0:3];
v = states[3:6];
# thrust unit vector
R = states[6:15];
R = numpy.reshape(R,(3,3))
n = R.dot(e3)
# ----------------------------------------------#
# ----------------------------------------------#
# rearrage to proper order
# [roll,pitch,throttle,yaw] in sticks
# in model order is [throttle,roll,pitch,yaw]
U_new = numpy.zeros(4)
U_new[0] = U[2]
U_new[1] = U[0]
U_new[2] = U[1]
U_new[3] = U[3]
U = U_new
# ----------------------------------------------#
# This model would require more work
# U[0:1] : as desired full actuation
# Td = U[0:3];
# Tddot = .... some derivator
# Tdnorm = numpy.linalg.norm(Td);
# nTd = Td/Tdnorm;
# w_feedforward = skew(nTd).dot(TdDot)/numpy.linalg.norm(Td)
# w = ktt*skew(n).dot(nTd) + wd_feedforward
# ----------------------------------------------#
# current euler angles
ee = GetEulerAngles(R)
# current psi
psi = ee[2]
# degrees per second
MAX_PSI_SPEED_Deg = parameters.MAX_PSI_SPEED_Deg
MAX_PSI_SPEED_Rad = MAX_PSI_SPEED_Deg*math.pi/180.0
MAX_ANGLE_DEG = parameters.MAX_ANGLE_DEG
MAX_ANGLE_RAD = MAX_ANGLE_DEG*math.pi/180.0
# desired roll and pitch
# U[1:3] : desired roll and pitch
roll_des = (U[1] - 1500.0)*MAX_ANGLE_RAD/500.0;
# ATTENTTION TO PITCH: WHEN STICK IS FORWARD PITCH,
# pwm GOES TO 1000, AND PITCH IS POSITIVE
pitch_des = -(U[2] - 1500)*MAX_ANGLE_RAD/500.0;
# desired euler angles
ee_des = numpy.array([roll_des,pitch_des,psi])
# gain of inner loop for attitude control
ktt = parameters.ktt_inner_loop
nTd = GetRotFromEulerAngles(ee_des).dot(e3);
w = ktt*skew(n).dot(nTd)
RT = numpy.transpose(R)
w = RT.dot(w)
# U[3]: yaw angular speed
w[2] = -(U[3] - 1500.0)*MAX_PSI_SPEED_Rad/500.0
# Throttle neutral
Throttle_neutral = parameters.Throttle_neutral
K_Throttle = m*g/Throttle_neutral
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
# Throttle = U[0]
# this increases the actual throtle
Throttle = U[0]/numpy.dot(n,e3)
pDot = v
# acceleration of quad
vDot = K_Throttle*(Throttle*n)/m - g*e3;
RDot = R.dot(skew(w))
RDot = numpy.reshape(RDot,9)
# collecting derivatives
derivatives = numpy.concatenate([pDot,vDot,RDot])
return derivatives
def sys_dynamics(states , U , parameters):
# U = Full actuation vehicles
# acceleration due to gravity (m/s^2)
g = parameters.g
# mass of vehicles (kg)
m = parameters.m
# third canonical basis vector
e3 = numpy.array([0.0,0.0,1.0])
# states
# transported mass: position and velocity
x = states[0:3];
v = states[3:6];
# thrust unit vector
R = states[6:15];
R = numpy.reshape(R,(3,3))
n = R.dot(e3)
#------------------------------------------------#
#------------------------------------------------#
# rearrage to proper order
# [roll,pitch,throttle,yaw] in sticks
# in model order is [throttle,roll,pitch,yaw]
U_new = numpy.zeros(4)
U_new[0] = U[2]
U_new[1] = U[0]
U_new[2] = U[1]
U_new[3] = U[3]
U = U_new
#------------------------------------------------#
#------------------------------------------------#
T = U[0];
w = U[1:4]
# Throttle neutral
Throttle_neutral = parameters.Throttle_neutral
ACRO_RP_P = parameters.ACRO_RP_P
K_Throttle = m*g/Throttle_neutral
Max = ACRO_RP_P*4500/100*math.pi/180;
w[0] = (w[0] - 1500)/500*Max;
w[1] = -(w[1] - 1500)/500*Max;
w[2] = -(w[2] - 1500)/500*Max;
pDot = v
# acceleration of quad
vDot = K_Throttle*(T*n)/m - g*e3;
RDot = R.dot(skew(w))
RDot = numpy.reshape(RDot,9)
# collecting derivatives
derivatives = numpy.concatenate([pDot,vDot,RDot])
return derivatives
def controller(states, states_load, states_d, parameters):
# mass of vehicles (kg)
m = parameters.m
ml = .136
# is there some onboard thrust control based on the vertical acceleration?
ml = .01
# acceleration due to gravity (m/s^2)
g = parameters.g
rospy.logwarn('g: ' + str(g))
# Angular velocities multiplication factor
# Dw = parameters.Dw
# third canonical basis vector
e3 = numpy.array([0.0, 0.0, 1.0])
#--------------------------------------#
# transported mass: position and velocity
p = states[0:3]
v = states[3:6]
# thrust unit vector and its angular velocity
R = states[6:15]
R = numpy.reshape(R, (3, 3))
r3 = R.dot(e3)
# load
pl = states_load[0:3]
vl = states_load[3:6]
Lmeas = norm(p - pl)
rospy.logwarn('rope length: ' + str(Lmeas))
L = 0.66
#rospy.logwarn('param Throttle_neutral: '+str(parameters.Throttle_neutral))
rl = (p - pl) / Lmeas
omegal = skew(rl).dot(v - vl) / Lmeas
omegal = zeros(3)
#--------------------------------------#
# desired quad trajectory
pd = states_d[0:3] - L * e3
vd = states_d[3:6]
ad = states_d[6:9]
jd = states_d[9:12]
sd = states_d[12:15]
# rospy.logwarn(numpy.concatenate((v,vl,vd)))
#--------------------------------------#
# position error and velocity error
ep = pl - pd
ev = vl - vd
rospy.logwarn('ep: ' + str(ep))
#--------------------------------------#
gstar = g * e3 + ad
d_gstar = jd
dd_gstar = sd
#rospy.logwarn('rl: '+str(rl))
#rospy.logwarn('omegal: '+str(omegal))
#rospy.logwarn('ev: '+str(ev))
# temp override
#rl = array([0.,0.,1.])
#omegal = zeros(3)
#L = 1
#--------------------------------------#
# rospy.logwarn('ep='+str(ep)+' ev='+str(ev)+' rl='+str(rl)+' omegal='+str(omegal))
Tl, tau, _, _ = backstep_ctrl(ep, ev, rl, omegal, gstar, d_gstar, dd_gstar)
rospy.logwarn('g: ' + str(g))
U = (eye(3) - outer(rl, rl)).dot(tau * m * L) + rl * (
# -m/L*inner(v-vl,v-vl)
+Tl * (m + ml))
U = R.T.dot(U)
n = rl
# -----------------------------------------------------------------------------#
# STABILIZE MODE:APM COPTER
# The throttle sent to the motors is automatically adjusted based on the tilt angle
# of the vehicle (i.e increased as the vehicle tilts over more) to reduce the
# compensation the pilot must fo as the vehicles attitude changes
# -----------------------------------------------------------------------------#
# Full_actuation = m*(ad + u + g*e3 + self.d_est)
Full_actuation = U
# -----------------------------------------------------------------------------#
U = numpy.zeros(4)
Throttle = numpy.dot(Full_actuation, r3)
# this decreases the throtle, which will be increased
Throttle = Throttle * numpy.dot(n, r3)
U[0] = Throttle
#--------------------------------------#
# current euler angles
euler = GetEulerAngles(R)
# current phi and theta
phi = euler[0]
theta = euler[1]
# current psi
psi = euler[2]
#--------------------------------------#
# Rz(psi)*Ry(theta_des)*Rx(phi_des) = r3_des
# desired roll and pitch angles
r3_des = Full_actuation / numpy.linalg.norm(Full_actuation)
r3_des_rot = Rz(-psi).dot(r3_des)
sin_phi = -r3_des_rot[1]
sin_phi = bound(sin_phi, 1, -1)
phi = numpy.arcsin(sin_phi)
U[1] = phi
sin_theta = r3_des_rot[0] / c(phi)
sin_theta = bound(sin_theta, 1, -1)
cos_theta = r3_des_rot[2] / c(phi)
cos_theta = bound(cos_theta, 1, -1)
pitch = numpy.arctan2(sin_theta, cos_theta)
U[2] = pitch
#--------------------------------------#
# yaw control: gain
k_yaw = parameters.k_yaw
# desired yaw: psi_star
psi_star = parameters.psi_star
psi_star_dot = 0
psi_dot = psi_star_dot - k_yaw * s(psi - psi_star)
U[3] = 1 / c(phi) * (c(theta) * psi_dot - s(phi) * U[2])
U = Cmd_Converter(U, parameters)
return U
def backstep_ctrl(p, v, r, omega, gstar, d_gstar, dd_gstar):
# evaluate controller for double integrator
u, u_p, u_v, u_p_p, u_v_v, u_p_v, Vpv, d_Vpv, Vpv_p, Vpv_v, Vpv_v_p, Vpv_v_v = _bounded_ctrl(
p, v)
kr = .5
wr = 250.
womega = 20.
komega = .5
Ul = u + gstar
Tl = dot(Ul, r)
# Tl = Ul[2] # for testing
# Lie derivative of v
d_v = u - (eye(3) - outer(r, r)).dot(Ul)
rstar = Ul / norm(Ul)
# Lie derivative of Ul
d_Ul = diagonal(u_p) * v + diagonal(u_v) * d_v + d_gstar
# turn 3rd-order tensors into vectors (assuming u is component wise -> tensors are diagonal)
u_p_p_vec = sum(u_p_p, (1, 2))
u_p_v_vec = sum(u_p_v, (1, 2))
u_v_v_vec = sum(u_v_v, (1, 2))
# Lie derivative of partial derivatives of u assuming u is component wise
d_u_p = diagflat(u_p_p_vec * v + u_p_v_vec * d_v)
d_u_v = diagflat(u_p_v_vec * v + u_v_v_vec * d_v)
d_r = skew(omega).dot(r)
dd_v = u_p.dot(v) + u_v.dot(d_v) + (outer(d_r, r) + outer(
r, d_r)).dot(Ul) - (eye(3) - outer(r, r)).dot(d_Ul)
dd_Ul = d_u_p.dot(v) + u_p.dot(d_v) + d_u_v.dot(d_v) + u_v.dot(
dd_v) + dd_gstar
d_rstar = (eye(3) - outer(rstar, rstar)).dot(d_Ul / norm(Ul))
dd_rstar = -(outer(d_rstar, rstar) +
outer(rstar, d_rstar)).dot(d_Ul) / norm(Ul) + (eye(3) - outer(
rstar, rstar)).dot(dd_Ul / norm(Ul) -
d_Ul * inner(d_Ul, Ul) / norm(Ul)**3)
omegastar = kr * skew(r).dot(rstar) + skew(rstar).dot(
d_rstar) - 1. / wr * norm(Ul) * skew(r).dot(Vpv_v)
d_omegastar = kr * skew(d_r).dot(rstar) + kr * skew(r).dot(d_rstar) + skew(
rstar).dot(dd_rstar) - inner(Ul, d_Ul) / (wr * norm(Ul)) * skew(r).dot(
Vpv_v) - norm(Ul) / wr * skew(d_r).dot(Vpv_v) - norm(
Ul) / wr * skew(r).dot(Vpv_v_p.dot(v) + Vpv_v_v.dot(d_v))
# this is the actual control input
tau = wr / womega * rstar + skew(d_r).dot(
omega - omegastar) + komega * skew(r).dot(omega - omegastar) - skew(
r).dot(d_omegastar)
tau = (eye(3) - outer(r, r)).dot(tau)
# Lyapunov function
V = Vpv + wr * (1. - inner(r, rstar)) + .5 * womega * norm(
skew(r).dot(omega - omegastar))**2
d_V = Vpv_p.dot(v) + Vpv_v.dot(u) - wr * kr * norm(skew(r).dot(
rstar))**2 - womega * komega * norm(skew(r).dot(omega - omegastar))**2
rospy.logwarn('Tl: ' + str(Tl) + ' Ul:' + str(Ul))
rospy.logwarn('rl. ' + str(r))
return Tl, tau, d_V, V
