1. Samir Menon Avatar Samir Menon
  2. Untitled project
  3. scl-manips-v2

Wiki

Clone wiki

scl-manips-v2 / people / Brian

View History

THIS IS OUTDATED. PLEASE SEE ME FOR LATEST TRAJECTORY VIRTUAL CLASS

Trajectory Generation

files:

  • TrajectoryWaypts.cpp
  • TrajectoryWaypts.hpp

Overview

TrajectoryWaypts creates a smooth trajectory from a set of waypoints. For the robot, each waypoint consists of xyz position and yaw orientation and corresponding time. For the camera, the waypoint consists of xyz position, camera pan, and camera angle (yaw and pitch). Velocity and acceleration are also required. For user must also specify a type of trajectory. For example:

start 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00
poly 5.00 1.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 180.00 0.00 0.00 turn around
poly 25.00 -5.00 0.00 0.00 3.00 0.00 0.00 0.80 0.00 0.00 90.00 0.00 0.00 move toward valve

The robot will start at the origin, will follow a polynomial trajectory that will arrive (1,0,0) at a time 5 seconds into the simulation. Upon arrival, it will have completed a 180 degrees. Velocity or acceleration are 0, so the robot will start and end at rest. The robot will then follow a polynomial trajectory that will start at (5,0,0) and arrive at (-5,3,0.8) at a time 25 seconds into the simulation. It will have completed a 90 degree turn.

The following trajectory types are allowed:

  • jump - the robot will jump to the waypoint at the appropriate time (may case the controller to crash).
  • poly - the robot will follow a 5th order polynomial
  • arc - the robot will follow an arc (xyz now specifies the arc center, and yaw gives the arc angle).

The camera commands are the same, but with pitch in addition to yaw. There is no arc option for the camera YET.

Public Functions

public functions for robot

  • void ImportBotWaypts(std::string wayptFilePath);
  • void UpdateBotPosition(void);
  • void AddBotWaypt(std::string new_transition, double new_time, double new_pos[4], double new_vel[4], double new_accel[4]);
  • Eigen::Vector3d GetBotPosition(void);
  • Eigen::Vector3d GetLeftBallPosition(void);
  • Eigen::Vector3d GetRightBallPosition(void);

public functions for camera

  • void ImportCameraWaypts(std::string wayptFilePath);
  • void UpdateCameraPosition(void);
  • void AddCameraWaypt(std::string new_transition, double new_time, double new_pos[5], double new_vel[5], double new_accel[5]);
  • Eigen::Vector3d GetCameraPosition(void);
  • Eigen::Matrix3d GetCameraOrientation(void);

public functions for trajectory

  • bool IsBotTrajActive(void);
  • bool IsCameraTrajActive(void);

beta version functionality

The body or camera will arrive at the waypoint at time "time" using a trajectory path of type "type"

for the body:
type time x dx ddx y dy ddy z dz ddz theta dtheta ddtheta

types:

  • jump (jump to waypoint)
  • poly (polynomial trajectory from previous waypoint to this one)
  • arc (center given by x y z, arc angle theta)

example:
start 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00
poly 28.00 -5.00 0.00 0.00 4.00 0.00 0.00 0.80 0.00 0.00 90.00 0.00 0.00 macro mini valve grab
poly 30.00 -5.00 0.00 0.00 4.00 0.00 0.00 0.80 0.00 0.00 90.00 0.00 0.00
arc 48.00 -5.00 0.00 0.00 5.00 0.00 0.00 0.80 0.00 0.00 -90.00 0.00 0.00 turn valve
poly 50.00 -5.00 0.00 0.00 6.00 0.00 0.00 0.80 0.00 0.00 -90.00 0.00 0.00 backup from valve

at time 25, be at the origin with orientation of 0
at time 28, finish a polynomial trajectory to arrive at (-5,4,0.8) with orientation of 90 degrees
at time 30, be at the same place (i.e. pause for 2 seconds)
at time 48, finish an arc trajectory, centered of (-5,5,0.8), arc angle of -90 degrees, implied radius of 1 meter
at time 50, finish a polyonomial to arrive at (-5,6,0.8) with orientation of -90 degrees
at time 10, be at the end of the arc path.

for the camera: type time x dx ddx y dy ddy z dz ddz pan tilt

types:

  • jump
  • poly

