Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Generate lissajous trajectories for system excitation #193

Closed
wants to merge 1 commit into from
Closed

Generate lissajous trajectories for system excitation #193

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 10 additions & 6 deletions geometric_controller/launch/sitl_trajectory_track_circle.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,16 +17,20 @@
<param name="ctrl_mode" value="$(arg command_input)" />
<param name="enable_sim" value="$(arg gazebo_simulation)" />
<param name="enable_gazebo_state" value="true"/>
<param name="max_acc" value="10.0" />
<param name="Kp_x" value="8.0" />
<param name="Kp_y" value="8.0" />
<param name="attctrl_constant" value="0.09" />
<param name="max_acc" value="12.0" />
<param name="Kp_x" value="5.0" />
<param name="Kp_y" value="5.0" />
<param name="Kp_z" value="10.0" />
<param name="Kv_x" value="3.0" />
<param name="Kv_y" value="3.0" />
<param name="Kv_z" value="3.0" />
</node>

<node pkg="trajectory_publisher" type="trajectory_publisher" name="trajectory_publisher" output="screen">
<param name="trajectory_type" value="1" />
<param name="shape_omega" value="1.2" />
<param name="initpos_z" value="2.0" />
<param name="trajectory_type" value="4" />
<param name="shape_omega" value="0.15" />
<param name="initpos_z" value="3.0" />
<param name="reference_type" value="2" />
</node>

