该场景说明了使用 CoppeliaSim 中提供的 Ruckig 在线轨迹生成功能的各种方法:
1. 在线程脚本内使用单个阻塞函数(红色)
2. 在线程脚本中使用多个非阻塞函数(黄色)
3. 在非线程脚本中使用多个非阻塞函数(蓝色)
4.使用关节回调函数(绿色)
-- 红色圆柱体 -- 线程化子脚本,使用sim.moveToPose:
-- 引入sim库
sim = require 'sim'
-- Threaded child script, using sim.moveToPose:
-- 线程化子脚本,使用sim.moveToPose:
function sysCall_init()
-- 获取当前对象句柄
h = sim.getObject('.')
-- 获取速度图形对象句柄
graph = sim.getObject('/Object_Velocity_Graph')
-- 创建图形流
stream = sim.addGraphStream(graph, 'Red cylinder', 'm/s', 0, {1, 0, 0})
-- 设置图形流的变换 对图形流进行原始类型的变换,不进行缩放,但在垂直方向上偏移了0.0002个单位。
-- graph: 图形对象的句柄,表示图形流所属的图形对象。
-- stream: 图形流的句柄,表示要设置变换的图形流。
-- sim.stream_transf_raw: 表示变换的类型,这里使用原始类型。
-- 1: 表示缩放比例的因子,这里为1,表示不进行缩放。
-- 0.0002: 表示平移的偏移量,这里设置为0.0002,表示在图形流上方偏移0.0002个单位。
sim.setGraphStreamTransformation(graph, stream, sim.stream_transf_raw, 1, 0.0002)
end
function sysCall_sensing()
-- 获取对象速度的第三个分量(在z轴上的速度)
sim.setGraphStreamValue(graph, stream, sim.getObjectVelocity(h)[3])--获取对象在Z方向速度
end
function cb(tr, vel, accel, handle)
-- 设置对象的姿态
sim.setObjectPose(handle, tr)
end
function sysCall_thread()
-- 定义最大速度
local maxVel = {0.02, 0.02, 0.02, 0.2} -- vx, vy, vz in m/s, Vtheta is rad/s
-- 定义最大加速度
local maxAccel = {0.002, 0.002, 0.002, 0.02} -- ax, ay, az in m/s^2, Atheta is rad/s^2
-- 定义最大加加速度(在RML类型2下被忽略,即无穷大)
local maxJerk = {0.001, 0.001, 0.001, 0.01} -- is ignored (i.e. infinite) with RML type 2
-- 获取起始姿态
local startTr = sim.getObjectPose(h)
-- 复制起始姿态作为目标姿态
local goalTr = sim.copyTable(startTr)
-- 将目标姿态的z分量增加0.5
goalTr[3] = goalTr[3] + 0.5
-- 使用sim.moveToPose移动到目标姿态 使机器人从当前姿态 (startTr) 移动到目标姿态 (goalTr),在移动过程中受到了速度、加速度和加加速度的限制,并在运动完成后调用回调函数 cb 来设置对象的姿态。
-- -1: 表示移动的方式。在这里,-1 表示使用默认的移动方式,即线性插值。
-- startTr: 表示起始姿态,即机器人当前的姿态。
-- maxVel: 表示最大速度的限制,是一个包含四个元素的表,分别对应 x、y、z 方向的线速度和绕姿态的角速度。
-- maxAccel: 表示最大加速度的限制,同样是一个包含四个元素的表,分别对应 x、y、z 方向的线加速度和绕姿态的角加速度。
-- maxJerk: 表示最大加加速度的限制,同样是一个包含四个元素的表,但在这里被忽略,因为 RML (Robotics Motion Library) 类型 2 下其值被视为无穷大。
-- goalTr: 表示目标姿态,即机器人要达到的目标位置和姿态。
-- cb: 表示在运动完成后的回调函数,即运动结束后会调用此函数,这里是 cb(tr, vel, accel, handle)。
-- h: 表示机器人对象的句柄,即要执行移动操作的对象。
sim.moveToPose(-1, startTr, maxVel, maxAccel, maxJerk, goalTr, cb, h)
end
-- 红色关节 -- 线程化子脚本,使用sim.moveToConfig:
-- 引入sim库
sim = require 'sim'
-- Threaded child script, using sim.moveToConfig:
-- 线程化子脚本,使用sim.moveToConfig:
function sysCall_init()
-- 获取当前对象句柄
h = sim.getObject('.')
-- 获取关节速度图形对象句柄
graph = sim.getObject('/Joint_Velocity_Graph')
-- 创建图形流
stream = sim.addGraphStream(graph, 'Red joint', 'deg/s', 0, {1, 0, 0})
-- 设置图形流的变换
sim.setGraphStreamTransformation(graph, stream, sim.stream_transf_raw, 1, 0.02)
end
function sysCall_sensing()
-- 获取关节速度并将其转换为度/秒
sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
end
function cb(config, vel, accel, handle)
-- 设置关节的位置
sim.setJointPosition(handle, config[1])
end
function sysCall_thread()
-- 定义最大关节速度
maxVel = 0.1 -- 弧度/秒
-- 定义最大关节加速度
maxAccel = 0.01 -- 弧度/秒^2
-- 定义最大关节加加速度
maxJerk = 0.001 -- 弧度/秒^3
-- 移动到目标关节配置 使机器人的第一个关节从当前位置移动到目标位置0弧度,并受到了关节速度、关节加速度和关节加加速度的限制,在运动完成后调用回调函数 cb 来设置关节的位置。
-- -1: 表示移动的方式。在这里,-1 表示使用默认的移动方式,即线性插值。
-- {0}: 表示目标关节的配置,这里是一个包含一个元素的表,表示第一个关节的目标位置为0弧度。
-- nil, nil: 表示不设置目标速度和目标加速度。
-- {maxVel}: 表示最大关节速度的限制,是一个包含一个元素的表,表示第一个关节的最大速度。
-- {maxAccel}: 表示最大关节加速度的限制,是一个包含一个元素的表,表示第一个关节的最大加速度。
-- {maxJerk}: 表示最大关节加加速度的限制,是一个包含一个元素的表,表示第一个关节的最大加加速度。
-- {179 * math.pi / 180}: 表示目标关节的位置范围,这里设置为179度转换为弧度。
-- {0}: 表示不设置目标关节的速度。
-- cb: 表示在运动完成后的回调函数,即运动结束后会调用此函数,这里是 cb(config, vel, accel, handle)。
-- h: 表示机器人对象的句柄,即要执行移动操作的对象。
sim.moveToConfig(-1, {0}, nil, nil, {maxVel}, {maxAccel}, {maxJerk}, {179 * math.pi / 180}, {0}, cb, h)
end
-- 黄色圆柱 -- 线程化子脚本,使用sim.ruckigPos和sim.ruckigStep:
-- 引入sim库
sim = require 'sim'
-- Threaded child script, using sim.ruckigPos and sim.ruckigStep:
-- 线程化子脚本,使用sim.ruckigPos和sim.ruckigStep:
function sysCall_init()
-- 获取当前对象句柄
h = sim.getObject('.')
-- 获取速度图形对象句柄
graph = sim.getObject('/Object_Velocity_Graph')
-- 创建图形流
stream = sim.addGraphStream(graph, 'Yellow cylinder', 'm/s', 0, {1, 1, 0})
-- 设置图形流的变换
sim.setGraphStreamTransformation(graph, stream, sim.stream_transf_raw, 1, 0.0001)
end
function sysCall_sensing()
-- 获取对象速度的第三个分量(在z轴上的速度)
sim.setGraphStreamValue(graph, stream, sim.getObjectVelocity(h)[3])
end
function sysCall_thread()
-- 获取对象当前位置
local p = sim.getObjectPosition(h)
-- 构建当前位置、速度、加速度信息的表
local currentPosVelAccel = {p[1], p[2], p[3], 0, 0, 0, 0, 0, 0}
-- 定义最大速度、加速度、加加速度信息的表
local maxVelAccelJerk = {0.02, 0.02, 0.02, 0.002, 0.002, 0.002, 0.001, 0.001, 0.001} -- vx, vy, vz in m/s, ax, ay, az in m/s^2, jx, jy, jz is ignored (i.e. infinite) with RML type 2
-- 定义目标位置和速度信息的表
local targetPosVel = {p[1], p[2], p[3] + 0.5, 0, 0, 0} -- x, y, z in m, vx, vy, vz in m/s
-- 创建Ruckig对象并初始化 创建一个用于进行三维运动规划的Ruckig对象,并初始化其参数。
-- 3: 表示运动的维度,这里为3,表示是三维运动。
-- 0.0001: 表示最小步长,即运动规划的最小时间间隔。
-- -1: 表示运动类型。在这里,-1 表示使用默认的运动类型,即 RML (Robotics Motion Library) 类型 2。
-- currentPosVelAccel: 表示当前位置、速度、加速度信息的表,包含9个元素,依次是x、y、z位置,线速度,角速度,线加速度,角加速度。
-- maxVelAccelJerk: 表示最大速度、加速度、加加速度信息的表,包含9个元素,依次是x、y、z方向的最大速度、加速度和加加速度。
-- {1, 1, 1}: 表示是否考虑每个维度的运动限制,这里都设置为1表示考虑所有维度的运动限制。
-- targetPosVel: 表示目标位置和速度信息的表,包含6个元素,依次是x、y、z位置,线速度,角速度,线加速度。
local rmlObject = sim.ruckigPos(3, 0.0001, -1, currentPosVelAccel, maxVelAccelJerk, {1, 1, 1}, targetPosVel)
-- 启用步进模式
local lb = sim.setStepping(true)
result = 0
while result == 0 do
-- 在每个模拟步长内执行Ruckig运动规划的一步
result, newPosVelAccel = sim.ruckigStep(rmlObject, sim.getSimulationTimeStep())
if result ~= -1 then
-- 更新对象的位置
sim.setObjectPosition(h, newPosVelAccel)
end
-- 推进仿真
sim.step()
end
-- 恢复仿真步进模式状态
sim.setStepping(lb)
-- 移除Ruckig对象
sim.ruckigRemove(rmlObject)
end
--黄色关节 -- 线程化子脚本,使用sim.ruckigPos和sim.ruckigStep:
-- 引入sim库
sim = require 'sim'
-- Threaded child script, using sim.ruckigPos and sim.ruckigStep:
-- 线程化子脚本,使用sim.ruckigPos和sim.ruckigStep:
function sysCall_init()
-- 获取当前对象句柄
h = sim.getObject('.')
-- 获取关节速度图形对象句柄
graph = sim.getObject('/Joint_Velocity_Graph')
-- 创建图形流
stream = sim.addGraphStream(graph, 'Yellow joint', 'deg/s', 0, {1, 1, 0})
-- 设置图形流的变换
sim.setGraphStreamTransformation(graph, stream, sim.stream_transf_raw, 1, 0.01)
end
function sysCall_sensing()
-- 获取关节速度并将其转换为度/秒
sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
end
function sysCall_thread()
-- 定义最大关节速度
maxVel = 0.1 -- 弧度/秒
-- 定义最大关节加速度
maxAccel = 0.01 -- 弧度/秒^2
-- 定义最大关节加加速度
maxJerk = 0.001 -- 弧度/秒^3
-- 创建Ruckig对象并初始化 创建一个用于进行单轴关节运动规划的Ruckig对象,并初始化其参数。
-- 1: 表示运动的维度,这里为1,表示是单轴运动(关节运动)。
-- 0.0001: 表示最小步长,即运动规划的最小时间间隔。
-- -1: 表示运动类型。在这里,-1 表示使用默认的运动类型,即 RML (Robotics Motion Library) 类型 2。
-- {0, 0, 0}: 表示当前位置、速度、加速度信息的表,包含3个元素,依次是关节的当前位置、速度和加速度。
-- {maxVel, maxAccel, maxJerk}: 表示最大速度、加速度、加加速度信息的表,包含3个元素,依次是关节的最大速度、加速度和加加速度。
-- {1}: 表示是否考虑每个维度的运动限制,这里设置为1表示考虑所有维度的运动限制。
-- {179 * math.pi / 180, 0, 0}: 表示目标位置、速度、加速度信息的表,包含3个元素,依次是关节的目标位置、速度和加速度。这里将179度转换为弧度。
rmlObject = sim.ruckigPos(1, 0.0001, -1, {0, 0, 0}, {maxVel, maxAccel, maxJerk}, {1}, {179 * math.pi / 180, 0, 0})
-- 启用步进模式
local lb = sim.setStepping(true)
result = 0
while result == 0 do
-- 在每个模拟步长内执行Ruckig运动规划的一步
result, newPosVelAccel = sim.ruckigStep(rmlObject, sim.getSimulationTimeStep())
if result ~= -1 then
-- 更新关节的位置
sim.setJointPosition(h, newPosVelAccel[1]) -- 或者根据关节模式使用 sim.setJointTargetPosition
end
-- 推进仿真
sim.step()
end
-- 恢复仿真步进模式状态
sim.setStepping(lb)
-- 移除Ruckig对象
sim.ruckigRemove(rmlObject)
end
--蓝色圆柱体 -- 非线程化子脚本,使用sim.ruckigPos和sim.ruckigStep:
function sysCall_init()
-- 获取当前对象句柄
h = sim.getObject('.')
-- 获取速度图形对象句柄
graph = sim.getObject('/Object_Velocity_Graph')
-- 创建图形流
stream = sim.addGraphStream(graph, 'Blue cylinder', 'm/s', 0, {0, 0.5, 1})
-- 获取对象当前位置
local p = sim.getObjectPosition(h)
-- 构建当前位置、速度、加速度信息的表
local currentPosVelAccel = {p[1], p[2], p[3], 0, 0, 0, 0, 0, 0}
-- 定义最大速度、加速度、加加速度信息的表
local maxVelAccelJerk = {0.02, 0.02, 0.02, 0.002, 0.002, 0.002, 0.001, 0.001, 0.001} -- vx, vy, vz in m/s, ax, ay, az in m/s^2, jx, jy, jz is ignored (i.e. infinite) with RML type 2
-- 定义目标位置和速度信息的表
local targetPosVel = {p[1], p[2], p[3] + 0.5, 0, 0, 0} -- x, y, z in m, vx, vy, vz in m/s
-- 创建Ruckig对象并初始化
rmlObject = sim.ruckigPos(3, 0.0001, -1, currentPosVelAccel, maxVelAccelJerk, {1, 1, 1}, targetPosVel)
end
function sysCall_sensing()
-- 获取对象速度的第三个分量(在z轴上的速度)
sim.setGraphStreamValue(graph, stream, sim.getObjectVelocity(h)[3])
end
function sysCall_actuation()
-- 如果Ruckig对象存在,则执行一步运动规划
if rmlObject then
local result, newPosVelAccel = sim.ruckigStep(rmlObject, sim.getSimulationTimeStep())
-- 如果运动规划成功,则更新对象的位置
if result ~= -1 then
sim.setObjectPosition(h, newPosVelAccel)
end
-- 如果运动规划完成或出错,则移除Ruckig对象
if result == 1 or result == -1 then
sim.ruckigRemove(rmlObject)
rmlObject = nil
end
end
end
function sysCall_cleanup()
-- 在清理阶段,确保移除Ruckig对象
if rmlObject then
sim.ruckigRemove(rmlObject)
end
end
--蓝色关节 -- 非线程化子脚本,使用sim.ruckigPos和sim.ruckigStep:
function sysCall_init()
-- 获取当前关节对象句柄
h = sim.getObject('.')
-- 获取关节速度图形对象句柄
graph = sim.getObject('/Joint_Velocity_Graph')
-- 创建图形流
stream = sim.addGraphStream(graph, 'Blue joint', 'deg/s', 0, {0, 0.5, 1})
-- 定义最大关节速度
maxVel = 0.1 -- 弧度/秒
-- 定义最大关节加速度
maxAccel = 0.01 -- 弧度/秒^2
-- 定义最大关节加加速度
maxJerk = 0.001 -- 弧度/秒^3
-- 创建Ruckig对象并初始化
rmlObject = sim.ruckigPos(1, 0.0001, -1, {0, 0, 0}, {maxVel, maxAccel, maxJerk}, {1}, {179 * math.pi / 180, 0, 0})
end
function sysCall_sensing()
-- 获取关节速度并将其转换为度/秒
sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
end
function sysCall_actuation()
-- 如果Ruckig对象存在,则执行一步运动规划
if rmlObject then
local result, newPosVelAccel = sim.ruckigStep(rmlObject, sim.getSimulationTimeStep())
-- 如果运动规划成功,则更新关节的位置
if result ~= -1 then
sim.setJointPosition(h, newPosVelAccel[1]) -- 或者根据关节模式使用 sim.setJointTargetPosition
end
-- 如果运动规划完成或出错,则移除Ruckig对象
if result == 1 or result == -1 then
sim.ruckigRemove(rmlObject)
rmlObject = nil
end
end
end
function sysCall_cleanup()
-- 在清理阶段,确保移除Ruckig对象
if rmlObject then
sim.ruckigRemove(rmlObject)
end
end
--绿色关节 -- 线程化子脚本,使用sim.moveToConfig:
function sysCall_init()
-- 获取当前关节对象句柄
h = sim.getObject('.')
-- 获取关节速度图形对象句柄
graph = sim.getObject('/Joint_Velocity_Graph')
-- 创建图形流
stream = sim.addGraphStream(graph, 'Green joint', 'deg/s', 0, {0, 1, 0})
-- 设置图形流的变换
sim.setGraphStreamTransformation(graph, stream, sim.stream_transf_raw, 1, -0.01)
end
function sysCall_sensing()
-- 获取关节速度并将其转换为度/秒
sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
end
function sysCall_thread()
-- 定义最大关节速度
maxVel = 0.1 -- 弧度/秒
-- 定义最大关节加速度
maxAccel = 0.01 -- 弧度/秒^2
-- 定义最大关节加加速度
maxJerk = 0.001 -- 弧度/秒^3
-- 通过设置关节的目标位置,触发关节的回调函数
sim.setJointTargetPosition(h, 179 * math.pi / 180, {maxVel, maxAccel, maxJerk})
end
function sysCall_joint(inData)
-- 用户可以在这里自由实现回调函数
-- 由于该函数没有返回任何值,调用将被重定向到场景层次结构的上层
-- 并最终由主脚本中的默认关节回调函数处理
end