Paths

A path is a pseudo object, representing a succession of points with orientation in space. pseudo object, since it is built using merely dummies and a customization script that describes its functionality and behaviour.

[Simple path containing 4 control points]


Path objects can be added to the scene with [Add > Path]. A path is composed by control points that define its curve in space. Control points can be shifted, copy/pasted or deleted. A path's basic properties are accessed and adjusted via its user parameters (implemented via a user config callback function), in the scene hierarchy:

[User parameter icon]


A path can also automatically generate extruded shapes; this functionality is enabled via its user parameters, and the shape profile, color and other details can be adjusted in its customization script, which uses the API function sim.generateShapefromPath:

--lua function path.shaping(path, pathIsClosed, upVector) -- following section generates a square extrusion shape: local section = {0.02, -0.02, 0.02, 0.02, -0.02, 0.02, -0.02, -0.02, 0.02, -0.02} local color={0.7, 0.9, 0.9} local options = 0 if pathIsClosed then options = options | 4 end local shape = sim.generateShapeFromPath(path, section, options, upVector) sim.setShapeColor(shape, nil, sim.colorcomponent_ambient_diffuse, color) return shape end

[Simple path generating an extruded square shape]


To generate an extruded circular shape, use following code to create the section data:

--lua local section = {} local radius = 0.02 local sides = 32 local da = math.pi * 2 / sides for i = 0, sides - 1 do section[2 * i + 1] = radius * math.cos(da * i) section[2 * i + 2] = radius * math.sin(da * i) end -- the section shoujld be closed (first and last points perfect overlap): section[#section + 1] = section[1] section[#section + 1] = section[2]

[Simple path generating an extruded circular shape]


Path data is stored inside of the path object, as a custom data property. It can be accessed with:

#python # control point data (each one has x, y, z, qx, qy, qz, qw (position and quaternion)): ctrlPts = sim.unpackDoubleTable(sim.getBufferProperty(pathHandle, 'customData.PATHCTRLPTS')) # path data (each one has x, y, z, qx, qy, qz, qw (position and quaternion)): pathData = sim.unpackDoubleTable(sim.getBufferProperty(pathHandle, 'customData.PATH')) --lua -- control point data (each one has x, y, z, qx, qy, qz, qw (position and quaternion)): local ctrlPts = sim.unpackDoubleTable(sim.getBufferProperty(pathHandle, 'customData.PATHCTRLPTS')) -- path data (each one has x, y, z, qx, qy, qz, qw (position and quaternion)): local pathData = sim.unpackDoubleTable(sim.getBufferProperty(pathHandle, 'customData.PATH')

Various API functions related to paths are available, e.g. in order to have an object follow a path in position and orientation, one could use following script:

#python import numpy as np def sysCall_init(): sim = require('sim') self.objectToFollowPath = sim.getObject('..') self.path = sim.getObject('/Path') pathData = sim.unpackDoubleTable(sim.getBufferProperty(self.path, 'customData.PATH')) m = np.array(pathData).reshape(len(pathData) // 7, 7) self.pathPositions = m[:, :3].flatten().tolist() self.pathQuaternions = m[:, 3:].flatten().tolist() self.pathLengths, self.totalLength = sim.getPathLengths(self.pathPositions, 3) self.velocity = 0.04 # m/s self.posAlongPath = 0 self.previousSimulationTime = 0 sim.setStepping(True) def sysCall_thread(): while not sim.getSimulationStopping(): t = sim.getSimulationTime() self.posAlongPath += self.velocity * (t - self.previousSimulationTime) self.posAlongPath %= self.totalLength pos = sim.getPathInterpolatedConfig(self.pathPositions, self.pathLengths, self.posAlongPath) quat = sim.getPathInterpolatedConfig(self.pathQuaternions, self.pathLengths, self.posAlongPath, None, [2, 2, 2, 2]) sim.setObjectPosition(self.objectToFollowPath, pos, self.path) sim.setObjectQuaternion(self.objectToFollowPath, quat, self.path) self.previousSimulationTime = t sim.step() --lua function sysCall_init() sim = require('sim') objectToFollowPath = sim.getObject('..') path = sim.getObject('/Path') pathData = sim.unpackDoubleTable(sim.getBufferProperty(path, 'customData.PATH')) local m = Matrix(#pathData // 7, 7, pathData) pathPositions = m:slice(1, 1, m:rows(), 3):data() pathQuaternions = m:slice(1, 4, m:rows(), 7):data() pathLengths, totalLength = sim.getPathLengths(pathPositions, 3) velocity = 0.04 -- m/s posAlongPath = 0 previousSimulationTime = 0 sim.setStepping(true) end function sysCall_thread() while not sim.getSimulationStopping() do local t = sim.getSimulationTime() posAlongPath = posAlongPath + velocity * (t - previousSimulationTime) posAlongPath = posAlongPath % totalLength local pos = sim.getPathInterpolatedConfig(pathPositions, pathLengths, posAlongPath) local quat = sim.getPathInterpolatedConfig(pathQuaternions, pathLengths, posAlongPath, nil, {2, 2, 2, 2}) sim.setObjectPosition(objectToFollowPath, pos, path) sim.setObjectQuaternion(objectToFollowPath, quat, path) previousSimulationTime = t sim.step() end end