Expand Down
2 changes: 1 addition & 1 deletion geometric_controller/src/geometric_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ geometricCtrl::geometricCtrl(const ros::NodeHandle &nh, const ros::NodeHandle &n
nh_private_.param<int>("ctrl_mode", ctrl_mode_, ERROR_QUATERNION);
nh_private_.param<bool>("enable_sim", sim_enable_, true);
nh_private_.param<bool>("velocity_yaw", velocity_yaw_, false);
nh_private_.param<double>("max_acc", max_fb_acc_, 9.0);
nh_private_.param<double>("max_acc", max_fb_acc_, 12.0);
nh_private_.param<double>("yaw_heading", mavYaw_, 0.0);
nh_private_.param<double>("drag_dx", dx_, 0.0);
nh_private_.param<double>("drag_dy", dy_, 0.0);
Expand Down
12 changes: 6 additions & 6 deletions trajectory_publisher/include/trajectory_publisher/shapetrajectory.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,11 @@

#include "trajectory_publisher/trajectory.h"

#define TRAJ_ZERO 0
#define TRAJ_CIRCLE 1
#define TRAJ_LAMNISCATE 2
#define TRAJ_STATIONARY 3
enum class TrajectoryType {ZERO, CIRCLE, LAMNISCATE, STATIONARY, LISSAJOUS};

class shapetrajectory : public trajectory {
private:
int type_;
TrajectoryType type_;
int N;
double dt_;
double T_;
Expand All @@ -58,8 +55,11 @@ class shapetrajectory : public trajectory {
double traj_radius_, traj_omega_;

public:
shapetrajectory(int type);
shapetrajectory(TrajectoryType type);
virtual ~shapetrajectory();
double LissajousAxis(const double c, double omega, double time);
double LissajousAxisVelocity(const double c, double omega, double time);
double LissajousAxisAcceleration(const double c, double omega, double time);
void initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis, double omega);
void generatePrimitives(Eigen::Vector3d pos);
void generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel);
Expand Down
62 changes: 44 additions & 18 deletions trajectory_publisher/src/shapetrajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,11 @@

#include "trajectory_publisher/shapetrajectory.h"

shapetrajectory::shapetrajectory(int type) : trajectory(), N(0), dt_(0.1), T_(10.0), type_(type) {
shapetrajectory::shapetrajectory(TrajectoryType type) : trajectory(), N(0), dt_(0.1), T_(10.0), type_(type) {
traj_omega_ = 2.0;
traj_axis_ << 0.0, 0.0, 1.0;
traj_radial_ << 1.0, 0.0, 0.0;
traj_origin_ << 0.0, 0.0, 1.0;
traj_origin_ << 0.0, 0.0, 10.0;
};

shapetrajectory::~shapetrajectory(){
Expand All @@ -53,7 +53,7 @@ void shapetrajectory::initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis,
// Generate primitives based on current state for smooth trajectory
traj_origin_ = pos;
traj_omega_ = omega;
T_ = 2 * 3.14 / traj_omega_;
T_ = 2 * M_PI / traj_omega_;
traj_axis_ = axis;
traj_radial_ << 2.0, 0.0, 0.0;
}
Expand All @@ -67,33 +67,49 @@ void shapetrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d ve
void shapetrajectory::generatePrimitives(Eigen::Vector3d pos, Eigen::Vector3d vel, Eigen::Vector3d acc,
Eigen::Vector3d jerk) {}

double shapetrajectory::LissajousAxis(const double c, double omega, double time) {
return std::sin(c * omega * time ) + 0.1 * std::sin(5 * c * omega * time);
}

double shapetrajectory::LissajousAxisVelocity(const double c, double omega, double time) {
return c * omega * std::cos(c * omega * time ) + 0.1 * 5 * c * omega * std::cos(5 * c * omega * time);
}

double shapetrajectory::LissajousAxisAcceleration(const double c, double omega, double time) {
return - c * omega * c * omega * std::sin(c * omega * time ) - 0.1 * 5 * c * omega * 5 * c * omega * std::sin(5 * c * omega * time);
}

Eigen::Vector3d shapetrajectory::getPosition(double time) {
Eigen::Vector3d position;
double theta;

switch (type_) {
case TRAJ_ZERO:

case TrajectoryType::ZERO:
position << 0.0, 0.0, 0.0;
break;

case TRAJ_CIRCLE:

case TrajectoryType::CIRCLE:
theta = traj_omega_ * time;
position = std::cos(theta) * traj_radial_ + std::sin(theta) * traj_axis_.cross(traj_radial_) +
(1 - std::cos(theta)) * traj_axis_.dot(traj_radial_) * traj_axis_ + traj_origin_;
break;

case TRAJ_LAMNISCATE: // Lemniscate of Genero

case TrajectoryType::LAMNISCATE: // Lemniscate of Genero
theta = traj_omega_ * time;
position = std::cos(theta) * traj_radial_ + std::sin(theta) * std::cos(theta) * traj_axis_.cross(traj_radial_) +
(1 - std::cos(theta)) * traj_axis_.dot(traj_radial_) * traj_axis_ + traj_origin_;
break;
case TRAJ_STATIONARY: // Lemniscate of Genero

case TrajectoryType::LISSAJOUS:
position(0) = LissajousAxis(8.0, traj_omega_, time) + traj_origin_(0);
position(1) = LissajousAxis(5.0, traj_omega_, time) + traj_origin_(1);
position(2) = LissajousAxis(3.0, traj_omega_, time) + traj_origin_(2);
break;

case TrajectoryType::STATIONARY:
position = traj_origin_;
break;

}
return position;
}
Expand All @@ -103,24 +119,28 @@ Eigen::Vector3d shapetrajectory::getVelocity(double time) {
double theta;

switch (type_) {
case TRAJ_CIRCLE:

case TrajectoryType::CIRCLE:
velocity = traj_omega_ * traj_axis_.cross(getPosition(time));
break;
case TRAJ_STATIONARY:

case TrajectoryType::STATIONARY:
velocity << 0.0, 0.0, 0.0;
break;

case TRAJ_LAMNISCATE: // Lemniscate of Genero

case TrajectoryType::LAMNISCATE: // Lemniscate of Genero
theta = traj_omega_ * time;
velocity = traj_omega_ *
(-std::sin(theta) * traj_radial_ +
(std::pow(std::cos(theta), 2) - std::pow(std::sin(theta), 2)) * traj_axis_.cross(traj_radial_) +
(std::sin(theta)) * traj_axis_.dot(traj_radial_) * traj_axis_);
break;

case TrajectoryType::LISSAJOUS:
velocity(0) = LissajousAxisVelocity(8.0, traj_omega_, time);
velocity(1) = LissajousAxisVelocity(5.0, traj_omega_, time);
velocity(2) = LissajousAxisVelocity(3.0, traj_omega_, time);
break;

default:
velocity << 0.0, 0.0, 0.0;
break;
Expand All @@ -132,14 +152,20 @@ Eigen::Vector3d shapetrajectory::getAcceleration(double time) {
Eigen::Vector3d acceleration;

switch (type_) {
case TRAJ_CIRCLE:
case TrajectoryType::CIRCLE:

acceleration = traj_omega_ * traj_axis_.cross(getVelocity(time));
break;
case TRAJ_STATIONARY:

case TrajectoryType::STATIONARY:
acceleration << 0.0, 0.0, 0.0;
break;

case TrajectoryType::LISSAJOUS:
acceleration(0) = LissajousAxisAcceleration(8.0, traj_omega_, time);
acceleration(1) = LissajousAxisAcceleration(5.0, traj_omega_, time);
acceleration(2) = LissajousAxisAcceleration(3.0, traj_omega_, time);
break;

default:
acceleration << 0.0, 0.0, 0.0;
break;
Expand Down
2 changes: 1 addition & 1 deletion trajectory_publisher/src/trajectoryPublisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ trajectoryPublisher::trajectoryPublisher(const ros::NodeHandle& nh, const ros::N
} else { // Shape trajectories

num_primitives_ = 1;
motionPrimitives_.emplace_back(std::make_shared<shapetrajectory>(trajectory_type_));
motionPrimitives_.emplace_back(std::make_shared<shapetrajectory>(static_cast<TrajectoryType>(trajectory_type_)));
primitivePub_.push_back(nh_.advertise<nav_msgs::Path>("trajectory_publisher/primitiveset", 1));
}

Expand Down