jackal_mpc_ros_trajectory_tracking.cpp

#include <math.h>
#include <algorithm>

#include <tf/tf.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PointStamped.h>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/PolygonStamped.h>

#include "mpc/jackal_mpc_acceleration.h"
#include "mpc/jackal_mpc_ros_trajectory_tracking.h"

#define JACKAL_WIDTH .3

JackalMPCROS::JackalMPCROS(ros::NodeHandle &nh)
{

	_estop = false;
	_is_init = false;
	_is_goal = false;
	_traj_reset = false;

	// _mpc_type = MPC_TYPE_CTE;

	// if (_mpc_type == MPC_TYPE_ACC){
	// 	ROS_INFO("MPC Model: Unicycle-Acceleration-CTE");
	// 	_mpc = new MPC_ACC();
	// } else if (_mpc_type == MPC_TYPE_CTE){
	// 	ROS_INFO("MPC Model: Unicycle-Velocity-CTE");
	// 	_mpc = new MPC();
	// }

	_curr_vel = 0;
	velMsg.linear.x = 0;
	velMsg.angular.z = 0;

	_dist_grid_ptr =
		std::make_shared<distmap::DistanceMap>(distmap::DistanceMap::Dimension(5, 5),
											   1,
											   distmap::DistanceMap::Origin());

	double freq;
	std::string mpc_type_str;

	nh.param<std::string>("jackal_mpc_track/mpc_type", mpc_type_str, "track_acc_ipopt");

	if (mpc_type_str == "track_acc_ipopt")
		_mpc_type = MPC_TYPE_ACC;
	else if (mpc_type_str == "track_acc_nlopt")
		_mpc_type = MPC_TYPE_NLOPT;
	else if (mpc_type_str == "track_vel_ipopt")
		_mpc_type = MPC_TYPE_CTE;
	else if (mpc_type_str == "goto_pos_nlopt")
		_mpc_type = MPC_TYPE_NLOPT_POS;
	else
	{
		ROS_WARN("Invalid MPC type. Defaulting to track_acc_ipopt");
		_mpc_type = MPC_TYPE_ACC;
	}

	// Localization params
	nh.param("jackal_mpc_track/use_vicon", _use_vicon, false);

	// MPC params
	nh.param("jackal_mpc_track/vel_pub_freq", _vel_pub_freq, 20.0);
	nh.param("jackal_mpc_track/controller_frequency", freq, 10.0);
	nh.param("jackal_mpc_track/mpc_steps", _mpc_steps, 10.0);

	// Cost function params
	nh.param("jackal_mpc_track/w_vel", _w_vel, 1.0);
	nh.param("jackal_mpc_track/w_angvel", _w_angvel, 1.0);
	nh.param("jackal_mpc_track/w_linvel", _w_linvel, 1.0);
	nh.param("jackal_mpc_track/w_angvel_d", _w_angvel_d, 1.0);
	nh.param("jackal_mpc_track/w_linvel_d", _w_linvel_d, .5);
	nh.param("jackal_mpc_track/w_etheta", _w_etheta, 1.0);
	nh.param("jackal_mpc_track/w_cte", _w_cte, 1.0);
	nh.param("jackal_mpc_track/w_pos", _w_pos, 1.0);

	// pos_mpc cost function params
	nh.param("jackal_mpc_track/pos_mpc_w_pos", _pos_mpc_w_pos, 1.0);
	nh.param("jackal_mpc_track/pos_mpc_w_angvel", _pos_mpc_w_angvel, 1.0);
	nh.param("jackal_mpc_track/pos_mpc_w_vel", _pos_mpc_w_vel, 1.0);
	nh.param("jackal_mpc_track/pos_mpc_w_angvel_d", _pos_mpc_w_angvel_d, 1.0);
	nh.param("jackal_mpc_track/pos_mpc_w_linvel_d", _pos_mpc_w_linvel_d, .5);
	nh.param("jackal_mpc_track/pos_mpc_max_linvel", _pos_mpc_max_linvel, 2.0);
	nh.param("jackal_mpc_track/pos_mpc_max_angvel", _pos_mpc_max_angvel, 3.0);

	// Constraint params
	nh.param("jackal_mpc_track/max_angvel", _max_angvel, 3.0);
	nh.param("jackal_mpc_track/max_linvel", _max_linvel, 2.0);
	nh.param("jackal_mpc_track/max_linacc", _max_linacc, 3.0);
	nh.param("jackal_mpc_track/max_anga", _max_anga, 2 * M_PI);
	nh.param("jackal_mpc_track/bound_value", _bound_value, 1.0e3);

	// Goal params
	nh.param("jackal_mpc_track/x_goal", _x_goal, 0.0);
	nh.param("jackal_mpc_track/y_goal", _y_goal, 0.0);
	nh.param("jackal_mpc_track/goal_tolerance", _tol, 0.3);

	// Teleop params
	nh.param("jackal_mpc_track/teleop", _teleop, false);
	nh.param<std::string>("jackal_mpc_track/frame_id", _frame_id, "odom");

	// cbf params
	nh.param("jackal_mpc_track/use_cbf", _use_cbf, false);
	nh.param("jackal_mpc_track/cbf_alpha", _cbf_alpha, .5);
	nh.param("jackal_mpc_track/cbf_colinear", _cbf_colinear, .1);
	nh.param("jackal_mpc_track/cbf_padding", _cbf_padding, .1);
	nh.param("jackal_mpc_track/dynamic_alpha", _use_dynamic_alpha, false);

	_dt = 1.0 / freq;

	_mpc_params["DT"] = _dt;
	_mpc_params["STEPS"] = _mpc_steps;
	_mpc_params["W_V"] = _w_linvel;
	_mpc_params["W_ANGVEL"] = _w_angvel;
	_mpc_params["W_DA"] = _w_linvel_d;
	_mpc_params["W_DANGVEL"] = _w_angvel_d;
	_mpc_params["W_ETHETA"] = _w_etheta;
	_mpc_params["W_POS"] = _w_pos;
	_mpc_params["W_CTE"] = _w_cte;
	_mpc_params["LINVEL"] = _max_linvel;
	_mpc_params["ANGVEL"] = _max_angvel;
	_mpc_params["BOUND"] = _bound_value;
	_mpc_params["X_GOAL"] = _x_goal;
	_mpc_params["Y_GOAL"] = _y_goal;

	_mpc_params["MAX_ANGA"] = _max_anga;
	_mpc_params["MAX_LINACC"] = _max_linacc;

	_mpc_params["USE_CBF"] = _use_cbf;
	_mpc_params["CBF_ALPHA"] = _cbf_alpha;
	_mpc_params["CBF_COLINEAR"] = _cbf_colinear;
	_mpc_params["CBF_PADDING"] = _cbf_padding;
	_mpc_params["CBF_DYNAMIC_ALPHA"] = _use_dynamic_alpha;
	_mpc_params["DEBUG"] = true;

	_pos_mpc_params["DT"] = _dt;
	_pos_mpc_params["STEPS"] = _mpc_steps;
	_pos_mpc_params["W_V"] = _pos_mpc_w_vel;
	_pos_mpc_params["W_ANGVEL"] = _pos_mpc_w_angvel;
	_pos_mpc_params["W_DA"] = _pos_mpc_w_linvel_d;
	_pos_mpc_params["W_DANGVEL"] = _pos_mpc_w_angvel_d;
	_pos_mpc_params["LINVEL"] = _pos_mpc_max_linvel;
	_pos_mpc_params["ANGVEL"] = _pos_mpc_max_angvel;
	_pos_mpc_params["BOUND"] = _bound_value;
	_pos_mpc_params["X_GOAL"] = _x_goal;
	_pos_mpc_params["Y_GOAL"] = _y_goal;

	_mpc_core = std::make_shared<JackalMPCCore>(_mpc_type);
	ROS_INFO("loading mpc params");
	_mpc_core->load_params(_mpc_params);
	ROS_INFO("done loading params!");

	if (_use_vicon)
		_odomSub = nh.subscribe("/vicon/jackal4/jackal4", 1, &JackalMPCROS::viconcb, this);
	else
		_odomSub = nh.subscribe("/odometry/filtered", 1, &JackalMPCROS::odomcb, this);

	_goalSub = nh.subscribe("recoveryGoal", 1, &JackalMPCROS::goalcb, this);
	_polySub = nh.subscribe("/recoveryPoly", 1, &JackalMPCROS::polycb, this);
	_alphaSub = nh.subscribe("/alpha", 1, &JackalMPCROS::alphacb, this);
	_obsSub = nh.subscribe("/curr_obstacle", 1, &JackalMPCROS::obstaclecb, this);
	_trajSub = nh.subscribe("/reference_trajectory", 1, &JackalMPCROS::trajectorycb, this);
	_distMapSub = nh.subscribe("/distance_map_node/distance_field_obstacles", 1, &JackalMPCROS::distmapcb, this);
	_trajNoResetSub = nh.subscribe("/reference_trajectory_no_reset", 1, &JackalMPCROS::trajectoryNoResetcb, this);

	_timer = nh.createTimer(ros::Duration(_dt), &JackalMPCROS::controlLoop, this);
	// _velPubTimer = nh.createTimer(ros::Duration(1./_vel_pub_freq), &JackalMPCROS::publishVel, this);

	_dist_pub = nh.advertise<std_msgs::Float64>("/mpc_dist", 0);
	_pathPub = nh.advertise<nav_msgs::Path>("/spline_path", 10);
	_velPub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
	_h_value_pub = nh.advertise<std_msgs::Float64>("/h_value", 0);
	_trajPub = nh.advertise<nav_msgs::Path>("/mpc_prediction", 10);
	_alpha_pub = nh.advertise<std_msgs::Float64>("/alpha_value", 0);
	_actualPathPub = nh.advertise<nav_msgs::Path>("/actual_path", 0);
	_pointPub = nh.advertise<geometry_msgs::PointStamped>("traj_point", 0);
	_goalReachedPub = nh.advertise<std_msgs::Bool>("/mpc_goal_reached", 10);
	_odomPub = nh.advertise<visualization_msgs::Marker>("robot_position", 10);
	_polyPub = nh.advertise<geometry_msgs::PolygonStamped>("/convex_free", 0);
	_polyPub2 = nh.advertise<geometry_msgs::PolygonStamped>("/convex_free2", 0);
	_solveTimePub = nh.advertise<std_msgs::Float64>("/mpc_solve_time", 0);
	_horizonPub = nh.advertise<trajectory_msgs::JointTrajectory>("/mpc_horizon", 0);
	_refPub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/current_reference", 10);

	_eStop_srv = nh.advertiseService("/eStop", &JackalMPCROS::eStopcb, this);
	_mode_srv = nh.advertiseService("/switch_mode", &JackalMPCROS::mode_switchcb, this);

	timer_thread = std::thread(&JackalMPCROS::publishVel, this);
}

