//////////////////////////////////////////////////////////////////////////////// // 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); }