I am having a Mobile platform that support a robotic arm which I would like to control the motion!
I am meant to set up the motion ( speed/direction .. ) in order to establish at the end the autonomous navigation and path planning model.
The mobile platform consists of 4 Wheels, each have a revolute joint in Force/Torque Mode.
I have written a function that from the wanted mobile platform linear/rotational speed, I derive each of the required wheel joint angular velocities as shows my code:
Code: Select all
-- Mobile Platform Inits Linear_Velocity = 1 -- Vm in m/s Rotational_Velocity = 0 -- Wm in °/s Wheels_Diameter = 0.12 -- d in m Wheels_Distance = 0.546 -- Distance between one left and right wheel in m Time_Of_Motion = 5 -- Movie the mobile for 5 seconds function MoveRobot(Vm,Wm,l,d) Wr = (Vm + Wm*l/2)/d Wl = (Vm - Wm*l/2)/d --simSetThreadSwitchTiming(50) simAddStatusbarMessage('Moving Robot ..') simSetExplicitHandling(IK_Clean_Group,1) simAddStatusbarMessage('IK desactivated') simSetJointTargetVelocity(Joint_Wheel_1,Wr) simSetJointTargetVelocity(Joint_Wheel_2,Wr) simSetJointTargetVelocity(Joint_Wheel_3,Wl) simSetJointTargetVelocity(Joint_Wheel_4,Wl) simWait(Time_Of_Motion) simAddStatusbarMessage('Stopping robot ..') simSetJointTargetVelocity(Joint_Wheel_1,-Wr) simSetJointTargetVelocity(Joint_Wheel_2,-Wr) simSetJointTargetVelocity(Joint_Wheel_3,-Wl) simSetJointTargetVelocity(Joint_Wheel_4,-Wl) --simSetExplicitHandling(IK_Clean_Group,0) simAddStatusbarMessage('IK Re activated again') end
When I run the simulation, the mobile platform motion is inconsistent! the direction isn't linear even though the 4 wheels are spinning with the same angular velocities!
When I leave the simulation for 20 seconds, the mobile platform seems to follow a circular path instead of a linear path!
Anybody have an idea how to fix this problem? Is it about the thread execution time? After all, of course, it is clear that I want that the 4 wheels start spinning at the same time!
V Rep model:
Thanks for any kind of support!