JackalMPCROS::~JackalMPCROS()
{
	if (timer_thread.joinable())
		timer_thread.join();

	// delete _mpc;
}

bool JackalMPCROS::eStopcb(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
	ROS_WARN("E-STOP RECEIVED");

	// geometry_msgs::Twist velMsg;
	velMsg.linear.x = 0;
	velMsg.angular.z = 0;

	trajectory.points.clear();

	_estop ^= true;

	return true;
}

bool JackalMPCROS::mode_switchcb(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
	ROS_WARN("SWITCHING MODE");

	_mpc_type = (_mpc_type == MPC_TYPE_CTE) ? MPC_TYPE_ACC : MPC_TYPE_CTE;

	if (_mpc_type == MPC_TYPE_CTE)
		ROS_WARN("SWITCHED TO VEL-CTE MODE");
	else
		ROS_WARN("SWITCHED TO ACC-CTE MODE");

	// geometry_msgs::Twist velMsg;
	velMsg.linear.x = 0;
	velMsg.angular.z = 0;

	_is_at_goal = false;
	_is_goal = false;

	return true;
}

void JackalMPCROS::alphacb(const std_msgs::Float64::ConstPtr &msg)
{
	_mpc_params["CBF_ALPHA"] = msg->data;
	_mpc_core->load_params(_mpc_params);
}

