////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboQuadrotorSimpleController::Update()
{
// Get new commands/state
callback_queue_.callAvailable();
double dt;
if (controlTimer.update(dt) && dt > 0.0) {
// Get Pose/Orientation from Gazebo (if no state subscriber is active)
if (imu_topic_.empty()) {
pose = link->GetWorldPose();
angular_velocity = link->GetWorldAngularVel();
euler = pose.rot.GetAsEuler();
}
if (state_topic_.empty()) {
acceleration = (link->GetWorldLinearVel() - velocity) / dt;
velocity = link->GetWorldLinearVel();
}
// static Time lastDebug;
// if ((world->GetSimTime() - lastDebug).Double() > 0.5) {
// ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Velocity: gazebo = [" << link->GetWorldLinearVel() << "], state = [" << velocity << "]");
// ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Acceleration: gazebo = [" << link->GetWorldLinearAccel() << "], state = [" << acceleration << "]");
// ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Angular Velocity: gazebo = [" << link->GetWorldAngularVel() << "], state = [" << angular_velocity << "]");
// lastDebug = world->GetSimTime();
// }
// Get gravity
math::Vector3 gravity_body = pose.rot.RotateVector(world->GetPhysicsEngine()->GetGravity());
double gravity = gravity_body.GetLength();
double load_factor = gravity * gravity / world->GetPhysicsEngine()->GetGravity().Dot(gravity_body); // Get gravity
// Rotate vectors to coordinate frames relevant for control
math::Quaternion heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2));
math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity);
math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration);
math::Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity);
// update controllers
force.Set(0.0, 0.0, 0.0);
torque.Set(0.0, 0.0, 0.0);
double pitch_command = controllers_.velocity_x.update(velocity_command_.linear.x, velocity_xy.x, acceleration_xy.x, dt) / gravity;
double roll_command = -controllers_.velocity_y.update(velocity_command_.linear.y, velocity_xy.y, acceleration_xy.y, dt) / gravity;
torque.x = inertia.x * controllers_.roll.update(roll_command, euler.x, angular_velocity_body.x, dt);
torque.y = inertia.y * controllers_.pitch.update(pitch_command, euler.y, angular_velocity_body.y, dt);
// torque.x = inertia.x * controllers_.roll.update(-velocity_command_.linear.y/gravity, euler.x, angular_velocity_body.x, dt);
// torque.y = inertia.y * controllers_.pitch.update(velocity_command_.linear.x/gravity, euler.y, angular_velocity_body.y, dt);
torque.z = inertia.z * controllers_.yaw.update(velocity_command_.angular.z, angular_velocity.z, 0, dt);
force.z = mass * (controllers_.velocity_z.update(velocity_command_.linear.z, velocity.z, acceleration.z, dt) + load_factor * gravity);
if (max_force_ > 0.0 && force.z > max_force_) force.z = max_force_;
if (force.z < 0.0) force.z = 0.0;
// static double lastDebugOutput = 0.0;
// if (last_time.Double() - lastDebugOutput > 0.1) {
// ROS_DEBUG_NAMED("quadrotor_simple_controller", "Velocity = [%g %g %g], Acceleration = [%g %g %g]", velocity.x, velocity.y, velocity.z, acceleration.x, acceleration.y, acceleration.z);
// ROS_DEBUG_NAMED("quadrotor_simple_controller", "Command: linear = [%g %g %g], angular = [%g %g %g], roll/pitch = [%g %g]", velocity_command_.linear.x, velocity_command_.linear.y, velocity_command_.linear.z, velocity_command_.angular.x*180/M_PI, velocity_command_.angular.y*180/M_PI, velocity_command_.angular.z*180/M_PI, roll_command*180/M_PI, pitch_command*180/M_PI);
// ROS_DEBUG_NAMED("quadrotor_simple_controller", "Mass: %g kg, Inertia: [%g %g %g], Load: %g g", mass, inertia.x, inertia.y, inertia.z, load_factor);
// ROS_DEBUG_NAMED("quadrotor_simple_controller", "Force: [%g %g %g], Torque: [%g %g %g]", force.x, force.y, force.z, torque.x, torque.y, torque.z);
// lastDebugOutput = last_time.Double();
// }
}
// set force and torque in gazebo
link->AddRelativeForce(force);
link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboQuadrotorStateController::Update()
{
math::Vector3 force, torque;
// Get new commands/state
callback_queue_.callAvailable();
// Get simulator time
common::Time sim_time = world->GetSimTime();
double dt = (sim_time - last_time).Double();
// Update rate is 200/per second
if (dt < 0.005) return;
// Get Pose/Orientation from Gazebo (if no state subscriber is active)
if (imu_topic_.empty()) {
pose = link->GetWorldPose();
angular_velocity = link->GetWorldAngularVel();
euler = pose.rot.GetAsEuler();
}
if (state_topic_.empty()) {
acceleration = (link->GetWorldLinearVel() - velocity) / dt;
velocity = link->GetWorldLinearVel();
}
// Rotate vectors to coordinate frames relevant for control
math::Quaternion heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2));
math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity);
math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration);
math::Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity);
// process robot operation information
if((m_takeoff)&&(robot_current_state == LANDED_MODEL))
{
m_timeAfterTakeOff = 0;
m_takeoff = false;
robot_current_state = TAKINGOFF_MODEL;
}
else if(robot_current_state == TAKINGOFF_MODEL)
{
// take off phase need more power
if (!sonar_topic_.empty())
{
if(robot_altitude > 0.5)
{
robot_current_state = FLYING_MODEL;
}
}
else
{
m_timeAfterTakeOff += dt;
if(m_timeAfterTakeOff > 0.5)
{
//ROS_INFO("%f",m_timeAfterTakeOff);
robot_current_state = FLYING_MODEL;
}
}
if(m_isFlying == false)
{
m_timeAfterTakeOff = 0;
robot_current_state = LANDING_MODEL;
}
}
else if((robot_current_state == FLYING_MODEL)||(robot_current_state == TO_FIX_POINT_MODEL))
{
if(m_isFlying == false)
{
m_timeAfterTakeOff = 0;
robot_current_state = LANDING_MODEL;
}
}
else if(robot_current_state == LANDING_MODEL)
{
if (!sonar_topic_.empty())
{
m_timeAfterTakeOff += dt;
if((robot_altitude < 0.2)||(m_timeAfterTakeOff > 5.0))
{
robot_current_state = LANDED_MODEL;
}
}
else
{
m_timeAfterTakeOff += dt;
if(m_timeAfterTakeOff > 1.0)
{
robot_current_state = LANDED_MODEL;
}
}
if(m_isFlying == true)
{
m_timeAfterTakeOff = 0;
m_takeoff = false;
robot_current_state = TAKINGOFF_MODEL;
}
}
if( ((robot_current_state != LANDED_MODEL)||m_isFlying) && m_drainBattery )
m_batteryPercentage -= dt / m_maxFlightTime * 100.;
ardrone_autonomy::Navdata navdata;
navdata.batteryPercent = m_batteryPercentage;
navdata.rotX = pose.rot.GetRoll() / M_PI * 180.;
navdata.rotY = pose.rot.GetPitch() / M_PI * 180.;
navdata.rotZ = pose.rot.GetYaw() / M_PI * 180.;
if (!sonar_topic_.empty())
navdata.altd = int(robot_altitude*1000);
else
navdata.altd = pose.pos.z * 1000.f;
navdata.vx = 1000*velocity_xy.x;
navdata.vy = 1000*velocity_xy.y;
navdata.vz = 1000*velocity_xy.z;
navdata.ax = acceleration_xy.x/10;
navdata.ay = acceleration_xy.y/10;
navdata.az = acceleration_xy.z/10 + 1;
navdata.tm = ros::Time::now().toSec()*1000000; // FIXME what is the real drone sending here?
navdata.header.stamp = ros::Time::now();
navdata.header.frame_id = "ardrone_base_link";
navdata.state = robot_current_state;
navdata.magX = 0;
navdata.magY = 0;
navdata.magZ = 0;
navdata.pressure = 0;
navdata.temp = 0;
navdata.wind_speed = 0.0;
navdata.wind_angle = 0.0;
navdata.wind_comp_angle = 0.0;
navdata.tags_count = 0;
// navdata.tags_type
// navdata.tags_xc
// navdata.tags_yc
// navdata.tags_width
// navdata.tags_height
// navdata.tags_orientation
// navdata.tags_distance
// filter for sensor information
// filter_rate = 0.1;
// navdata.rotX = navdata.rotX*filter_rate + (1-filter_rate)*last_navdata.rotX;
// navdata.rotY = navdata.rotY*filter_rate + (1-filter_rate)*last_navdata.rotY;
// navdata.rotZ = navdata.rotZ*filter_rate + (1-filter_rate)*last_navdata.rotZ;
// navdata.altd = navdata.altd*filter_rate + (1-filter_rate)*last_navdata.altd;
// navdata.vx = navdata.vx*filter_rate + (1-filter_rate)*last_navdata.vx;
// navdata.vy = navdata.vy*filter_rate + (1-filter_rate)*last_navdata.vy;
// navdata.vz = navdata.vz*filter_rate + (1-filter_rate)*last_navdata.vz;
// navdata.ax = navdata.ax*filter_rate + (1-filter_rate)*last_navdata.ax;
// navdata.ay = navdata.ay*filter_rate + (1-filter_rate)*last_navdata.ay;
// navdata.az = navdata.az*filter_rate + (1-filter_rate)*last_navdata.az;
// last_navdata = navdata;
m_navdataPub.publish( navdata );
// for tum_ardrone
m_navdataPub_tum.publish( navdata );
// save last time stamp
last_time = sim_time;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void BonebrakerController::Update()
{
// Get new commands/state
callback_queue_.callAvailable();
double dt;
if (controlTimer.update(dt) && dt > 0.0) {
// Get Pose/Orientation from Gazebo (if no state subscriber is active)
if (imu_topic_.empty()) {
pose = link->GetWorldPose();
angular_velocity = link->GetWorldAngularVel();
euler = pose.rot.GetAsEuler();
}
if (state_topic_.empty()) {
acceleration = (link->GetWorldLinearVel() - velocity) / dt;
velocity = link->GetWorldLinearVel();
}
// Get gravity
// math::Vector3 gravity_body = pose.rot.RotateVector(world->GetPhysicsEngine()->GetGravity());
// double gravity = gravity_body.GetLength();
//double load_factor = gravity * gravity / world->GetPhysicsEngine()->GetGravity().GetDotProd(gravity_body); // Get gravity
// Rotate vectors to coordinate frames relevant for control
math::Quaternion heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2));
math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity);
//math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration);
math::Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity);
//Init matlab inputs
//Commands
quadrotor_controller_U.PositionCommand[0] = control_ref_rw_.quad_control_references.position_ref.x;
quadrotor_controller_U.PositionCommand[1] = control_ref_rw_.quad_control_references.position_ref.y;
quadrotor_controller_U.PositionCommand[2] = control_ref_rw_.quad_control_references.position_ref.z;
quadrotor_controller_U.YawCommand = control_ref_rw_.quad_control_references.heading;
quadrotor_controller_U.VelocityCommand = control_ref_rw_.quad_control_references.velocity_ref;
if(arm_control_ref_.arm_control_references.position_ref.size()>6)
{
quadrotor_controller_U.ARMAnglesIN[0] = arm_control_ref_.arm_control_references.position_ref[0];
quadrotor_controller_U.ARMAnglesIN[1] = arm_control_ref_.arm_control_references.position_ref[1];
quadrotor_controller_U.ARMAnglesIN[2] = arm_control_ref_.arm_control_references.position_ref[2];
quadrotor_controller_U.ARMAnglesIN[3] = arm_control_ref_.arm_control_references.position_ref[3];
quadrotor_controller_U.ARMAnglesIN[4] = arm_control_ref_.arm_control_references.position_ref[4];
quadrotor_controller_U.ARMAnglesIN[5] = arm_control_ref_.arm_control_references.position_ref[5];
quadrotor_controller_U.ARMAnglesIN[6] = arm_control_ref_.arm_control_references.position_ref[6];
quadrotor_controller_U.ARMAnglesIN[7] = arm_control_ref_.arm_control_references.battery;
}
quadrotor_controller_U.Joystick[0] = joystick_.thrust;
quadrotor_controller_U.Joystick[1] = joystick_.yaw;
quadrotor_controller_U.Joystick[2] = joystick_.pitch;
quadrotor_controller_U.Joystick[3] = joystick_.roll;
//State
quadrotor_controller_U.AngularVelocity[0] = angular_velocity_body.x;
quadrotor_controller_U.AngularVelocity[1] = angular_velocity_body.y;
quadrotor_controller_U.AngularVelocity[2] = angular_velocity_body.z;
quadrotor_controller_U.LinealVelocity[0] = velocity_xy.x;
quadrotor_controller_U.LinealVelocity[1] = velocity_xy.y;
quadrotor_controller_U.LinealVelocity[2] = velocity_xy.z;
quadrotor_controller_U.Position[0] = link->GetWorldPose().pos.x;
quadrotor_controller_U.Position[1] = link->GetWorldPose().pos.y;
quadrotor_controller_U.Position[2] = link->GetWorldPose().pos.z;
//Euler 321 for control
quadrotor_controller_U.Attitude[0] = euler.x;//pose.rot.x;
quadrotor_controller_U.Attitude[1] = euler.y;//pose.rot.y;
quadrotor_controller_U.Attitude[2] = euler.z;//pose.rot.z;
for(int i=0;i<7;i++)
{
// std::cerr << "PositionJoint Despues: " << joint_positions_state_[i] << std::endl;
quadrotor_controller_U.ARMAnglesStateIN[i] = joint_positions_state_[i];
}
//update model
MatlabUpdate();
//get matlab outputs
force.x = quadrotor_controller_Y.Force[0];
force.y = quadrotor_controller_Y.Force[1];
force.z = quadrotor_controller_Y.Force[2];
torque.x = quadrotor_controller_Y.Torque[0];
torque.y = quadrotor_controller_Y.Torque[1];
torque.z = quadrotor_controller_Y.Torque[2];
//Get arm matlab outputs.
for(int i=0;i<7;i++)
{
if(quadrotor_controller_Y.ARMAnglesOUT[i]!=joint_positions_[i])
{
joint_positions_[i] = quadrotor_controller_Y.ARMAnglesOUT[i];
joint_position_to_send_.position = joint_positions_[i];
joint_position_to_send_.joint_name = joint_names_[i];
joint_position_publisher_.publish(joint_position_to_send_);
}
}
if(joint_positions_[6]!=joint_positions_[7])
{
joint_positions_[7] = joint_positions_[6];
joint_position_to_send_.position = joint_positions_[7];
joint_position_to_send_.joint_name = joint_names_[7];
joint_position_publisher_.publish(joint_position_to_send_);
}
if(quadrotor_controller_Y.ARMAnglesOUT[7]!=joint_positions_[8])
{
joint_positions_[8] = quadrotor_controller_Y.ARMAnglesOUT[7];
joint_position_to_send_.position = joint_positions_[8];
joint_position_to_send_.joint_name = joint_names_[8];
joint_position_publisher_.publish(joint_position_to_send_);
}
}
/*math::Pose actual_pose = link->GetWorldPose();
actual_pose.pos.x = 0.0;
actual_pose.pos.y = 0.0;
actual_pose.pos.z = 1.5;
*/
link->AddRelativeForce(force);
link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
// link->SetWorldPose(actual_pose);
}
