Project

General

Profile

History

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