## 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

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
Posts: 8989
Joined: 14 Dec 2012, 00:25

### Re: How to stop robot motion

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

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,TipPosition,TipPosition,TipVelocity,TipVelocity,TipVelocity,maxIkAccel,maxIkAccel,maxIkAccel}
local maxVelAccelJerk={TipVelocity,TipVelocity,TipVelocity,maxIkAccel,maxIkAccel,maxIkAccel,maxIkJerk,maxIkJerk,maxIkJerk}
local maxAccelJerk={maxIkAccel,maxIkAccel,maxIkAccel,maxIkJerk,maxIkJerk,maxIkJerk}
-- targetPos = Third IK target point coordinates
local targetPosVel={Target_3,Target_3,Target_3,maxIkVel,maxIkVel,maxIkVel}
-- Set the minimum speed
local targetVel={minIkVel,minIkVel,minIkVel}

-- 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)

-- 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
end

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
Posts: 8989
Joined: 14 Dec 2012, 00:25

### Re: How to stop robot motion

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

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
Posts: 8989
Joined: 14 Dec 2012, 00:25

### Re: How to stop robot motion

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

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.

Code: Select all

 -- Robot start condition
if Robot_Start then
-- First part of the movement performed with IK
sim.setJointTargetVelocity(simJoints,maxIkVel)
sim.setJointTargetVelocity(simJoints,maxIkVel)
sim.setJointTargetVelocity(simJoints,maxIkVel)
sim.setJointTargetVelocity(simJoints,maxIkVel)
sim.setJointTargetVelocity(simJoints,maxIkVel)
sim.setJointTargetVelocity(simJoints,maxIkVel)
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,TipPosition,TipPosition,TipLinearVelocity,TipLinearVelocity,TipLinearVelocity,maxIkAccel,maxIkAccel,maxIkAccel}

local maxVelAccelJerk={TipLinearVelocity,TipLinearVelocity,TipLinearVelocity,maxIkAccel,maxIkAccel,maxIkAccel,maxIkJerk,maxIkJerk,maxIkJerk}

local maxAccelJerk={maxIkAccel,maxIkAccel,maxIkAccel,maxIkJerk,maxIkJerk,maxIkJerk}
-- TargetPosVel when result = 0
local targetPosVel={Target_3,Target_3,Target_3,maxIkVel,maxIkVel,maxIkVel}
-- TargetPosVel when result = 1
local targetPosVel_min={Target_3,Target_3,Target_3,minIkVel,minIkVel,minIkVel}

-- 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.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
end