set model to a new position but it's orientation isn't correct

Typically: "How do I... ", "How can I... " questions
Post Reply
lwang87
Posts: 41
Joined: 24 May 2021, 16:47

set model to a new position but it's orientation isn't correct

Post by lwang87 »

Hello,
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)
--------------------------------------------------  
Then, when I need to set it's position, the code is:

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) 
It did succeed in setting the model to a new place. But the only problem is it's orientation is different from the initial state. Please help me solve this problem, thank you!

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: set model to a new position but it's orientation isn't correct

Post by coppelia »

Hello,

well, your code tries to set the new position/orientation of the model itself to something different from its initial state, i.e. your code contains:

Code: Select all

                            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
Leave above out and your model will be restored into the exact same initial position/orientation.

Cheers

lwang87
Posts: 41
Joined: 24 May 2021, 16:47

Re: set model to a new position but it's orientation isn't correct

Post by lwang87 »

coppelia wrote: 22 Jul 2021, 09:31 Hello,

well, your code tries to set the new position/orientation of the model itself to something different from its initial state, i.e. your code contains:

Code: Select all

                            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
Leave above out and your model will be restored into the exact same initial position/orientation.

Cheers
Thanks for your reply. I'm sorry, maybe my description was a little wrong. I want to set my model to a new position, but it's position/orientation is different from what I wrote at the script. For example, I wrote sim.setObjectOrientation(lineTracer,-1,{-90,0,-90}) , but when I simulate it, the model's orientation is actually {90.1, -20, 90.2}, The number in the middle is very different from what I set. I want to know why, and How to solve this problem?

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: set model to a new position but it's orientation isn't correct

Post by coppelia »

Keep in mind that API functions expect angles in radians, not degrees. So do this instead:

Code: Select all

sim.setObjectOrientation(lineTracer,-1,{-math.pi/2,0,-math.pi/2})
Cheers

lwang87
Posts: 41
Joined: 24 May 2021, 16:47

Re: set model to a new position but it's orientation isn't correct

Post by lwang87 »

It helps! Appreciate!

lwang87
Posts: 41
Joined: 24 May 2021, 16:47

Re: set model to a new position but it's orientation isn't correct

Post by lwang87 »

And, could you please check https://forum.coppeliarobotics.com/view ... f=9&t=9321 and express your thoughts about this question? I think this is also a common problem in terms of data acquisition and recording. Thank you!

Post Reply