How to control the robot to maintain a stable posture?

Typically: "How do I... ", "How can I... " questions
Post Reply
Shaoxiang Wang
Posts: 18
Joined: 23 May 2020, 15:39

How to control the robot to maintain a stable posture?

Post by Shaoxiang Wang »

Hello,

In the initial state I have adjusted the joint angle of the robot. Make the end of the manipulator face down. But when the manipulator moves, he cannot maintain a stable posture. It is possible to make the grasped object face the sky. How can he keep his posture unchanged and relatively stable?

Should I adjust the direction of Dummy, I don’t know?
The end of the robot moves along the path I have set. There is no dummy in the path, only the starting point and the ending point.

Image start position
Image during moving


best regards
shaoxiang

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: How to control the robot to maintain a stable posture?

Post by fferri »

How are you generating the motion?

Are you using sim.generateIkPath?

Shaoxiang Wang
Posts: 18
Joined: 23 May 2020, 15:39

Re: How to control the robot to maintain a stable posture?

Post by Shaoxiang Wang »

I used sim.getPositionOnPath, sim.getOrientationOnPath, sim.setObjectPosition, sim.setObjectOrientation.

Here is my code.

Code: Select all

    -- do some initialization here
    
-- Get handles and postions of dummies
    tip = sim.getObjectHandle("tip")
    ownHandle = sim.getObjectHandle("Target")
    
    startPath1=sim.getObjectHandle("Path1_1")
    startPath2=sim.getObjectHandle("Path1_2")
    mittelPath1=sim.getObjectHandle("Path2_1")
    mittelPath2=sim.getObjectHandle("Path2_2")
    endPath1=sim.getObjectHandle("Path3_1")
    endPath2=sim.getObjectHandle("Path3_2")
    pathLength1=sim.getPathLength(startPath1)
    pathLength2=sim.getPathLength(startPath2)
    pathLength3=sim.getPathLength(mittelPath1)
    pathLength4=sim.getPathLength(mittelPath2)
    pathLength5=sim.getPathLength(endPath1)
    pathLength6=sim.getPathLength(endPath2)

    speed=0.02 -- end-effector velocity in m/s
    sim.setUserParameter(ownHandle,'@enable','')
    timestep=sim.getSimulationTimeStep() -- in s
    
    currentPositionRelative1=0
    currentPositionRelative2=0
    currentPositionRelative3=0
    currentPositionRelative4=0
    currentPositionRelative5=0
    currentPositionRelative6=0
    moveForward=true
    epsilon1=0.01
    epsilon2=0.01
    epsilon3=0.01
    epsilon4=0.01
    epsilon5=0.01
    epsilon6=0.01

