【仿真】ruckig在线轨迹生成器示例

该场景说明了使用 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)
endfunction sysCall_sensing()-- 获取对象速度的第三个分量（在z轴上的速度）sim.setGraphStreamValue(graph, stream, sim.getObjectVelocity(h)[3])--获取对象在Z方向速度
endfunction cb(tr, vel, accel, handle)-- 设置对象的姿态sim.setObjectPose(handle, tr)
endfunction 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.5goalTr[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)
endfunction sysCall_sensing()-- 获取关节速度并将其转换为度/秒sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
endfunction cb(config, vel, accel, handle)-- 设置关节的位置sim.setJointPosition(handle, config[1])
endfunction 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)
endfunction sysCall_sensing()-- 获取对象速度的第三个分量（在z轴上的速度）sim.setGraphStreamValue(graph, stream, sim.getObjectVelocity(h)[3])
endfunction 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 = 0while 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)
endfunction sysCall_sensing()-- 获取关节速度并将其转换为度/秒sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
endfunction 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 = 0while result == 0 do-- 在每个模拟步长内执行Ruckig运动规划的一步result, newPosVelAccel = sim.ruckigStep(rmlObject, sim.getSimulationTimeStep())if result ~= -1 then-- 更新关节的位置sim.setJointPosition(h, newPosVelAccel[1]) -- 或者根据关节模式使用 sim.setJointTargetPositionend-- 推进仿真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)
endfunction sysCall_sensing()-- 获取对象速度的第三个分量（在z轴上的速度）sim.setGraphStreamValue(graph, stream, sim.getObjectVelocity(h)[3])
endfunction sysCall_actuation()-- 如果Ruckig对象存在，则执行一步运动规划if rmlObject thenlocal result, newPosVelAccel = sim.ruckigStep(rmlObject, sim.getSimulationTimeStep())-- 如果运动规划成功，则更新对象的位置if result ~= -1 thensim.setObjectPosition(h, newPosVelAccel)end-- 如果运动规划完成或出错，则移除Ruckig对象if result == 1 or result == -1 thensim.ruckigRemove(rmlObject)rmlObject = nilendend
endfunction sysCall_cleanup()-- 在清理阶段，确保移除Ruckig对象if rmlObject thensim.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})
endfunction sysCall_sensing()-- 获取关节速度并将其转换为度/秒sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
endfunction sysCall_actuation()-- 如果Ruckig对象存在，则执行一步运动规划if rmlObject thenlocal result, newPosVelAccel = sim.ruckigStep(rmlObject, sim.getSimulationTimeStep())-- 如果运动规划成功，则更新关节的位置if result ~= -1 thensim.setJointPosition(h, newPosVelAccel[1]) -- 或者根据关节模式使用 sim.setJointTargetPositionend-- 如果运动规划完成或出错，则移除Ruckig对象if result == 1 or result == -1 thensim.ruckigRemove(rmlObject)rmlObject = nilendend
endfunction sysCall_cleanup()-- 在清理阶段，确保移除Ruckig对象if rmlObject thensim.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)
endfunction sysCall_sensing()-- 获取关节速度并将其转换为度/秒sim.setGraphStreamValue(graph, stream, sim.getJointVelocity(h) * 180 / math.pi)
endfunction sysCall_thread()-- 定义最大关节速度maxVel = 0.1 -- 弧度/秒-- 定义最大关节加速度maxAccel = 0.01 -- 弧度/秒^2-- 定义最大关节加加速度maxJerk = 0.001 -- 弧度/秒^3-- 通过设置关节的目标位置，触发关节的回调函数sim.setJointTargetPosition(h, 179 * math.pi / 180, {maxVel, maxAccel, maxJerk})
endfunction sysCall_joint(inData)-- 用户可以在这里自由实现回调函数-- 由于该函数没有返回任何值，调用将被重定向到场景层次结构的上层-- 并最终由主脚本中的默认关节回调函数处理
end