I'm a student and i want to control a quadrotor in the Vrep environment without using the particles control based method.
Since there's no way to control the quadrotor using the motion of the propellers (that as we know generate the thrust and the torques), my control algorithm would directly give as output the trust and the torques that will be simply applied on the quadrotor object using the sim.addForceAndTorque function.
Before implementing my control scheme, as first attempt i tried to implement a simple height control as follows:
Code: Select all
function sysCall_init()
uav = sim.getObjectAssociatedWithScript(sim.handle_self)
mass_uav = sim.getShapeMassAndInertia(uav)
g = -sim.getArrayParameter(sim.arrayparam_gravity)[3]
z_des = 2
zV_des = 0
Kp = 10
Kv = 10
end
function sysCall_actuation()
z_curr = sim.getObjectPosition(uav,-1)[3]
vel,ang = sim.getObjectVelocity(uav,-1)
zV_curr = vel[3]
matrix = sim.getObjectMatrix(uav,-1)
matrix[4] = 0
matrix[8] = 0
matrix[12] = 0
T = mass_uav*(g + Kp*(z_des-z_curr) + Kv*(zV_des-zV_curr)) --height control
thrust_vrepFrame = sim.multiplyVector(matrix,{0,0,T})
sim.addForceAndTorque(uav,thrust_vrepFrame,{0,0,0.0})
end
Then, i added an attitude control, while keeping the quadrotor at constant height and starting with orientation = (0,0,0), as follows:
Code: Select all
function sysCall_init()
uav = sim.getObjectAssociatedWithScript(sim.handle_self)
mass_uav = sim.getShapeMassAndInertia(uav)
g = -sim.getArrayParameter(sim.arrayparam_gravity)[3]
z_des = 1
zV_des = 0
Kp = 20
Kv = 10
Kp_tau = 0.01
Kv_tau = 0.01
ori_des = {(0)*3.14/180,(0)*3.14/180,(10)*3.14/180}--conversion from degree to radiant
oriV_des = {0,0,0}
end
function sysCall_actuation()
matrix= sim.getObjectMatrix(uav,-1)
matrix[4] = 0
matrix[8] = 0
matrix[12] = 0
z_curr = sim.getObjectPosition(uav,-1)[3]
ori_curr = sim.getObjectOrientation(uav,-1)
vel,oriV_curr = sim.getObjectVelocity(uav,-1)
zV_curr = vel[3]
T = mass_uav*(g + Kp*(z_des-z_curr) + Kv*(zV_des-zV_curr)) --height control
thrust_vrepFrame = sim.multiplyVector(matrix,{0,0,T})
tau_phi = Kp_tau*(ori_des[1]-ori_curr[1]) + Kv_tau*(oriV_des[1]-oriV_curr[1]) --wrt angle along x axis of the quadrotor
tau_alpha = Kp_tau*(ori_des[2]-ori_curr[2]) + Kv_tau*(oriV_des[2]-oriV_curr[2]) --wrt angle along y axis of the quadrotor
tau_psi = Kp_tau*(ori_des[3]-ori_curr[3]) + Kv_tau*(oriV_des[3]-oriV_curr[3]) --wrt angle along z axis of the quadrotor
tau = {tau_phi,tau_alpha,tau_psi} --attitude control
sim.addForceAndTorque(uav,thrust_vrepFrame,tau)
end
- if i give only a desired psi (like in the previous code) which is smaller than 31 degree the control works fine (the quadrotor goes to the desired psi), otherwise the quadrotor starts to move in an uncontrollable way
- if i give only a desired alpha, the control works fine for any possible value and the quadrotor starts moving in the y direction as expected.
- if i give only a desired phi, the quadrotor starts to move in an uncontrollable way for any possible value, even small one (e.g., phi = 0.000001 degree)
Besides i also notice that changing the starting psi value leads to an uncontrollable behavior even with only a small desired value for alpha.
So my questions are:
1)Is this behavior in some way related to how addForceAndTorque apply a torque on the quadrotor?
2)Is there a way to correctly apply an attitude control like the one proposed here?
3)If i have a reference velocity, e.g. i want to drag the quadrotor in the y direction, is there a way in Vrep to correctly drag the robot, that is, continuously changing its orientation to reorient the thrust vector, without using particles? I know that there are control methods that starting from a desired pose return a desired thrust and torque but i don't know who to apply these commands in Vrep in a proper way.
Thank you in advance,
Regards