Project

General

Profile

maneuver component

Kinodynamic Trajectory Planner

This component is a spline-based, minimum time, constant snap trajectory generator. It applies to any robot with decoupled dynamics (or differencially flat).

Ports

state (in)

Data structure
  • struct ::or_pose_estimator::state state

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::pos_cov > pos_cov

      • double cov[6]

    • optional< struct ::or::t3d::att_cov > att_cov

      • double cov[10]

    • optional< struct ::or::t3d::att_pos_cov > att_pos_cov

      • double cov[12]

    • optional< struct ::or::t3d::vel_cov > vel_cov

      • double cov[6]

    • optional< struct ::or::t3d::avel_cov > avel_cov

      • double cov[6]

    • optional< struct ::or::t3d::acc_cov > acc_cov

      • double cov[6]

    • optional< struct ::or::t3d::aacc_cov > aacc_cov

      • double cov[6]


desired (out)

Data structure
  • struct ::or_rigid_body::state desired

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::jerk > jerk

      • double jx

      • double jy

      • double jz

    • optional< struct ::or::t3d::snap > snap

      • double sx

      • double sy

      • double sz


horizon (out)

Data structure
  • sequence< struct ::or_rigid_body::state > horizon

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::jerk > jerk

      • double jx

      • double jy

      • double jz

    • optional< struct ::or::t3d::snap > snap

      • double sx

      • double sy

      • double sz

Moving horizon of the current motion.

See set_horizon (activity) to configure this port.


Services

set_bounds (function)

Inputs
  • double xmin

  • double xmax

  • double ymin

  • double ymax

  • double zmin

  • double zmax

  • double yawmin

  • double yawmax

Configure the limits of the free space.

x, y and z minimum and maximum bounds limit the extend of the planned motion. Any trajectory going beyond these limits will be silently clipped. yaw limits in the same way the heading of the robot.

Note The planner does not consider the yaw orientation as modulo 2π. Hence, 0 and 2π represent different positions and going from an orientation of 0 to a one of 2π will make a full rotation. This must be considered when setting the yaw limits. A typical setting could thus be yawmin = -2π and yawmax = 2π to allow any positive or negative orientation goal.

set_velocity_limit (function)

Inputs
  • double v

  • double w

Throws
  • exception ::maneuver::e_limits


set_acceleration_limit (function)

Inputs
  • double a

  • double dw

Throws
  • exception ::maneuver::e_limits


set_jerk_limit (function)

Inputs
  • double j

  • double ddw

Throws
  • exception ::maneuver::e_limits


set_snap_limit (function)

Inputs
  • double s

  • double dddw

Throws
  • exception ::maneuver::e_limits


get_limits (function)

Outputs
  • double xmin

  • double xmax

  • double ymin

  • double ymax

  • double zmin

  • double zmax

  • double yawmin

  • double yawmax

  • double v

  • double w

  • double a

  • double dw

  • double j

  • double ddw

  • double s

  • double dddw


set_state (function)

Inputs
  • double x

  • double y

  • double z

  • double yaw

Set initial planning position


set_current_state (activity)

Throws
  • exception ::maneuver::e_nostate

Context

Set initial planning position to current one


take_off (activity)

Inputs
  • double height

  • double duration

Throws
  • exception ::maneuver::e_nostate

  • exception ::maneuver::e_limits

  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Context

Vertical take-off or landing from current state.

take_off plans and executes a trajectory from the current internal state (see set_current_state (activity)) to the state corresponding to the same X, Y position and the absolute Z coordinate equal to height. This position is reached with a 0 acceleration and velocity.

Any ongoing motion is cancelled and the planned trajectory is executed immediately.


goto (activity)

Inputs
  • double x

  • double y

  • double z

  • double yaw

  • double duration

Throws
  • exception ::maneuver::e_nostate

  • exception ::maneuver::e_limits

  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Context

Reach a given position from current state.

Note The planner does not consider the yaw orientation as modulo 2π. Hence, 0 and 2π represent different positions and going from an orientation of 0 to a one of 2π will make a full rotation.

waypoint (activity)

Inputs
  • double x

  • double y

  • double z

  • double yaw

  • double vx

  • double vy

  • double vz

  • double wz

  • double ax

  • double ay

  • double az

  • double duration

Throws
  • exception ::maneuver::e_nostate

  • exception ::maneuver::e_limits

  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Context

Push a given position to be reached after the last one.

Note The planner does not consider the yaw orientation as modulo 2π. Hence, 0 and 2π represent different positions and going from an orientation of 0 to a one of 2π will make a full rotation.

velocity (function)

Inputs
  • double vx

  • double vy

  • double vz

  • double wz

  • double ax

  • double ay

  • double az

  • double duration

Throws
  • exception ::maneuver::e_limits

  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Context

Reach a desired velocity within a given duration


replay (activity)

Inputs
  • string<128> filename (default "maneuver.log") Log file name

Throws
  • exception ::maneuver::e_nostate

  • exception ::maneuver::e_limits

  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Context

Replay a trajectory log file


wait (activity)

Context

Wait until last waypoint/goto point is reached


stop (activity)

Throws
  • exception ::maneuver::e_limits

  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Context

Cancel any motion and bring the velocity to zero


get_reference (attribute)

Outputs
  • struct ::or_rigid_body::state reference

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::jerk > jerk

      • double jx

      • double jy

      • double jz

    • optional< struct ::or::t3d::snap > snap

      • double sx

      • double sy

      • double sz


set_horizon (activity)

Inputs
  • double horizon Amount of time (s)

  • unsigned long samples Number of samples

Outputs
  • double horizon Amount of time (s)

  • unsigned long samples Number of samples

Throws
  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Context

Set sampling rate and duration of horizon port

This service activates the publication of the horizon port. The input parameters are adapted to match the internal sampling rate of trajectories, but are guaranteed to be at least as big as requested. The actual values are returned as output parameters

Passing 0 as the number of samples uses the internal sampling rate. Passing 0 for both duration and number of samples disables publishing of the horizon port.


log (function)

Inputs
  • string<64> path (default "/tmp/maneuver.log") Log file name

  • unsigned long decimation (default "1") Reduced logging frequency

Throws
  • exception ::maneuver::e_sys

    • short code

    • string<128> what

Log state


log_stop (function)

Stop logging


log_info (function)

Outputs
  • unsigned long miss Missed log entries

  • unsigned long total Total log entries

Show missed log entries


Tasks

plan

Context
  • Free running


exec

Context