function sysCall_threadmain()
    -- Put some initialization code here

  while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
    slowerWork=sim.getStringSignal("slowerWork")
    if slowerWork=="1" then
        slowerWork=true
    else
        slowerWork=false
    end
    
    -- path1
    if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
        if currentPositionRelative1 < 1-epsilon1 and moveForward then
            currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
            pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
    else
        pathAdvanceAbsolute=0.2*speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
        if currentPositionRelative1 < 1-epsilon1 and moveForward then
            currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
            pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
    end
    
    -- path2
    if currentPositionRelative1 >= 1-epsilon1 then
       if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength2
        if currentPositionRelative2 < 1-epsilon2 and moveForward then
            currentPositionRelative2=currentPositionRelative2+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(startPath2,currentPositionRelative2)
            pathOrientation=sim.getOrientationOnPath(startPath2,currentPositionRelative2)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
       else
          pathAdvanceAbsolute=0.2*speed*timestep -- in m
          pathAdvanceRelative=pathAdvanceAbsolute/pathLength2
         if currentPositionRelative2 < 1-epsilon2 and moveForward then
            currentPositionRelative2=currentPositionRelative2+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(startPath2,currentPositionRelative2)
            pathOrientation=sim.getOrientationOnPath(startPath2,currentPositionRelative2)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
         end
       end
      
    end
    
    -- path3
    
    if currentPositionRelative2 >= 1-epsilon2 then
       if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength3
        if currentPositionRelative3 < 1-epsilon3 and moveForward then
            currentPositionRelative3=currentPositionRelative3+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(mittelPath1,currentPositionRelative3)
            pathOrientation=sim.getOrientationOnPath(mittelPath1,currentPositionRelative3)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
       else
          pathAdvanceAbsolute=0.2*speed*timestep -- in m
          pathAdvanceRelative=pathAdvanceAbsolute/pathLength3
         if currentPositionRelative3 < 1-epsilon3 and moveForward then
            currentPositionRelative3=currentPositionRelative3+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(mittelPath1,currentPositionRelative3)
            pathOrientation=sim.getOrientationOnPath(mittelPath1,currentPositionRelative3)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
         end
       end
      
    end
    
    -- path4
    
    if currentPositionRelative3 >= 1-epsilon3 then
       if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength4
        if currentPositionRelative4 < 1-epsilon4 and moveForward then
            currentPositionRelative4=currentPositionRelative3+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(endPath1,currentPositionRelative4)
            pathOrientation=sim.getOrientationOnPath(endPath1,currentPositionRelative4)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
       else
          pathAdvanceAbsolute=0.2*speed*timestep -- in m
          pathAdvanceRelative=pathAdvanceAbsolute/pathLength4
         if currentPositionRelative4 < 1-epsilon4 and moveForward then
            currentPositionRelative4=currentPositionRelative4+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(endPath1,currentPositionRelative4)
            pathOrientation=sim.getOrientationOnPath(endPath1,currentPositionRelative4)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
         end
       end
      
    end
    
    -- path5
    
    if currentPositionRelative4 >= 1-epsilon4 then
       if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength5
        if currentPositionRelative5 < 1-epsilon5 and moveForward then
            currentPositionRelative5=currentPositionRelative5+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(endPath2,currentPositionRelative5)
            pathOrientation=sim.getOrientationOnPath(endPath2,currentPositionRelative5)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
       else
          pathAdvanceAbsolute=0.2*speed*timestep -- in m
          pathAdvanceRelative=pathAdvanceAbsolute/pathLength5
         if currentPositionRelative5 < 1-epsilon5 and moveForward then
            currentPositionRelative5=currentPositionRelative5+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(endPath2,currentPositionRelative5)
            pathOrientation=sim.getOrientationOnPath(endPath2,currentPositionRelative5)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
         end
       end
      
    end
    
    -- path6
    
    if currentPositionRelative5 >= 1-epsilon5 then
       if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength6
        if currentPositionRelative6 < 1-epsilon6 and moveForward then
            currentPositionRelative6=currentPositionRelative6+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(mittelPath2,currentPositionRelative6)
            pathOrientation=sim.getOrientationOnPath(mittelPath2,currentPositionRelative6)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
       else
          pathAdvanceAbsolute=0.2*speed*timestep -- in m
          pathAdvanceRelative=pathAdvanceAbsolute/pathLength6
         if currentPositionRelative6 < 1-epsilon6 and moveForward then
            currentPositionRelative6=currentPositionRelative6+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(mittelPath2,currentPositionRelative6)
            pathOrientation=sim.getOrientationOnPath(mittelPath2,currentPositionRelative6)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
         end
       end
      
    end
    
    if currentPositionRelative6 >= 1-epsilon6 then
      currentPositionRelative1=0
      currentPositionRelative2=0
      currentPositionRelative3=0
      currentPositionRelative4=0
      currentPositionRelative5=0
      currentPositionRelative6=0
    end
    
  end
end

function sysCall_cleanup()
    -- Put some clean-up code here
end

-- See the user manual or the available code snippets for additional callback functions and details

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: How to control the robot to maintain a stable posture?

Post by fferri »

is the orientation of the path control point correct?

Shaoxiang Wang
Posts: 18
Joined: 23 May 2020, 15:39

Re: How to control the robot to maintain a stable posture?

Post by Shaoxiang Wang »

Thanks a lot for your tips! I adjust the orientation of point in the path. Now it is better!

We can see ,the Z axis (blue axis)of TargetDummy face ground. But the tip(another blue axis) don't follow it. I don't know why .

Image
Image
Image

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: How to control the robot to maintain a stable posture?

Post by fferri »

Made a quick test using these functions and it seems to work as expected:

Image

Code: Select all

function sysCall_init()
    Path=sim.getObjectHandle('Path')
    manipSphere=sim.getObjectHandle('redundantRob_manipSphere')
end

function sysCall_actuation()
    local t=math.fmod(sim.getSimulationTime()*0.1,1)
    local p=sim.getPositionOnPath(Path,t)
    local o=sim.getOrientationOnPath(Path,t)
    sim.setObjectPosition(manipSphere,-1,p)
    sim.setObjectOrientation(manipSphere,-1,o)
end
make sure IK is not failing and the calculation includes alpha beta gamma angles.

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

Re: How to control the robot to maintain a stable posture?

Post by coppelia »

also: you are using a damped resolution method (DLS): it will take more iterations to converge to the desired end-effector position/orientation. Either reduce the damping factor, or use a non-damped resolution method (i.e. pseudo-inverse)

Cheers

Shaoxiang Wang
Posts: 18
Joined: 23 May 2020, 15:39

Re: How to control the robot to maintain a stable posture?

Post by Shaoxiang Wang »

Thank you very much! Now it works well! Thank!

Post Reply