【机器人学】机器人开源项目KDL源码学习:(3)机器人操作空间路径规划(Path Planning)和轨迹规划(Trajectory Planning)示例
2017-02-21 19:34
627 查看
很多同学会把路径规划(Path Planning)和轨迹规划(Trajectory Planning)这两个概念混淆,路径规划只是表示了机械臂末端在操作空间中的几何信息,比如从工作台的一端(A点)沿直线移动到另一端(B点)。而轨迹规划则加上了时间律,比如它要完成的任务是从A点开始到B点结束,中间是以梯形的速度规律来运行的(先以一个加速度a加速运动到一定的速度Vmax,然后再以固定的速度Vmax巡航,最后再以加速度-a减速到0,同时到达B点),经过轨迹规划后,得到的是操作空间位置和时间的关系式。
(orocos_kinematics_dynamics-master\orocos_kdl\examples\trajectory_example.cpp)
这几段表示将从起始到终点的速度设定为梯形波,最大速度为0.5,加速度为0.1。
下面两张图是利用matlab绘出的轨迹曲线和机械臂末端与时间的关系,可以看出在明显的圆弧过渡。
Path velocity呈明显的梯形形状。
(orocos_kinematics_dynamics-master\orocos_kdl\examples\trajectory_example.cpp)
int main(int argc,char* argv[]) { using namespace KDL; // Create the trajectory: // use try/catch to catch any exceptions thrown. // NOTE: exceptions will become obsolete in a future version. try { // Path_RoundedComposite defines the geometric path along // which the robot will move. // Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.01,new RotationalInterpolation_SingleAxis()); path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))); path->Finish(); VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1); velpref->SetProfile(0,path->PathLength()); Trajectory* traject = new Trajectory_Segment(path, velpref); Trajectory_Composite* ctraject = new Trajectory_Composite(); ctraject->Add(traject); ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)))); // use the trajectory double dt=0.1; std::ofstream of("./trajectory.dat"); for (double t=0.0; t <= traject->Duration(); t+= dt) { Frame current_pose; current_pose = traject->Pos(t); for (int i=0;i<4;++i) for (int j=0;j<4;++j) of << current_pose(i,j) << "\t"; of << "\n"; // also velocities and accelerations are available ! //traject->Vel(t); //traject->Acc(t); } of.close(); // you can get some meta-info on the path: for (int segmentnr=0; segmentnr < path->GetNrOfSegments(); segmentnr++) { double starts,ends; Path::IdentifierType pathtype; if (segmentnr==0) { starts = 0.0; } else { starts = path->GetLengthToEndOfSegment(segmentnr-1); } ends = path->GetLengthToEndOfSegment(segmentnr); pathtype = path->GetSegment(segmentnr)->getIdentifier(); std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends; switch(pathtype) { case Path::ID_CIRCLE: std::cout << " circle"; break; case Path::ID_LINE: std::cout << " line "; break; default: std::cout << " unknown "; break; } std::cout << std::endl; } std::cout << " trajectory written to the ./trajectory.dat file " << std::endl; delete ctraject; } catch(Error& error) { std::cout <<"I encountered this error : " << error.Description() << std::endl; std::cout << "with the following type " << error.GetType() << std::endl; } }关键代码详细解读:
path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))); path->Finish();这几段代码表示在操作空间中插入6个中间路径点,注意需要以path->Finish()结束。
VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1); velpref->SetProfile(0,path->PathLength()); Trajectory* traject = new Trajectory_Segment(path, velpref);
这几段表示将从起始到终点的速度设定为梯形波,最大速度为0.5,加速度为0.1。
Trajectory_Composite* ctraject = new Trajectory_Composite(); ctraject->Add(traject); ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));这几段没看出起到了什么作用。
double dt=0.1; std::ofstream of("./trajectory.dat"); for (double t=0.0; t <= traject->Duration(); t+= dt) { Frame current_pose; current_pose = traject->Pos(t); for (int i=0;i<4;++i) for (int j=0;j<4;++j) of << current_pose(i,j) << "\t"; of << "\n"; } of.close();这几段用来输出轨迹,步长为0.1,将轨迹的位姿存放在trajectory.dat文件中。
for (int segmentnr=0; segmentnr < path->GetNrOfSegments(); segmentnr++) { double starts,ends; Path::IdentifierType pathtype; if (segmentnr==0) { starts = 0.0; } else { starts = path->GetLengthToEndOfSegment(segmentnr-1); } ends = path->GetLengthToEndOfSegment(segmentnr); pathtype = path->GetSegment(segmentnr)->getIdentifier(); std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends; switch(pathtype) { case Path::ID_CIRCLE: std::cout << " circle"; break; case Path::ID_LINE: std::cout << " line "; break; default: std::cout << " unknown "; break; } std::cout << std::endl; } std::cout << " trajectory written to the ./trajectory.dat file " << std::endl;这几段是打印出各个相邻的路径点的轨迹形式,在笛卡尔空间中,轨迹基元有直线和圆两种。
下面两张图是利用matlab绘出的轨迹曲线和机械臂末端与时间的关系,可以看出在明显的圆弧过渡。
Path velocity呈明显的梯形形状。
相关文章推荐
- 【机器人学】机器人开源项目KDL源码学习:(6)笛卡尔空间轨迹规划、圆弧过渡、姿态插值、梯形速度、pathlength
- 【机器人学】机器人开源项目KDL源码学习:(7)examples中的CMakeList.txt文件解读
- 【机器人学】机器人开源项目KDL源码学习:(4)机械臂逆动力学的牛顿欧拉算法
- 【机器人学】机器人开源项目KDL源码学习:(8)KDL的精髓
- 【机器人学】机器人开源项目KDL源码学习:(1)下载源码并在ubuntu下运行geometry部分(旋转矩阵)
- 【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解
- 【机器人学】机器人开源项目KDL源码学习:(5)KDL如何求解几何雅克比矩阵
- 机器人中的轨迹规划(Trajectory Planning )
- BaseRecyclerViewAdapterHelper开源项目之BaseQuickAdapter源码学习上拉加载的实现代码(三)
- 学习php开源项目的源码指南
- 转: 如何快速学习一个开源项目源码?
- BaseRecyclerViewAdapterHelper开源项目之BaseSectionQuickAdapter 实现Expandable And collapse效果的源码学习
- BaseRecyclerViewAdapterHelper开源项目之BaseQuickAdapter源码学习之预加载的实现(二)
- apache的开源项目-模板引擎(Velocity)_学习了两天就上手啦_源码下载
- BaseRecyclerViewAdapterHelper开源项目之BaseMultiItemQuickAdapter 实现多类型源码学习
- BaseRecyclerViewAdapterHelper开源项目之BaseSectionQuickAdapter 实现Expandable And collapse效果的源码学习
- BaseRecyclerViewAdapterHelper开源项目之BaseQuickAdapter源码学习上拉加载的实现代码(三)
- 路径规划path planning-人工势场法
- apache的开源项目-模板引擎(Velocity)_学习了两天就上手啦_源码下载
- 关于 Android 开源项目汇总、学习、源码解析