void
UAVSimulation::update3DOF(const double& timestep)
{
if (timestep <= 0)
return;
//! Wind effects
m_velocity(2) = m_wind(2);
calcUAV2AirData();
//==========================================================================
//! Aircraft Dynamics
//==========================================================================
integratePosition(timestep);
/*
//for debug
double vt_velocity1[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
//! Command effect
//! - Airspeed command
m_airspeed = m_airspeed_cmd;
//! - Roll command
m_position(3) = m_bank_cmd;
//! Turn rate
m_velocity(5) = m_g * std::tan(m_position(3))/m_airspeed;
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
updateVelocity();
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
if (std::abs(vt_velocity1[0] - vt_velocity2[0]) > 0.5*m_airspeed*timestep/m_speed_time_cst ||
std::abs(vt_velocity1[1] - vt_velocity2[1]) > 0.5*m_airspeed*timestep/m_speed_time_cst)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Velocity difference: " << vt_velocity1[0]-vt_velocity2[0] << ", " << vt_velocity1[1]-vt_velocity2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(vt_position1[5]) << "º" << std::endl;
}
*/
/*
//for debug
double vt_velocity3[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
std::cout << "Prev velocity: " << vt_velocity1[0] << ", " << vt_velocity1[1] << std::endl;
std::cout << "Int velocity: " << vt_velocity2[0] << ", " << vt_velocity2[1] << std::endl;
std::cout << "New velocity: " << vt_velocity3[0] << ", " << vt_velocity3[1] << std::endl;
*/
}
void MovingFrame::updateCoordinates(const Timestamp& timestamp)
{
// don't compute the velocity during substepping, it is assumed constant.
if (!timestamp.substep) {
bool cacheAvail = true;
if (!timestamp.reiterate) {
cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
if (m_function)
(*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
}
// only compute velocity if we have a previous pose
if (cacheAvail && timestamp.interpolate) {
unsigned int iXu;
m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
for (iXu=0; iXu<6; iXu++)
m_xudot(iXu) = m_velocity(iXu);
} else if (!timestamp.reiterate) {
// new position is forced, no velocity as we cannot interpolate
m_internalPose = m_nextPose;
m_velocity = Twist::Zero();
m_xudot = e_zero_vector(6);
// recompute the jacobian
updateJacobian();
}
}
}
void
UAVSimulation::setVelocity(const DUNE::Math::Matrix& vel)
{
int i_vel_size = vel.rows();
if (i_vel_size < 2 && i_vel_size > 6)
throw Error("Invalid velocity vector dimension. Vector size must be between 2 and 6.");
//! Vehicle velocity vector, relative to the ground, in the ground reference frame
m_velocity.set(0, i_vel_size-1, 0, 0, vel);
// Reset the vertical velocity for the simulations that do not update it
if (m_sim_type.compare("3DOF") == 0 || m_sim_type.compare("4DOF_bank") == 0)
m_velocity(2) = 0;
// Reset the pitch angular rate for the simulations that do not update it
if (m_sim_type.compare("6DOF_dyn") != 0)
m_velocity(4) = 0;
calcUAV2AirData();
}
SNS_IK::SNS_IK(const std::string& base_link, const std::string& tip_link,
const std::string& URDF_param, double loopPeriod, double eps,
sns_ik::VelocitySolveType type) :
m_initialized(false),
m_eps(eps),
m_loopPeriod(loopPeriod),
m_nullspaceGain(1.0),
m_solvetype(type)
{
ros::NodeHandle node_handle("~");
urdf::Model robot_model;
std::string xml_string;
std::string urdf_xml, full_urdf_xml;
node_handle.param("urdf_param",urdf_xml,URDF_param);
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG_NAMED("sns_ik","Reading xml file from parameter server");
if (!node_handle.getParam(full_urdf_xml, xml_string)) {
ROS_FATAL_NAMED("sns_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
return;
}
node_handle.param(full_urdf_xml, xml_string, std::string());
robot_model.initString(xml_string);
ROS_DEBUG_STREAM_NAMED("sns_ik","Reading joints and links from URDF");
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
ROS_FATAL("Failed to extract kdl tree from xml robot description.");
return;
}
if(!tree.getChain(base_link, tip_link, m_chain)) {
ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str());
}
std::vector<KDL::Segment> chain_segments = m_chain.segments;
boost::shared_ptr<const urdf::Joint> joint;
m_lower_bounds.resize(m_chain.getNrOfJoints());
m_upper_bounds.resize(m_chain.getNrOfJoints());
m_velocity.resize(m_chain.getNrOfJoints());
m_acceleration.resize(m_chain.getNrOfJoints());
m_jointNames.resize(m_chain.getNrOfJoints());
unsigned int joint_num=0;
for(std::size_t i = 0; i < chain_segments.size(); ++i) {
joint = robot_model.getJoint(chain_segments[i].getJoint().getName());
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
double lower=0; //TODO Better default values? Error if these arent found?
double upper=0;
double velocity=0;
double acceleration=0;
if ( joint->type == urdf::Joint::CONTINUOUS ) {
lower=std::numeric_limits<float>::lowest();
upper=std::numeric_limits<float>::max();
} else {
if(joint->safety) {
lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
} else {
lower = joint->limits->lower;
upper = joint->limits->upper;
}
velocity = std::fabs(joint->limits->velocity);
}
// Checking the Param server for limit modifications
// and acceleration limits
std::string prefix = urdf_xml + "_planning/joint_limits/" + joint->name + "/";
double ul;
if(node_handle.getParam(prefix + "max_position", ul)){
upper = std::min(upper, ul);
}
double ll;
if(node_handle.getParam(prefix + "min_position", ll)){
lower = std::max(lower, ll);
}
double vel;
if(node_handle.getParam(prefix + "max_velocity", vel)){
if (velocity > 0)
velocity = std::min(velocity, std::fabs(vel));
else
velocity = std::fabs(vel);
}
node_handle.getParam(prefix + "max_acceleration", acceleration);
m_lower_bounds(joint_num)=lower;
m_upper_bounds(joint_num)=upper;
m_velocity(joint_num) = velocity;
m_acceleration(joint_num) = std::fabs(acceleration);
m_jointNames[joint_num] = joint->name;
ROS_INFO("sns_ik: Using joint %s lb: %.3f, ub: %.3f, v: %.3f, a: %.3f", joint->name.c_str(),
m_lower_bounds(joint_num), m_upper_bounds(joint_num), m_velocity(joint_num), m_acceleration(joint_num));
joint_num++;
}
}
initialize();
}
void
UAVSimulation::update5DOF(const double& timestep)
{
//! Check if model has the required parameters
//! - Model control time constants
if (!m_bank_time_cst_f && !m_speed_time_cst_f && !m_alt_time_cst_f)
{
throw Error("No model parameters defined! The state was not updated.");
//inf("No model parameters defined! The state was not updated.");
return;
}
else if (!m_alt_time_cst_f)
{
throw Error("Model parameter missing (Altitude time constant)! The state was not updated.");
//inf("No model parameters defined! The state was not updated.");
return;
}
if (timestep <= 0)
return;
//! Wind effects
calcUAV2AirData();
//==========================================================================
//! Aircraft Dynamics
//==========================================================================
integratePosition(timestep);
/*
//for debug
double vt_velocity1[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
//! Turn rate
m_velocity(5) = m_g * std::tan(m_position(3))/m_airspeed;
//! Command effect
//! - Horizontal acceleration command
double d_lon_accel = (m_airspeed_cmd - m_airspeed)/m_speed_time_cst;
if (m_lon_accel_lim_f)
d_lon_accel = DUNE::Math::trimValue(d_lon_accel, -m_lon_accel_lim, m_lon_accel_lim);
m_airspeed += d_lon_accel*timestep;
//! - Roll rate command
m_velocity(3) = (m_bank_cmd - m_position(3))/m_bank_time_cst;
if (m_bank_rate_lim_f)
m_velocity(3) = DUNE::Math::trimValue(m_velocity(3), -m_bank_rate_lim, m_bank_rate_lim);
//! - Vertical rate command
m_velocity(2) = (-m_altitude_cmd - m_position(2))/m_alt_time_cst;
if (m_vert_slope_lim_f)
{
double d_vert_rate_lim = m_vert_slope_lim*m_airspeed;
m_velocity(2) = DUNE::Math::trimValue(m_velocity(2), -d_vert_rate_lim, d_vert_rate_lim);
}
else
//! The vertical speed should not exceed the airspeed, even if there is no specified vertical slope limit
m_velocity(2) = DUNE::Math::trimValue(m_velocity(2), -m_airspeed, m_airspeed);
//! - Computing flight path angle
m_sin_pitch = -m_velocity(2)/m_airspeed;
m_cos_pitch = std::sqrt(1 - m_sin_pitch*m_sin_pitch);
m_position(4) = std::asin(m_sin_pitch);
updateVelocity();
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
if (std::abs(vt_velocity1[0] - vt_velocity2[0]) > 0.5*m_airspeed*timestep/m_speed_time_cst ||
std::abs(vt_velocity1[1] - vt_velocity2[1]) > 0.5*m_airspeed*timestep/m_speed_time_cst)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Velocity difference: " << vt_velocity1[0]-vt_velocity2[0] << ", " << vt_velocity1[1]-vt_velocity2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(m_position(5)) << "º" << std::endl;
}
*/
}
void
UAVSimulation::update4DOF_Bank(const double& timestep)
{
//! Check if model has the required parameters
//! - Model control time constants
if (!m_bank_time_cst_f)// && !m_speed_time_cst_f)
{
throw Error("No model parameters defined! The state was not updated.");
//inf("No model parameters defined! The state was not updated.");
return;
}
if (timestep <= 0)
return;
calcUAV2AirData();
//==========================================================================
//! Aircraft Dynamics
//==========================================================================
integratePosition(timestep);
/*
//for debug
double vt_velocity1[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
//! Turn rate
m_velocity(5) = m_g * std::tan(m_position(3))/m_airspeed;
//! Command effect
//! - Horizontal acceleration command
double d_lon_accel = (m_airspeed_cmd - m_airspeed)/m_speed_time_cst;
if (m_lon_accel_lim_f)
d_lon_accel = DUNE::Math::trimValue(d_lon_accel, -m_lon_accel_lim, m_lon_accel_lim);
m_airspeed += d_lon_accel*timestep;
//! - Roll rate command
m_velocity(3) = (m_bank_cmd - m_position(3))/m_bank_time_cst;
if (m_bank_rate_lim_f)
m_velocity(3) = DUNE::Math::trimValue(m_velocity(3), -m_bank_rate_lim, m_bank_rate_lim);
//! Wind effects
m_velocity(2) = m_wind(2);
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
*/
updateVelocity();
/*
//for debug
double vt_velocity2[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
if (std::abs(vt_velocity1[0] - vt_velocity2[0]) > 0.5*m_airspeed*timestep/m_speed_time_cst ||
std::abs(vt_velocity1[1] - vt_velocity2[1]) > 0.5*m_airspeed*timestep/m_speed_time_cst)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Velocity difference: " << vt_velocity1[0]-vt_velocity2[0] << ", " << vt_velocity1[1]-vt_velocity2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(vt_position1[5]) << "º" << std::endl;
}
*/
/*
//for debug
double vt_velocity3[6] = {m_velocity(0), m_velocity(1), m_velocity(2), m_velocity(3), m_velocity(4), m_velocity(5)};
std::cout << "Prev velocity: " << vt_velocity1[0] << ", " << vt_velocity1[1] << std::endl;
std::cout << "Int velocity: " << vt_velocity2[0] << ", " << vt_velocity2[1] << std::endl;
std::cout << "New velocity: " << vt_velocity3[0] << ", " << vt_velocity3[1] << std::endl;
*/
}
void
UAVSimulation::integratePosition(const double& timestep)
{
/*
//for debug
double vt_position1[6] = {m_position(0), m_position(1), m_position(2), m_position(3), m_position(4), m_position(5)};
*/
double d_initial_yaw = m_position(5);
//! Vertical position and Euler angles state update
m_position.set(2, 5, 0, 0, m_position.get(2, 5, 0, 0) + m_velocity.get(2, 5, 0, 0)*timestep);
m_position(5) = DUNE::Math::Angles::normalizeRadian(m_position(5));
// Optimization variables
m_cos_yaw = std::cos(m_position(5));
m_sin_yaw = std::sin(m_position(5));
if (m_sim_type == "5DOF" || m_sim_type == "4DOF_alt")
{
m_cos_pitch = std::cos(m_position(4));
m_sin_pitch = std::sin(m_position(4));
}
else if (m_sim_type == "6DOF_stab")
{
m_cos_roll = std::cos(m_position(3));
m_sin_roll = std::sin(m_position(3));
}
//! Horizontal position state update
if (std::abs(m_position(3)) < 0.1)
{
m_position.set(0, 1, 0, 0, m_position.get(0, 1, 0, 0) + m_velocity.get(0, 1, 0, 0)*timestep);
}
else
{
double d_turn_radius = m_airspeed/m_velocity(5);
m_position(0) += d_turn_radius*(m_sin_yaw - std::sin(d_initial_yaw));
m_position(1) += d_turn_radius*(std::cos(d_initial_yaw) - m_cos_yaw);
}
/*
//for debug
double vt_position2[6] = {m_position(0), m_position(1), m_position(2), m_position(3), m_position(4), m_position(5)};
if (std::abs(vt_position1[0] - vt_position2[0]) > 2*m_airspeed*timestep || std::abs(vt_position1[1] - vt_position2[1]) > 2*m_airspeed*timestep)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Position difference: " << vt_position1[0]-vt_position2[0] << ", " << vt_position1[1]-vt_position2[1] << std::endl;
}
if (std::abs(vt_position1[0] - vt_position2[0]) > 100 || std::abs(vt_position1[1] - vt_position2[1]) > 100)
{
std::cout << "Time step: " << timestep << std::endl;
std::cout << "Position difference: " << vt_position1[0]-vt_position2[0] << ", " << vt_position1[1]-vt_position2[1] << std::endl;
std::cout << "Heading: " << DUNE::Math::Angles::degrees(vt_position1[5]) << "º" << std::endl;
}
*/
/*
//for debug
double vt_position2[6] = {m_position(0), m_position(1), m_position(2), m_position(3), m_position(4), m_position(5)};
std::cout << "Prev position: " << vt_position1[0] << ", " << vt_position1[1] << std::endl;
std::cout << "New position: " << vt_position2[0] << ", " << vt_position2[1] << std::endl;
*/
}
