【仿真】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)
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