void JackalMPCROS::distmapcb(const distance_map_msgs::DistanceMap::ConstPtr &msg)
{
	if (msg == nullptr)
	{
		ROS_WARN("Distance map is null");
		return;
	}

	*_dist_grid_ptr = utils::distmap_from_msg(*msg);
	_mpc_core->set_dist_map(_dist_grid_ptr);
}

void JackalMPCROS::obstaclecb(const geometry_msgs::Point::ConstPtr &msg)
{
	_obstacle = Eigen::Vector3d(msg->x, msg->y, msg->z);
	_mpc_core->set_obstacle(_obstacle);
}

void JackalMPCROS::polycb(const geometry_msgs::PoseArray::ConstPtr &msg)
{

	Eigen::MatrixX4d currPoly;
	for (int i = 0; i < msg->poses.size(); ++i)
	{
		geometry_msgs::Pose p = msg->poses[i];
		if (p.orientation.x == 0 && p.orientation.y == 0 && p.orientation.z == 0 && p.orientation.w == 0)
		{
			if (currPoly.rows() > 0)
			{
				_poly = currPoly;
				break;
			}
		}
		else
		{
			Eigen::Vector4d plane(p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
			currPoly.conservativeResize(currPoly.rows() + 1, 4);
			currPoly.row(currPoly.rows() - 1) = plane;
		}
	}
}

void JackalMPCROS::publishVel()
{
	constexpr double pub_vel_loop_rate_hz = 50;
	const std::chrono::milliseconds pub_loop_period(static_cast<int>(1000.0 / pub_vel_loop_rate_hz));

	while (ros::ok())
	{
		if (trajectory.points.size() > 0)
		{
			_refPub.publish(current_reference);
		}

		// ROS_INFO("PUBLISHING {vel = %.2f, ang_z = %.2f}", velMsg.linear.x, velMsg.angular.z);
		_velPub.publish(velMsg);
		std::this_thread::sleep_for(pub_loop_period);
	}
}

void JackalMPCROS::goalcb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	_x_goal = msg->pose.position.x;
	_y_goal = msg->pose.position.y;

	_is_goal = true;
	_is_at_goal = false;

	_mpc_core->set_goal(Eigen::Vector2d(_x_goal, _y_goal));

	ROS_WARN("GOAL RECEIVED (%.2f, %.2f)", _x_goal, _y_goal);
}

