Robot Arm Constrained Path Following Problem.

Typically: "How do I... ", "How can I... " questions
ag_raed
Posts: 11
Joined: 04 Jul 2017, 14:54

Robot Arm Constrained Path Following Problem.

Postby ag_raed » 05 Jul 2017, 15:41

Hello people,

This is my first days using V-Rep, and I am meant to model Dynamical/Kinematical behavior of a robot arm that shall clean a wall, in other meaning, its End-Effector tool shall mate the wall and perform a given path.
( Just like this from a CAD perspective view )
https://ibb.co/mw1CQF

For now, I was able to create the complete model, including all the IK, Collision Detection, Sensor implementation and Path Planning tasks and the controller algorithms in embedded Lua script and it works fine ( I have also written a Remote Matlab code to interact with the V-Rep environment but it is still unable to read correct joint position value, neither be able to control the joints).

But

My Problem is:



- I still not able to perform the task with the constraint: End-Effector tool mating the Wall.
I tried the Geometrical Const Solver by coinciding a Reference Plane created on the EE-Tool surface and the Wall but didn't work.
https://ibb.co/nPLE5F
Somebody have an idea how to deal with this ?

- I am also getting incorrect values from the ForceTorque sensor.

- I am trying the output in the V-Rep Bar Command the instantaneous distance between the EE-tool and the Wall but I am getting the value 1 all the time, couldn't understand the reason. ( same problem when I used my Matlab code )

My Lua-Script is:

Code: Select all

threadFunction=function()
   -- Put your thread code here (initialization and clean-up code should not be in here

-- Setting the ranges! (Roll is ignored as it is a cyclic joint )
simExtRemoteApiStart(19999)

simSetJointInterval(Joint_Base_Yaw,false,{min_base_yaw,max_base_yaw-min_base_yaw})
simSetJointInterval(Joint_Wrist_Yaw,false,{min_wrist_yaw,max_wrist_yaw-min_wrist_yaw})

simSetJointInterval(Joint_Pitch_1,false,{min_pitch1,max_pitch1-min_pitch1})
simSetJointInterval(Joint_Pitch_2,false,{min_pitch2,max_pitch2-min_pitch2})
simSetJointInterval(Joint_Pitch_3,false,{min_pitch3,max_pitch3-min_pitch3})
simSetJointInterval(Joint_Pitch_4,false,{min_pitch4,max_pitch4-min_pitch4})

-- Moving the EE to the desired position using IK
Target_position = simGetObjectPosition(Target,-1)
LeftWall_Position = simGetObjectPosition(Wall_Left,-1)

simMoveToPosition(Target,-1,desired_EE_position,nil,desired_EE_velocity)

simWait(1)

pathLength = simGetPathLength(S_Path_1)
posOnPath = 0

PathPos_0 = simGetPositionOnPath(S_Path_1,0)
simMoveToPosition(Target,-1,PathPos_0,nil,desired_EE_velocity)
simAddStatusbarMessage('base yaw pos is  '..tostring(simGetJointPosition(Joint_Base_Yaw)))

simWait(3)

while true do

   l=posOnPath/pathLength
   if (l>1) then
      l=1
   end

simAddStatusbarMessage('EE_LeftWall =  '..tostring(simReadDistance(Distance_EE_LeftWall)))

   position=simGetPositionOnPath(S_Path_1,l)
   orientation=simGetOrientationOnPath(S_Path_1,l)

   simSetObjectPosition(Target,-1,position)
   simSetObjectOrientation(Target,-1,orientation)

   posOnPath= posOnPath + desired_EE_velocity*simGetSimulationTimeStep()

   simSwitchThread()

end

end



-- Put some initialization code here:
simSetThreadSwitchTiming(2) -- Default timing for automatic thread switching
-- Defining range here
min_base_yaw, max_base_yaw = -120,120
min_wrist_yaw, max_wrist_yaw = -90,90

min_pitch1, max_pitch1 = -10,80
min_pitch2, max_pitch2 = -30,80
min_pitch3, max_pitch3 = -35,80
min_pitch4, max_pitch4 = -90,90

-- Objects Inits

Target =simGetObjectHandle('Target')
Wall_Left = simGetObjectHandle('Wall_Left')
Robot_Platform =simGetObjectHandle('Rigide_Mobile_Platform_')
S_Path = simGetObjectHandle('S_Path')
S_Path_1 = simGetObjectHandle('S_Path_1')
Distance_EE_LeftWall = simGetDistanceHandle('Distance_EE_LeftWall_')

-- Joints Inits
Joint_Base_Yaw = simGetObjectHandle('Joint_Base_Yaw')
Joint_Wrist_Yaw = simGetObjectHandle('Joint_Wrist_Yaw')

Joint_Pitch_1 = simGetObjectHandle('Joint_Pitch_1')
Joint_Pitch_2 = simGetObjectHandle('Joint_Pitch_2')
Joint_Pitch_3 = simGetObjectHandle('Joint_Pitch_3')
Joint_Pitch_4 = simGetObjectHandle('Joint_Wrist_Pitch')
-- Joints current pos


CorrectionCoeff = 0.697
X_EE_Position = -1.5; Y_EE_Position = -0.616; Z_EE_Position = 1.066;
 
desired_EE_position = {X_EE_Position,Y_EE_Position,Z_EE_Position} -- Move the EE to the 2,2,2 m from the World Ref Frame
desired_EE_velocity = 0.3 -- End Effector Velocity m/s

--simAddPointCloud()

-- Here we execute the regular thread code:
res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
   simAddStatusbarMessage('Lua runtime error: '..err)
end

-- Put some clean-up code here:


My V-Rep Model is:
https://ufile.io/w0809

Thank you in advance for any support.

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

Re: Robot Arm Constrained Path Following Problem.

Postby coppelia » 06 Jul 2017, 16:37

Hello,

try deactivating your threaded child script first, then run your simulation. You will notice that the manipulator converges very slowly to the target position/orientation. This is because you selected the DLS method with a large damping factor. Reduce the damping factor, or select a non-damped resolution method for your IK group. Then manually move the target around, and also rotate it: the manipulator will correctly follow your target.
When you move your target along the path, you should make sure the target is correctly oriented, otherwise the manipulator won't be able to follow. (i.e. do not use the orientation that comes from the path, but compute it yourself).

Cheers

ag_raed
Posts: 11
Joined: 04 Jul 2017, 14:54

Re: Robot Arm Constrained Path Following Problem.

Postby ag_raed » 11 Jul 2017, 14:18

Thanks for the answer!

Indeed, by setting up the damping factor to 0.1 and deactivating the child script, I could find the proper Target orientation! ( which should be gamma = 90 so that the end-effector mate the wall! )

But when I have implemented it in the code, It didn't work perfectly, In my function FollowPath(path):

Code: Select all

function FollowPath(Path)

simAddStatusbarMessage('Movin to Initial Path Position')
pathLength = simGetPathLength(Path)
posOnPath = 0

PathPos_0 = simGetPositionOnPath(Path,0) -- this is the entry position of the path
simMoveToPosition(Target,-1,PathPos_0,nil,desired_EE_velocity)
simSetObjectOrientation(Target,-1,{0,0,math.pi/2})

simAddStatusbarMessage(simGetJointPosition(Joint_Base_Yaw))

simAddStatusbarMessage('Following S Path after 3s')
simWait(3)

while true do

   l=posOnPath/pathLength
   if (l>1) then
      l=1
   end

--simAddStatusbarMessage('EE_LeftWall = '..simReadDistance(Distance_EE_LeftWall)))

   position=simGetPositionOnPath(Path,l)
   orientation=simGetOrientationOnPath(Path,l)

   simSetObjectPosition(Target,-1,position)
   simSetObjectOrientation(Target,-1,{0,0,math.pi/2})

   posOnPath= posOnPath + desired_EE_velocity*simGetSimulationTimeStep()

   simSwitchThread()

