I've succeeded in setting my model to a new position. But it's orientation was different from it's initial state. For example, it's initial beta[deg] is 0, after it get to the new place, it's beta[deg] will change to -0.16734. The code is following:
firstly, memorize the initial state:
Code: Select all
------------ memorize the object poses
-----------------------------------------------
sim.setThreadAutomaticSwitch(false) -- in case this runs in a thread
initialState={}
local modelObjects=sim.getObjectsInTree(lineTracer,sim.handle_all,0)
for i=1,#modelObjects,1 do
local obj=modelObjects[i]
local data={}
data.localMatrix=sim.getObjectMatrix(obj,sim.handle_parent)
data.localPose=sim.getObjectPosition(obj,sim.handle_parent)
data.localOrie=sim.getObjectOrientation(obj,sim.handle_parent)
if sim.getObjectType(obj)==sim.object_joint_type then
if sim.getJointType(obj)==sim.joint_spherical_subtype then
data.jointM=sim.getJointMatrix(obj)
else
data.jointPos=sim.getJointPosition(obj)
end
end
initialState[obj]=data
end
sim.setThreadAutomaticSwitch(true)
--------------------------------------------------
Code: Select all
sim.setThreadAutomaticSwitch(false) -- in case this runs in a thread
for handle, data in pairs(initialState) do
if handle==lineTracer then -- Set the model's new position/orientation:
sim.setObjectPosition(lineTracer,-1,{X[b+1][2],0,0.0275})
sim.setObjectOrientation(lineTracer,-1,{-90,0,-90})
else
sim.setObjectMatrix(handle,sim.handle_parent,data.localMatrix)
if data.jointPos then
sim.setJointPosition(handle,data.jointPos)
end
if data.jointM then
sim.setSphericalJointMatrix(handle,data.jointM)
end
end
sim.resetDynamicObject(handle)
end
sim.setThreadAutomaticSwitch(true)