void JackalMPCROS::viconcb(const geometry_msgs::TransformStamped::ConstPtr &msg)
{

	tf::Quaternion q(
		msg->transform.rotation.x,
		msg->transform.rotation.y,
		msg->transform.rotation.z,
		msg->transform.rotation.w);

	tf::Matrix3x3 m(q);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);

	_odom = Eigen::VectorXd(3);

	_odom(XI) = msg->transform.translation.x;
	_odom(YI) = msg->transform.translation.y;
	_odom(THETAI) = yaw;

	_is_init = true;

	visualization_msgs::Marker marker;
	marker.header.frame_id = _frame_id;
	marker.header.stamp = ros::Time();
	marker.ns = "position";
	marker.id = 0;
	marker.type = visualization_msgs::Marker::ARROW;
	marker.action = visualization_msgs::Marker::ADD;
	marker.pose.position.x = _odom(XI);
	marker.pose.position.y = _odom(YI);
	marker.pose.position.z = 0;
	marker.pose.orientation.x = msg->transform.rotation.x;
	marker.pose.orientation.y = msg->transform.rotation.y;
	marker.pose.orientation.z = msg->transform.rotation.z;
	marker.pose.orientation.w = msg->transform.rotation.w;
	marker.scale.x = 0.4;
	marker.scale.y = 0.2;
	marker.scale.z = 0.4;
	marker.color.r = 1.0;
	marker.color.g = 1.0;
	marker.color.b = 1.0;
	marker.color.a = 1.0;
	_odomPub.publish(marker);
}

void JackalMPCROS::odomcb(const nav_msgs::Odometry::ConstPtr &msg)
{

	tf::Quaternion q(
		msg->pose.pose.orientation.x,
		msg->pose.pose.orientation.y,
		msg->pose.pose.orientation.z,
		msg->pose.pose.orientation.w);

	tf::Matrix3x3 m(q);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);

	_odom = Eigen::VectorXd(3);

	_odom(XI) = msg->pose.pose.position.x;
	_odom(YI) = msg->pose.pose.position.y;
	_odom(THETAI) = yaw;

	_mpc_core->set_odom(_odom);

	if (!_is_init)
	{
		_is_init = true;
		ROS_INFO("tracker initialized");
	}
}

