Problems about sim.setObjectPosition for LineTracer

[lwang87]

Hello!

I want to achieve that after my LineTracer (a car that dragged from a menu of Model Browser on the left of Copprliasim Edu 4.1.0 interface) collide with a wall, then set its position to a place(by sim.setObjectPosition), and continue to drive it. btw... I drive my car with a external Joystick.

And, I've searched from this Forum and got that I should use code as following:

Code: Select all

	    sim.setThreadAutomaticSwitch(false)
                sim.getObjectsInTree(lineTracer,sim.handle_all,0)
                sim.resetDynamicObject(sim.handle_all)
                sim.setObjectPosition(lineTracer,-1,{0.66924,-0.050004,0.027544})
                sim.setObjectOrientation(lineTracer,-1,{0,8.998,-0.018})            
            sim.setThreadAutomaticSwitch(true)

and my code start from collision to the place setting is:

Code: Select all

local current_state_2=sim.checkCollision(car_body,stop)        
        if(current_state_2==1)then 
            dialog_H=sim.displayDialog('Congratulations','You finish the task!', sim.dlgstyle_message, true)
            sleep(3)
            sim.setThreadAutomaticSwitch(false)
                sim.getObjectsInTree(lineTracer,sim.handle_all,0)
                sim.resetDynamicObject(sim.handle_all)
                sim.setObjectPosition(lineTracer,-1,{0.66924,-0.050004,0.027544})
                sim.setObjectOrientation(lineTracer,-1,{0,8.998,-0.018})            
            sim.setThreadAutomaticSwitch(true)
        end 

I did achieved it, and I can also drive it by the joystick, but my car that set to the place I input, will keep shaking that I can't control.

When I use a new scene with only a car and a wall, and input my code mentioned above. after the car back to the place I set, it will shake, or turn over, or have other strange behavior. And I don't know how to solve it, please help me!

[coppelia]

Hello,

the above code to reset or teleport a dynamic model is wrong. Following is the correct way of doing this:

In the initialization phase of the script, make sure to memorize the relative poses of all objects in the model. Additionally, you could also memorize the joint states, etc.:

Code: Select all

    -- Init phase: memorize the object poses, etc.:
    -----------------------------------------------
    sim.setThreadAutomaticSwitch(false) -- in case this runs in a thread
    initialState={}
    local modelObjects=sim.getObjectsInTree(modelHandle,sim.handle_all,0)
    for i=1,#modelObjects,1 do
        local obj=modelObjects[i]
        local data={}
        data.localMatrix=sim.getObjectMatrix(obj,sim.handle_parent)
        -- e.g. you could also take into account the joints' initial states:
        --[[ 
        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)
    -----------------------------------------------

Then, if you need to teleport your dynamic model, use this:

Code: Select all

            -- Teleport the model to a new position/orientation:
            ----------------------------------------------------
            sim.setThreadAutomaticSwitch(false) -- in case this runs in a thread
            for handle, data in pairs(initialState) do
                if handle==modelHandle then
                    -- Set the model's new position/orientation:
                    sim.setObjectPosition(modelHandle,-1,...)
                    sim.setObjectOrientation(modelHandle,-1,...)
                else
                    sim.setObjectMatrix(handle,sim.handle_parent,data.localMatrix)
                    -- e.g. you could also take into account the joints' initial states:
                    --[[
                    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)
            ----------------------------------------------------

Cheers

[coppelia]

You can find a demo scene related to dynamic model reset and/or teleportation in CoppeliaSim V4.2.0+ (in scenes/teleportDynamicModel.ttt), or here (requires CoppeliaSim V4.2.0).

Cheers

[lwang87]

You can find a demo scene related to dynamic model reset and/or teleportation in CoppeliaSim V4.2.0+ (in scenes/teleportDynamicModel.ttt), or here (requires CoppeliaSim V4.2.0).

Cheers

Appreciate for your sincere reply! The problem has been solved! Thank you so much! TOT

[lwang87]

Hello,

the above code to reset or teleport a dynamic model is wrong. Following is the correct way of doing this:

In the initialization phase of the script, make sure to memorize the relative poses of all objects in the model. Additionally, you could also memorize the joint states, etc.:

Code: Select all

    -- Init phase: memorize the object poses, etc.:
    -----------------------------------------------
    sim.setThreadAutomaticSwitch(false) -- in case this runs in a thread
    initialState={}
    local modelObjects=sim.getObjectsInTree(modelHandle,sim.handle_all,0)
    for i=1,#modelObjects,1 do
        local obj=modelObjects[i]
        local data={}
        data.localMatrix=sim.getObjectMatrix(obj,sim.handle_parent)
        -- e.g. you could also take into account the joints' initial states:
        --[[ 
        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)
    -----------------------------------------------

Then, if you need to teleport your dynamic model, use this:

Code: Select all

            -- Teleport the model to a new position/orientation:
            ----------------------------------------------------
            sim.setThreadAutomaticSwitch(false) -- in case this runs in a thread
            for handle, data in pairs(initialState) do
                if handle==modelHandle then
                    -- Set the model's new position/orientation:
                    sim.setObjectPosition(modelHandle,-1,...)
                    sim.setObjectOrientation(modelHandle,-1,...)
                else
                    sim.setObjectMatrix(handle,sim.handle_parent,data.localMatrix)
                    -- e.g. you could also take into account the joints' initial states:
                    --[[
                    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)
            ----------------------------------------------------

Cheers

Hello, thanks for your answer, it helped a lot to me! And I have a new problem TOT. I've achieved to reset the position. But the car will keep moving forward without my control, until a reminding window disappear. Actually, I want to set 9 different situation, and I use a "for loop" to achieve it. each time when my car was reset, there would be a new situation, which is reminded from a dialog display. I want to know why my car is out of control(means keep forward) before the reminding dialog disappear when it is reset to the position, and how to solve it??

[coppelia]

not sure what dialog you refer to. Please post a minimalistic, self-contained version of your scene that illustrates the problem.

If you reset your model, you can/should maybe also reset the motors, e.g. by setting the velocity to zero, e.g. sim.setJointTargetVelocity(jointHandle,0) (if your joint is dynamically enabled, and the control loop not enabled).

Cheers