当前位置:网站首页>【coppeliasim】6自由度路径规划
【coppeliasim】6自由度路径规划
2022-07-06 02:00:00 【十年一梦实验室】
function sysCall_threadmain()
robotHandle=sim.getObjectHandle('Start')--获取dummy:Start的句柄
targetHandle=sim.getObjectHandle('End')--获取目标dummy:End的句柄
t=simOMPL.createTask('t')--创建OMPL规划任务t
ss={simOMPL.createStateSpace('6d',simOMPL.StateSpaceType.pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}--创建状态空间:6d,状态空间类型-姿态3d,Start的句柄,边界{x,y,z}->{X,Y,Z},权重1
simOMPL.setStateSpace(t,ss)--设置任务t的状态空间ss
simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)--设置算法
simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})--设置碰撞er 碰撞ee
startpos=sim.getObjectPosition(robotHandle,-1)--取 起始位置
startorient=sim.getObjectQuaternion(robotHandle,-1)--取起始方向
startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}--起始位姿状态:位置和四元数
simOMPL.setStartState(t,startpose)--设置起始位姿状态
goalpos=sim.getObjectPosition(targetHandle,-1)--目标位置
goalorient=sim.getObjectQuaternion(targetHandle,-1)--目标姿态
goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}--目标位姿状态
simOMPL.setGoalState(t,goalpose)--设置目标位姿状态
r,path=simOMPL.compute(t,20,-1,200)--计算:20——>计算路径查找程序in seconds.最大时间,用于简化路径的最大时间-1表示默认时间,minimum number of states to be returned最少要返回的状态数200
--r=true: true if a solution has been found
--path: a table of states, representing the solution, from start to goal. States are specified linearly.路径状态集合
while true do
-- Simply jump through the path points, no interpolation here:
for i=1,#path-7,7 do--每7状态 动一下
pos={path[i],path[i+1],path[i+2]}--位置
orient={path[i+3],path[i+4],path[i+5],path[i+6]}--姿态四元数
sim.setObjectPosition(robotHandle,-1,pos)--设置移动Start的位置
sim.setObjectQuaternion(robotHandle,-1,orient)--设置姿态
sim.switchThread()--切换线程
end
end
end
边栏推荐
- 一题多解,ASP.NET Core应用启动初始化的N种方案[上篇]
- leetcode-两数之和
- Exness: Mercedes Benz's profits exceed expectations, and it is predicted that there will be a supply chain shortage in 2022
- Basic operations of databases and tables ----- non empty constraints
- Win10 add file extension
- Dynamics 365 开发协作最佳实践思考
- 2022年PMP项目管理考试敏捷知识点(8)
- UE4 unreal engine, editor basic application, usage skills (IV)
- Unreal browser plug-in
- selenium 元素定位(2)
猜你喜欢
Selenium waiting mode
How to improve the level of pinduoduo store? Dianyingtong came to tell you
[Jiudu OJ 09] two points to find student information
Know MySQL database
PHP campus movie website system for computer graduation design
Adapter-a technology of adaptive pre training continuous learning
Exness: Mercedes Benz's profits exceed expectations, and it is predicted that there will be a supply chain shortage in 2022
Audio and video engineer YUV and RGB detailed explanation
TrueType字体文件提取关键信息
Publish your own toolkit notes using NPM
随机推荐
Comments on flowable source code (XXXV) timer activation process definition processor, process instance migration job processor
[solution] add multiple directories in different parts of the same word document
Ali test open-ended questions
[ssrf-01] principle and utilization examples of server-side Request Forgery vulnerability
Prepare for the autumn face-to-face test questions
I like Takeshi Kitano's words very much: although it's hard, I will still choose that kind of hot life
Computer graduation design PHP campus restaurant online ordering system
This time, thoroughly understand the deep copy
PHP campus financial management system for computer graduation design
Selenium element positioning (2)
You are using pip version 21.1.1; however, version 22.0.3 is available. You should consider upgradin
抓包整理外篇——————状态栏[ 四]
Global and Chinese market of wheelchair climbing machines 2022-2028: Research Report on technology, participants, trends, market size and share
leetcode-2.回文判断
Redis-字符串类型
NiO related knowledge (II)
【clickhouse】ClickHouse Practice in EOI
Multi function event recorder of the 5th National Games of the Blue Bridge Cup
Xshell 7 Student Edition
Pangolin Library: subgraph