Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F120833491
nmpf_node.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Mon, Jul 7, 10:13
Size
9 KB
Mime Type
text/x-c
Expires
Wed, Jul 9, 10:13 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
27257235
Attached To
R1517 test_package
nmpf_node.cpp
View Options
#include "nmpf_node.hpp"
#include "ekf_node.h"
using namespace casadi;
void KiteNMPF_Node::filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg)
{
//boost::unique_lock<boost::mutex> scoped_lock(m_mutex, boost::try_to_lock);
kite_state = convertToDM(*msg);
if(!is_initialized())
initialize();
}
KiteNMPF_Node::KiteNMPF_Node(const ros::NodeHandle &_nh, const KiteProperties &kite_props,
const AlgorithmProperties &algo_props )
{
std::shared_ptr<KiteDynamics> kite = std::make_shared<KiteDynamics>(kite_props, algo_props);
/** @badcode : parametrize path outside NMPF node constructor*/
SX x = SX::sym("x");
double radius = 3.0;
double altitude = 0.00;
SX Path = SX::vertcat(SXVector{radius * cos(x), radius * sin(x), altitude});
/** rotate path */
SX q_rot = SX::vertcat({cos(M_PI / 24), 0, sin(M_PI / 24), 0});
SX q_rot_inv = kmath::quat_inverse(q_rot);
SX qP_tmp = kmath::quat_multiply(q_rot_inv, SX::vertcat({0, Path}));
SX qP_q = kmath::quat_multiply(qP_tmp, q_rot);
Path = qP_q(Slice(1,4), 0);
Function path = Function("path", {x}, {Path});
controller = std::make_shared<KiteNMPF>(kite, path);
nh = std::make_shared<ros::NodeHandle>(_nh);
/** set control constraints */
double angle_sat = kmath::deg2rad(8.0);
DM lbu = DM::vertcat({0.1, -angle_sat, -angle_sat, -1});
DM ubu = DM::vertcat({0.2, angle_sat, angle_sat, 1});
controller->setLBU(lbu);
controller->setUBU(ubu);
/** set variable constraints */
DM lbx = DM::vertcat({1.0, -0.5, -DM::inf(1), -2 * M_PI, -2 * M_PI, -2 * M_PI, -DM::inf(1), -DM::inf(1), -DM::inf(1),
-1, -1, -1, -1, -DM::inf(1), -2 * M_PI});
DM ubx = DM::vertcat({12, 5, DM::inf(1), 2 * M_PI, 2 * M_PI, 2 * M_PI, DM::inf(1), DM::inf(1), DM::inf(1),
1, 1, 1, 1, DM::inf(1), 2 * M_PI});
//controller->setLBX(lbx);
//controller->setUBX(ubx);
DM vel_ref = 0.05;
controller->setReferenceVelocity(vel_ref);
/** create NLP */
controller->createNLP();
/** create solver for delay compensation */
nh->param<double>("delay", transport_delay, 0.0);
Dict opts;
opts["tf"] = transport_delay;
opts["poly_order"] = 2;
opts["tol"] = 1e-4;
opts["method"] = IntType::CVODES;
Function system = kite->getNumericDynamics();
solver = std::make_shared<ODESolver>(system, opts);
/** initialize subscribers and publishers */
control_pub = nh->advertise<test_package::aircraft_controls>("kite_controls", 100);
traj_pub = nh->advertise<sensor_msgs::MultiDOFJointState>("/opt_traj", 10);
diagnostic_pub = nh->advertise<test_package::mpc_diagnostic>("/mpc_diagnostic", 10);
std::string state_topic = "/kite_state";
state_sub = nh->subscribe(state_topic, 100, &KiteNMPF_Node::filterCallback, this);
m_initialized = false;
comp_time_ms = 0.0;
}
DM KiteNMPF_Node::convertToDM(const sensor_msgs::MultiDOFJointState &_value)
{
DM value = DM::zeros(13);
value[0] = _value.twist.back().linear.x;
value[1] = _value.twist.back().linear.y;
value[2] = _value.twist.back().linear.z;
value[3] = _value.twist.back().angular.x;
value[4] = _value.twist.back().angular.y;
value[5] = _value.twist.back().angular.z;
value[6] = _value.transforms.back().translation.x;
value[7] = _value.transforms.back().translation.y;
value[8] = _value.transforms.back().translation.z;
value[9] = _value.transforms.back().rotation.w;
value[10] = _value.transforms.back().rotation.x;
value[11] = _value.transforms.back().rotation.y;
value[12] = _value.transforms.back().rotation.z;
return value;
}
void KiteNMPF_Node::publish()
{
/** pack estimation to ROS message */
DM opt_ctl = controller->getOptimalControl();
control = opt_ctl(Slice(0,3), opt_ctl.size2() - 1);
std::vector<double> controls = control.get_nonzeros();
test_package::aircraft_controls control_msg;
if(!controls.empty())
{
control_msg.header.stamp = ros::Time::now();
control_msg.thrust = controls[0];
control_msg.elevator = controls[1];
control_msg.rudder = controls[2];
/** publish current control */
control_pub.publish(control_msg);
}
}
void KiteNMPF_Node::publish_trajectory()
{
DM opt_trajectory = controller->getOptimalTrajetory();
int array_size = opt_trajectory.size2();
if(array_size == 0)
return;
DMVector split_traj = DM::horzsplit(opt_trajectory, 1);
sensor_msgs::MultiDOFJointState opt_msg;
opt_msg.twist.resize(array_size);
opt_msg.transforms.resize(array_size);
opt_msg.joint_names.resize(array_size);
opt_msg.wrench.resize(array_size);
opt_msg.header.frame_id = "optimal_trajectory";
int idx = 0;
for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end(); std::advance(it, 1))
{
std::vector<double> row = (*it).nonzeros();
opt_msg.twist[idx].linear.x = row[0];
opt_msg.twist[idx].linear.y = row[1];
opt_msg.twist[idx].linear.z = row[2];
opt_msg.twist[idx].angular.x = row[3];
opt_msg.twist[idx].angular.y = row[4];
opt_msg.twist[idx].angular.z = row[5];
opt_msg.transforms[idx].translation.x = row[6];
opt_msg.transforms[idx].translation.y = row[7];
opt_msg.transforms[idx].translation.z = row[8];
opt_msg.transforms[idx].rotation.w = row[9];
opt_msg.transforms[idx].rotation.x = row[10];
opt_msg.transforms[idx].rotation.y = row[11];
opt_msg.transforms[idx].rotation.z = row[12];
/** virtual state */
Function path = controller->getPathFunction();
DM point = path(DMVector{row[13]})[0];
std::vector<double> virt_point = point.nonzeros();
opt_msg.wrench[idx].force.x = virt_point[0];
opt_msg.wrench[idx].force.y = virt_point[1];
opt_msg.wrench[idx].force.z = virt_point[2];
idx++;
}
traj_pub.publish(opt_msg);
}
/** publish diagnostic info */
void KiteNMPF_Node::publish_mpc_diagnostic()
{
test_package::mpc_diagnostic diag_msg;
diag_msg.header.stamp = ros::Time::now();
diag_msg.pos_error = controller->getPathError();
diag_msg.comp_time_ms = comp_time_ms * 1000;
diag_msg.virt_state = controller->getVirtState();
diag_msg.vel_error = controller->getVelocityError();
/** dummy output */
diag_msg.cost = 0;
diagnostic_pub.publish(diag_msg);
}
void KiteNMPF_Node::compute_control()
{
/** augment state with pseudo state*/
DM augmented_state;
DM opt_traj = controller->getOptimalTrajetory();
if(!opt_traj.is_empty())
{
/** transport delay compensation */
DM predicted_state = solver->solve(kite_state, control, transport_delay);
augmented_state = DM::vertcat({predicted_state, opt_traj(Slice(13, opt_traj.size1()), opt_traj.size2() - 2)});
// Dict stats = controller->getStats();
// std::string solve_status = static_cast<std::string>(stats["return_status"]);
// if(solve_status.compare("Solve_Succeeded") != 0)
// {
// std::cout << "REINIT! \n";
// augmented_state(13) = controller->findClosestPointOnPath(kite_state(Slice(6,9)), augmented_state(13));
// }
}
else
{
DM closest_point = controller->findClosestPointOnPath(kite_state(Slice(6,9)));
std::cout << "Initialiaztion: " << closest_point << "\n";
augmented_state = DM::vertcat({kite_state, closest_point, 0});
}
/** @badcode : dirty hack : zero-speed workaround */
DM minimal_speed = DM(1.7);
if (augmented_state(0, 0).nonzeros()[0] < minimal_speed.nonzeros()[0])
augmented_state(0, 0) = minimal_speed;
/** compute control */
controller->computeControl(augmented_state);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "nmpf_node");
ros::NodeHandle n;
/** create a kite object */
std::string kite_params_file;
n.param<std::string>("kite_params", kite_params_file, "umx_radian.yaml");
std::cout << kite_params_file << "\n";
KiteProperties kite_props = kite_utils::LoadProperties(kite_params_file);
AlgorithmProperties algo_props;
algo_props.Integrator = RK4;
algo_props.sampling_time = 0.02;
int broadcast_trajectory;
n.param<int>("broadcast_trajectory", broadcast_trajectory, 1);
/** create a NMPF instance */
KiteNMPF_Node tracker(n, kite_props, algo_props);
ros::Rate loop_rate(15); /** 18 Hz */
while (ros::ok())
{
ros::spinOnce();
if(tracker.is_initialized())
{
double start = ros::Time::now().toSec();
tracker.compute_control();
double finish = ros::Time::now().toSec();
tracker.publish();
tracker.publish_mpc_diagnostic();
tracker.comp_time_ms = finish - start;
std::cout << "Control computational delay: " << finish - start << "\n";
if(broadcast_trajectory)
tracker.publish_trajectory();
}
else
{
loop_rate.sleep();
}
}
return 0;
}
Event Timeline
Log In to Comment