How to stop robot motion

Typically: "How do I... ", "How can I... " questions
Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

Post by Leizah »

Hello again, thank you for your answer.

I've looked at the demo you suggested and I think I got how rml works for a simple environment.
Unfortunately, I'm still not able to merge rml with the IK that I'm actually using.
Which part of the robot I've to set as rml object? The tip or all the joints?
In the demo you suggested both the cube and the joint start from a position {0,0,0} and reach a position {x,y,z}. I could find the start position of the tip (using the coordinates of the second control point of the path) or joints (using sim.getJointposition), but I don't know how to set the final pose.
The tip needs both info about position and orientation (And I don't know how to set the orientation with rml), while for the final position of joints I've no clue.
Since I just need the sim.rmlVel, how can I set the robot to continue to follow the IK path?

I'm sorry stress you, but I've to finalize my master thesis work in a week.
Thank you again for your support

Cheers.

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

Re: How to stop robot motion

Post by coppelia »

With the RML libraries you can generate smooth movements of 1-n DoFs. Those DoFs can be synchronized.

So if you wanted to move in 3D space, just in position, then you'd have a 3DoFs movement, where you need to specify the start pos (x1,y1,z1), start vel (e.g. (0,0,0)), start accel (e.g. (0,0,0)), a max. velocity (vx,vy,vz) a max. accel. (ax,ay,az), a goal pos (x2,y2,z2) and a goal vel. (vx2,vy2,vz2). If you have the RML library control an object that is the target object of an IK task, then the joints of the robot would follow.

If your movement need to be in 3D space and include orientation, then it get more tricky (have a look at how the sim.moveToPose function is implemented in lua/sim.lua

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

Post by Leizah »

Thank you for your reply.

The movetoPose command is already present into the movetoPose_viaIK function that I used to generate the IK path, but I was not able to make it work for my case.

However, I changed my code according to the demo, since the tip is a dummy point, I made the following code:

Code: Select all

    -- Human starts to approach the robot
   sim.setIntegerSignal('Robot_Working',2)
      -- Get tip position
      TipPosition=sim.getObjectPosition(simTip,-1)
      -- Get tip velocity
      TipVelocity=sim.getObjectVelocity(simTip)
      
      local currentPosVelAccel={TipPosition[1],TipPosition[2],TipPosition[3],TipVelocity[1],TipVelocity[2],TipVelocity[3],maxIkAccel[1],maxIkAccel[2],maxIkAccel[3]}
      local maxVelAccelJerk={TipVelocity[1],TipVelocity[2],TipVelocity[3],maxIkAccel[1],maxIkAccel[2],maxIkAccel[3],maxIkJerk[1],maxIkJerk[2],maxIkJerk[3]}
      local maxAccelJerk={maxIkAccel[1],maxIkAccel[2],maxIkAccel[3],maxIkJerk[1],maxIkJerk[2],maxIkJerk[3]}
      -- targetPos = Third IK target point coordinates
      local targetPosVel={Target_3[1],Target_3[2],Target_3[3],maxIkVel[1],maxIkVel[2],maxIkVel[3]}
      -- Set the minimum speed
      local targetVel={minIkVel[1],minIkVel[2],minIkVel[3]}
      
      local lb=sim.setThreadAutomaticSwitch(false)
      -- HumanInDanger = 0 when the human is outside the robot area, 1 when it is inside the robot area
      result=HumanInDanger
      
      while result==0 do
      
            rmlObject=sim.rmlPos(3,0.001,-1,currentPosVelAccel,maxVelAccelJerk,{1,1,1},targetPosVel)
            result,newPosVelAccel=sim.rmlStep(rmlObject,sim.getSimulationTimeStep())
            -- The human is outside the area, move at the same speed
            sim.setObjectPosition(rmlObject,-1,newPosVelAccel)
            
            sim.switchThread()
            
               -- The human enters the robot area
               if result==1 then
                  sim.rmlRemove(rmlObject1)
                  
                  -- Change speed
                  rmlObject=sim.rmlVel(3,0.001,-1,currentPosVelAccel,maxAccelJerk,{1,1,1},targetVel)
                  
                  result,newPosVelAccel=sim.rmlStep(rmlObject,sim.getSimulationTimeStep())
                  sim.setObjectPosition(rmlObject,-1,newPosVelAccel)
                  
               end
                  sim.switchThread()
       end
       
       sim.setThreadAutomaticSwitch(lb)
       
       sim.rmlRemove(rmlObject)
    end
Unfortunately, when the robot arrives to the second point of the IK (The one when the rml option should trigger) it simply stops.

What am I missing?

Cheers

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

Re: How to stop robot motion

Post by coppelia »

You are mixing up sim.rmlPos and sim.rmlVel: you should use one or the other. But don't mix them.
The first is to plan a movement from a position A to a position B
The second one is the plan a movement from a velocity A to a velocity B

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

Post by Leizah »

Thank you for your answer, I'll try using RmlPos.
However, using the same code it returns me an error "Incorrect table size" for the first rmlPos function.
The tip is a dummy, so I set the value of 3 DoF (As into the example) and then I considered the following vectors:

CurrentPosVelAccel = {TipPosition (1x3 vector), TipLinearVelocity(1x3 vector), MaxIkAccel(1x3 vector)

MaxVelAccelJerk = {TipLinearVelocity(1x3 vector), MaxIkAccel(1x3 vector, MaxIkJerk(1x3 vector)}

targetPosVel = {TargetPos(1x3 vector),MaxIkVel(1x3 vector}

What is wrong with these tables?

Cheers

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

Re: How to stop robot motion

Post by coppelia »

You should use flat tables, e.g. instead of:

Code: Select all

CurrentPosVelAccel = { {px,py,pz}, {vx,vy,vz}, {ax,ay,az} }
do:

Code: Select all

CurrentPosVelAccel = { px,py,pz, vx,vy,vz, ax,ay,az }
Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

Post by Leizah »

Hi, thank you for your answer, it actually solved the error in Coppelia.
Unfortunately the code stil doesn't work, but I don't understand why.
This is the code I modified according to your last advices:

Code: Select all

 -- Robot start condition
if Robot_Start then
        -- First part of the movement performed with IK
        sim.setJointTargetVelocity(simJoints[1],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[2],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[3],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[4],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[5],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[6],maxIkVel[1])
        moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_1,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_2,data)
        
        -- Human restart condition
        sim.setIntegerSignal('Robot_Working',2)
        
              -- Get tip position
              TipPosition=sim.getObjectPosition(simTip,-1)
              -- Get tip velocity
              TipLinearVelocity, TipAngularVelocity=sim.getObjectVelocity(simTip)
              
              local currentPosVelAccel={TipPosition[1],TipPosition[2],TipPosition[3],TipLinearVelocity[1],TipLinearVelocity[2],TipLinearVelocity[3],maxIkAccel[1],maxIkAccel[2],maxIkAccel[3]}
              
              local maxVelAccelJerk={TipLinearVelocity[1],TipLinearVelocity[2],TipLinearVelocity[3],maxIkAccel[1],maxIkAccel[2],maxIkAccel[3],maxIkJerk[1],maxIkJerk[2],maxIkJerk[3]}
              
              local maxAccelJerk={maxIkAccel[1],maxIkAccel[2],maxIkAccel[3],maxIkJerk[1],maxIkJerk[2],maxIkJerk[3]}
              -- TargetPosVel when result = 0
              local targetPosVel={Target_3[1],Target_3[2],Target_3[3],maxIkVel[1],maxIkVel[2],maxIkVel[3]}
              -- TargetPosVel when result = 1
              local targetPosVel_min={Target_3[1],Target_3[2],Target_3[3],minIkVel[1],minIkVel[2],minIkVel[3]}
              
              local lb=sim.setThreadAutomaticSwitch(false)
              
              -- SafetyZoneresult is the return value of the proximity sensor (1 when Bill is inside the area, 0 when it isn't)
              if SafetyZoneresult>0 then
                result=0
                else
                result=1
                end
              
              -- RML condition (Same as the demos)
              while result==0 do
                    rmlObject=sim.rmlPos(3,0.001,-1,currentPosVelAccel,maxVelAccelJerk,{1,1,1},targetPosVel)
                    result,newPosVelAccel=sim.rmlStep(rmlObject,sim.getSimulationTimeStep())
                    sim.switchThread()
                    sim.setObjectPosition(rmlObject,-1,newPosVelAccel)
                       if result==1 then
                          sim.rmlRemove(rmlObject)
                          
                          rmlObject=sim.rmlPos(3,0.001,-1,currentPosVelAccel,maxAccelJerk,{1,1,1},targetPosVel_min)
                          
                          result,newPosVelAccel=sim.rmlStep(rmlObject,sim.getSimulationTimeStep())
                          sim.setObjectPosition(rmlObject,-1,newPosVelAccel)
                       end
                          sim.switchThread()
                          sim.rmlRemove(rmlObject)
               end
               sim.setThreadAutomaticSwitch(lb)
            end
The robot reaches the second IK point (The one where it sets the signal for the human) and then stops (Does not stop the simulation, simply doesn't move anymore).
I'm sorry to bother, but I still do not get why it is not working.

Cheers

Post Reply