// TODO: Support appending trajectories
void JackalMPCROS::trajectorycb(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
{
	trajectory = *msg;
	_traj_reset = true;

	ROS_INFO("MPC received trajectory!");
}

void JackalMPCROS::trajectoryNoResetcb(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
{
	trajectory = *msg;

	ROS_INFO("MPC received trajectory (no time reset)!");
}

void JackalMPCROS::cte_ctrl_loop()
{
	static ros::Time start;

	if (!_is_init || _estop)
		return;

	if (_traj_reset)
	{
		start = ros::Time::now();
		_traj_reset = false;
	}

	if (trajectory.points.size() != 0)
	{
		ros::Time now = ros::Time::now();
		double t = (now - start).toSec();
		current_reference = evalTraj(t);

		double traj_duration = trajectory.points.back().time_from_start.toSec();
		Eigen::Vector2d goal(trajectory.points.back().positions[0],
							 trajectory.points.back().positions[1]);

		// If trajectory is done, stop
		if (t > traj_duration)
		{
			ROS_INFO("trajectory done!");
			velMsg.linear.x = 0;
			velMsg.angular.z = 0;
			trajectory.points.clear();
			return;
		}

		// Send next _mpc_steps reference points to solver
		// For feasible tracking, trajectory MUST be C2-continuous due to
		// trajectory angular velocity calculations in MPC
		Eigen::MatrixXd wpts(6, (int)_mpc_steps);
		for (int i = 0; i < _mpc_steps; i++)
		{
			trajectory_msgs::JointTrajectoryPoint pt;

			if (i * _dt + t > traj_duration)
				pt = trajectory.points.back();
			else
				pt = evalTraj(i * _dt + t);

			// posX, velX, accX
			wpts(0, i) = pt.positions[0];
			wpts(1, i) = i * _dt + t >= traj_duration ? 0 : pt.velocities[0];
			wpts(2, i) = i * _dt + t >= traj_duration ? 0 : pt.accelerations[0];

			// posY, velY, accY
			wpts(3, i) = pt.positions[1];
			wpts(4, i) = i * _dt + t >= traj_duration ? 0 : pt.velocities[1];
			wpts(5, i) = i * _dt + t >= traj_duration ? 0 : pt.accelerations[1];
		}

		_mpc_core->set_reference(wpts);

		ros::Time start = ros::Time::now();
		mpc_results = _mpc_core->solve();
		double solve_time = (ros::Time::now() - start).toSec();

		std::vector<double> h_vals = _mpc_core->evaluateHFunction();

		// publish solve time and associated odom
		std_msgs::Float64 solveTimeMsg;
		solveTimeMsg.data = solve_time;
		_solveTimePub.publish(solveTimeMsg);

		ROS_INFO("{vel = %.2f, ang_z = %.2f}", mpc_results[0], mpc_results[1]);
		velMsg.angular.z = mpc_results[1];
		velMsg.linear.x = mpc_results[0];

		publishReference();
		publishMPCTrajectory();
		publishActualPath();

		if (_mpc_type == MPC_TYPE_NLOPT && _use_cbf)
		{
			std_msgs::Float64 h_msg;
			h_msg.data = h_vals[0];
			_h_value_pub.publish(h_msg);

			std_msgs::Float64 alpha_msg;
			alpha_msg.data = h_vals[1];
			_alpha_pub.publish(alpha_msg);

			std_msgs::Float64 dist_msg;
			dist_msg.data = _dist_grid_ptr->atPositionSafe(_odom(0), _odom(1), true);
			_dist_pub.publish(dist_msg);
		}

		// publish reference point
		geometry_msgs::PointStamped pointMsg;
		pointMsg.header.stamp = ros::Time::now();
		pointMsg.header.frame_id = _frame_id;
		pointMsg.point.x = wpts(0, 0);
		pointMsg.point.y = wpts(3, 0);
		_pointPub.publish(pointMsg);
	}
}

void JackalMPCROS::controlLoop(const ros::TimerEvent &)
{
	cte_ctrl_loop();
}

trajectory_msgs::JointTrajectoryPoint JackalMPCROS::evalTraj(double t)
{

	for (trajectory_msgs::JointTrajectoryPoint pt : trajectory.points)
	{
		if (t < pt.time_from_start.toSec())
			return pt;
	}

	return trajectory.points.back();
}

void JackalMPCROS::publishReference()
{

	nav_msgs::Path msg;
	msg.header.stamp = ros::Time::now();
	msg.header.frame_id = _frame_id;

	bool published = false;
	for (trajectory_msgs::JointTrajectoryPoint pt : trajectory.points)
	{
		if (!published)
		{
			published = true;
			_refPub.publish(pt);
		}
		geometry_msgs::PoseStamped pose;
		pose.header.stamp = ros::Time::now();
		pose.header.frame_id = _frame_id;

		pose.pose.position.x = pt.positions[0];
		pose.pose.position.y = pt.positions[1];
		pose.pose.position.z = 0;
		pose.pose.orientation.x = 0;
		pose.pose.orientation.y = 0;
		pose.pose.orientation.z = 0;
		pose.pose.orientation.w = 1;
		msg.poses.push_back(pose);
	}

	_pathPub.publish(msg);
}

void JackalMPCROS::publishActualPath()
{

	// for visualizing actual path
	poses.push_back(_odom);

	nav_msgs::Path msg;
	msg.header.stamp = ros::Time::now();
	msg.header.frame_id = _frame_id;
	for (Eigen::Vector3d p : poses)
	{
		geometry_msgs::PoseStamped pose;
		pose.pose.position.x = p[0];
		pose.pose.position.y = p[1];
		pose.pose.position.z = 0;

		pose.pose.orientation.x = 0;
		pose.pose.orientation.y = 0;
		pose.pose.orientation.z = 0;
		pose.pose.orientation.w = 1;
		msg.poses.push_back(pose);
	}

	_actualPathPub.publish(msg);
}

void JackalMPCROS::publishMPCTrajectory()
{

	geometry_msgs::PoseStamped goal;
	goal.header.stamp = ros::Time::now();
	goal.header.frame_id = _frame_id;
	goal.pose.position.x = _x_goal;
	goal.pose.position.y = _y_goal;
	goal.pose.orientation.w = 1;

	nav_msgs::Path pathMsg;
	pathMsg.header.frame_id = _frame_id;
	pathMsg.header.stamp = ros::Time::now();

	std::vector<Eigen::VectorXd> horizon = _mpc_core->getHorizon();
	for (int i = 0; i < horizon.size(); ++i)
	{
		Eigen::VectorXd state = horizon[i];
		geometry_msgs::PoseStamped tmp;
		tmp.header = pathMsg.header;
		if (state.size() == 6)
		{
			tmp.pose.position.x = state(1);
			tmp.pose.position.y = state(2);
		}
		else
		{
			tmp.pose.position.x = state(0);
			tmp.pose.position.y = state(1);
		}
		tmp.pose.position.z = .1;
		tmp.pose.orientation.w = 1;
		pathMsg.poses.push_back(tmp);
	}

	_trajPub.publish(pathMsg);

	if (horizon.size() > 1 && horizon[0].size() == 6)
	{
		// convert to JointTrajectory
		trajectory_msgs::JointTrajectory traj;
		traj.header.stamp = ros::Time::now();
		traj.header.frame_id = _frame_id;

		double dt = horizon[1](0) - horizon[0](0);

		for (int i = 0; i < horizon.size(); ++i)
		{
			Eigen::VectorXd state = horizon[i];

			double t = state(0);
			double x = state(1);
			double y = state(2);
			double theta = state(3);
			double linvel = state(4);
			double linacc = state(5);

			// compute velocity and acceleration in x and y directions
			double vel_x = linvel * cos(theta);
			double vel_y = linvel * sin(theta);

			double acc_x = linacc * cos(theta);
			double acc_y = linacc * sin(theta);

			// compute jerk in x and y directions from acceleration
			double jerk_x = 0;
			double jerk_y = 0;
			if (i < horizon.size() - 1)
			{
				double next_linacc = horizon[i + 1](5);
				double next_linacc_x = next_linacc * cos(horizon[i + 1](3));
				double next_linacc_y = next_linacc * sin(horizon[i + 1](3));
				jerk_x = (next_linacc_x - acc_x) / dt;
				jerk_y = (next_linacc_y - acc_y) / dt;

				// ROS_INFO("jerk_x = %.2f, jerk_y = %.2f", jerk_x, jerk_y);
			}
			else
			{
				jerk_x = 0;
				jerk_y = 0;

				// ROS_INFO("(in else cond) jerk_x = 0, jerk_y = 0");
			}
			// if (i > 0)
			// {
			// 	double prev_linacc = horizon[i - 1](5);
			// 	double prev_linacc_x = prev_linacc * cos(horizon[i - 1](3));
			// 	double prev_linacc_y = prev_linacc * sin(horizon[i - 1](3));
			// 	// jerk_x = (acc_x - prev_linacc_x) / dt;
			// 	// jerk_y = (acc_y - prev_linacc_y) / dt;
			// 	jerk_x = 0;
			// 	jerk_y = 0;
			// }
			// else
			// {
			// 	jerk_x = 0;
			// 	jerk_y = 0;
			// }

			trajectory_msgs::JointTrajectoryPoint pt;
			pt.time_from_start = ros::Duration(t);
			pt.positions = {x, y, 0};
			pt.velocities = {vel_x, vel_y, 0};
			pt.accelerations = {acc_x, acc_y, 0};
			pt.effort = {jerk_x, jerk_y, 0};

			traj.points.push_back(pt);
		}

		_horizonPub.publish(traj);
	}
}