example:
start 0.00 -1.00 0.00 0.00 -2.20 0.00 0.00 0.75 0.00 0.00 0.00 0.00 0.00 -10.00 0.00 0.00
jump 0.00 -1.00 0.00 0.00 -5.20 0.00 0.00 4.00 0.00 0.00 -80.00 0.00 0.00 -35.00 0.00 0.00 3rd person view of moving to valve
jump 15.00 -1.00 0.00 0.00 -5.20 0.00 0.00 4.00 0.00 0.00 -80.00 0.00 0.00 -35.00 0.00 0.00
poly 23.00 -1.00 0.00 0.00 3.75 0.00 0.00 1.50 0.00 0.00 0.00 0.00 0.00 -10.00 0.00 0.00 close up of macro mini

Implementation in Simulation


void CPostureApp::stepMySimulation()
{
sutil::CSystemClock::tick(db_->sim_dt_); //Tick the clock.

if (use_trajectory_)
{
//initiate trajectories - Brian
my_bot_traj.UpdateBotPosition();
my_cam_traj.UpdateCameraPosition();
if (my_cam_traj.IsCameraTrajActive() )
{
scl::SChaiGraphics *mychai = db_->s_gui_.chai_data_.at("DivebotStdView");
mychai->chai_cam_->setLocalPos( my_cam_traj.GetCameraPosition() );
mychai->chai_cam_->setLocalRot( my_cam_traj.GetCameraOrientation() );
}
}

//Update the operational point tasks (if any)
std::vector<SOpPointUiLinkData>::iterator it,ite;
for(it = taskvec_op_point_.begin(), ite = taskvec_op_point_.end(); it!=ite; ++it )
{ it->task_->setGoal(db_->s_gui_.ui_point_[it->ui_pt_]); } Set the goal position.

if(has_been_init_com_task_) //Update the com task (if any)
{ task_com_->setGoal(db_->s_gui_.ui_point_[ui_pt_com_]); }

if(has_been_init_leftop_task_) //Update the left-hand-op task (if any)
{
if (use_trajectory_ && my_bot_traj.IsBotTrajActive() )
{
db_->s_gui_.ui_point_[ui_pt_leftop_] = my_bot_traj.GetLeftBallPosition();
}
task_leftop_->setGoal(db_->s_gui_.ui_point_[ui_pt_leftop_]);
}

if(has_been_init_rightop_task_) //Update the right-hand-op task (if any)
{
if (use_trajectory_ && my_bot_traj.IsBotTrajActive() )
{
db_->s_gui_.ui_point_[ui_pt_rightop_] = my_bot_traj.GetRightBallPosition();
}
task_rightop_->setGoal(db_->s_gui_.ui_point_[ui_pt_rightop_]);
}

...
}

Future Work

  • Pausing trajectory
  • Arc for camera waypoints
  • Integration of OTG
  • XML input file, rather than txt file

Getting Started

  1. Get master repository
    In Bitbucket, navigate to repositories: samirmemon / scl-manips-group. The Overview tab (front page) has instructions to get started. This will download the repository onto the local computer.
  2. Enable git colors in terminal with git config --global color.ui auto
  3. Create new local branch
    Open a terminal. Create a new branch with git checkout -b [new_branch_name]. This will create a new branch and check it out.
  4. Get Remote Branch
    1. In Bitbucket, navigate to repsitory branch of interest (e.g. geraldb / SCL_Divebot). On the right hand side, select HTTPS and copy the url (e.g. https://bsoe@bitbucket.org/geraldb/scl_divebot.git).
    2. In terminal and add the remote branch with git remote add bitbucket [copied_url], followed by git remote to view the new repository, and git fetch bitbucket. Then pull the branch. git pull bitbucket [BRANCH_NAME].
  5. Compile Branch
    In terminal, navigate to the branch you just pulled. Compile with sh make_dbg.sh and sh make_rel.sh. Then run the program with sh run.sh.
  6. Install Eclipse
    Follow the instructions on the bitbucket wiki: install_eclipse
  7. Configure Eclipse
    Follow the instructions on the bitbucket wiki: configure_eclipse
  8. Enable folding and line numbers in eclipse
    • Window -> Preferences -> C++ -> Editor -> Folding -> Enable folding of control flow statements
    • Window -> Preferences -> General -> Editor -> Text Editors -> Show line number
  9. Other Wiki Pages
    Go to the wiki tab on bitbucket repository samirmemon / scl-manips-group: wiki_home

Updated