当前位置:网站首页>[coppeliasim] 6-DOF path planning
[coppeliasim] 6-DOF path planning
2022-07-06 02:12:00 【Ten year dream laboratory】
function sysCall_threadmain()
robotHandle=sim.getObjectHandle('Start')-- obtain dummy:Start The handle of
targetHandle=sim.getObjectHandle('End')-- Get the target dummy:End The handle of
t=simOMPL.createTask('t')-- establish OMPL Planning tasks t
ss={simOMPL.createStateSpace('6d',simOMPL.StateSpaceType.pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}-- Create state space :6d, State space type - Posture 3d,Start The handle of , The border {x,y,z}->{X,Y,Z}, The weight 1
simOMPL.setStateSpace(t,ss)-- Set up tasks t State space of ss
simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)-- Set algorithm
simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})-- Set up collision er Collision ee
startpos=sim.getObjectPosition(robotHandle,-1)-- take The starting position
startorient=sim.getObjectQuaternion(robotHandle,-1)-- Take the starting direction
startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}-- Initial pose state : Position and quaternion
simOMPL.setStartState(t,startpose)-- Set the initial pose state
goalpos=sim.getObjectPosition(targetHandle,-1)-- Target location
goalorient=sim.getObjectQuaternion(targetHandle,-1)-- Target attitude
goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}-- Target pose status
simOMPL.setGoalState(t,goalpose)-- Set the target pose state
r,path=simOMPL.compute(t,20,-1,200)-- Calculation :20——> Calculate the path finder in seconds. Maximum time , Maximum time to simplify the path -1 Indicates the default time ,minimum number of states to be returned The minimum number of States to return 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. Path state set
while true do
-- Simply jump through the path points, no interpolation here:
for i=1,#path-7,7 do-- Every time 7 state Move
pos={path[i],path[i+1],path[i+2]}-- Location
orient={path[i+3],path[i+4],path[i+5],path[i+6]}-- Attitude quaternion
sim.setObjectPosition(robotHandle,-1,pos)-- Set up mobile Start The location of
sim.setObjectQuaternion(robotHandle,-1,orient)-- Set posture
sim.switchThread()-- Switching thread
end
end
end
边栏推荐
- Adapter-a technology of adaptive pre training continuous learning
- The ECU of 21 Audi q5l 45tfsi brushes is upgraded to master special adjustment, and the horsepower is safely and stably increased to 305 horsepower
- Text editing VIM operation, file upload
- NiO related knowledge (II)
- [eight part essay] what is the difference between unrepeatable reading and unreal reading?
- Flutter Doctor:Xcode 安装不完整
- Campus second-hand transaction based on wechat applet
- Numpy array index slice
- 【机器人手眼标定】eye in hand
- 抓包整理外篇——————状态栏[ 四]
猜你喜欢
Derivation of Biot Savart law in College Physics
Computer graduation design PHP enterprise staff training management system
PHP campus financial management system for computer graduation design
[solution] add multiple directories in different parts of the same word document
抓包整理外篇——————状态栏[ 四]
Blue Bridge Cup embedded_ STM32 learning_ Key_ Explain in detail
国家级非遗传承人高清旺《四大美人》皮影数字藏品惊艳亮相!
Prepare for the autumn face-to-face test questions
It's wrong to install PHP zbarcode extension. I don't know if any God can help me solve it. 7.3 for PHP environment
[depth first search] Ji Suan Ke: Betsy's trip
随机推荐
Text editing VIM operation, file upload
Redis如何实现多可用区?
Basic operations of databases and tables ----- default constraints
729. 我的日程安排表 I / 剑指 Offer II 106. 二分图
Redis-列表
Derivation of Biot Savart law in College Physics
1. Introduction to basic functions of power query
How to improve the level of pinduoduo store? Dianyingtong came to tell you
PHP campus movie website system for computer graduation design
How to upgrade kubernetes in place
It's wrong to install PHP zbarcode extension. I don't know if any God can help me solve it. 7.3 for PHP environment
Social networking website for college students based on computer graduation design PHP
02.Go语言开发环境配置
Leetcode sum of two numbers
通过PHP 获取身份证相关信息 获取生肖,获取星座,获取年龄,获取性别
Accelerating spark data access with alluxio in kubernetes
Minecraft 1.18.1、1.18.2模组开发 22.狙击枪(Sniper Rifle)
2022年PMP项目管理考试敏捷知识点(8)
2 power view
Reasonable and sensible