end


end


You can see, that the on every loop step on the path, I am setting the Target orientation to Gamma = 90 ( pi/2 ), It has almost worked!

But on some points on the path, the Target orientation changes from 90° and the constraint EE-Wall mating is violated!

I tried to add Collision-Avoidance as a parameter but it didn't work! ( Between the EE and Wall )

Do you think the reason for this is just simply: No solution exists! Or there should be further mathematical conditions to add? Coz in CAD Mechanisms it is shown that the solution exists!

Thanks for any support!

ttt Model:
https://www.file-upload.com/75z621imvfqg

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

Re: Robot Arm Constrained Path Following Problem.

Postby coppelia » 13 Jul 2017, 12:26

Your manipulator is still not following the path in a clean manner. A damped resolution is normally not needed, also because you have enough degrees of freedom (7 in your manipulator).
Also, you have some elements of your manipulator that are dynamically enabled, others that are not dynamically enabled. For instance you have dynamic items built on top of static items, themselves built on top of dynamic items. Do not mix-up everything.

I highly recommend that you have a look at the various manipulator models available in the V-REP library, and how they have been built and how the operate. Also, make sure to very carefully read this page.

Cheers

ag_raed
Posts: 11
Joined: 04 Jul 2017, 14:54

Re: Robot Arm Constrained Path Following Problem.

Postby ag_raed » 17 Jul 2017, 16:37

Hello,

Thanks for the suggestions!

Why I actually build the model on that way is that at the beginning when I wanted to build a Dynamically enabled model in which the joints are acting in Hybrid mode, the arm fall to the ground when I start the simulation, even though the joints are set in Hybrid mode (IK) !!



All I want to do is:
- Having the robot arm in IK mode in which in its End effector is mounted an FT Sensor that measures the Force/Torque response when the end-effector touch the wall.
- Having the robot arm mounted on top of a controlled mobile platform that consists of 4 wheels! ( I will solve this problem once I have a clean model )

I think I have followed exactly youBotAndHanoiTower.ttm scene! but still, I can't figure out what is going on! I even tried to follow the "How to build a clean model" and the " How to perform dynamic simulation" but:

Problem:

- Joints aren't dynamically enabled and the arm falls during the simulation!

Scene
https://ufile.io/0f4vh

Thanks for Any help!

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

Re: Robot Arm Constrained Path Following Problem.

Postby coppelia » 17 Jul 2017, 20:58

Your should carefully read this page, about dynamically enabled joints. In your case, you have situations like:

Code: Select all

joint --> shape
      --> joint


Which is not allowed for dynamically enabled joints (i.e. you need a dynamically enabled shape in-between). So something like:

Code: Select all

joint --> shape --> joint


Cheers

ag_raed
Posts: 11
Joined: 04 Jul 2017, 14:54

Re: Robot Arm Constrained Path Following Problem.

Postby ag_raed » 18 Jul 2017, 12:52

Hello!

Thanks it works now! ( Model Dynamiquely enable correctly mating a position on the wall and FT sensor getting consistent data ).
I will try to solve the mobile platform controller problem and the path following problem.


Return to “General questions”

Who is online

Users browsing this forum: Bing [Bot], Google [Bot] and 10 guests