Control Robot Arm with ROS Topic

[MrHide]

Good morning, I have some doubts about the control of a Kuka robotic arm simulated on CoppeliaSim and controlled through Rviz. However, since the problem seems complex to me, I wanted first to simply try to send some ROS Topic to control the individual joints of the robot. The script I wrote is the following, however the robot doesn't execute the commands. Is it a script problem or is it possible that it created the urdf file wrong?

Code: Select all

function sysCall_init ()
       jointHandles={-1,-1,-1,-1,-1,-1}
       jointHandles[1]=sim.getObjectHandle('joint_1')
       jointHandles[2]=sim.getObjectHandle('joint_2')
       jointHandles[3]=sim.getObjectHandle('joint_3')
       jointHandles[4]=sim.getObjectHandle('joint_4')
       jointHandles[5]=sim.getObjectHandle('joint_5')
       jointHandles[6]=sim.getObjectHandle('joint_6')
    
    if simROS then
        print ("ROS INTERFACE WAS FOUND")
      
        --enable joint subscriber
        sub1=simROS.subscribe('/kukakr16/joint_1/ctrl', "std_msgs/Float64", "callback1")
        simROS.subscriberTreatUInt8ArrayAsString(sub1)
        sub2=simROS.subscribe('/kukakr16/joint_a2/ctrl', "std_msgs/Float64","callback2")
        simROS.subscriberTreatUInt8ArrayAsString(sub2)
        sub3=simROS.subscribe('/kukakr16/joint_3/ctrl', "std_msgs/Float64","callback3")
        simROS.subscriberTreatUInt8ArrayAsString(sub3)
        sub4=simROS.subscribe('/kukakr16/joint_4/ctrl', "std_msgs/Float64","callback4")
        simROS.subscriberTreatUInt8ArrayAsString(sub4)
        sub5=simROS.subscribe('/kukakr16/joint_5/ctrl', "std_msgs/Float64","callback5")
        simROS.subscriberTreatUInt8ArrayAsString(sub5)
        sub6=simROS.subscribe('/kukakr16/joint_6/ctrl', "std_msgs/Float64","callback6")
        simROS.subscriberTreatUInt8ArrayAsString(sub6)
        
    else 
        print ("ROS INTERFACE WAS NOT FOUND")
    end
end 

function callback1 (config)
    if config then
           sim.setJointTargetPosition(jointHandles[1],config.data)
    end
end
function callback2 (config)
    if config then
           sim.setJointTargetPosition(jointHandles[2],config.data)
    end
end
function callback3 (config)
    if config then
           sim.setJointTargetPosition(jointHandles[3],config.data)
    end
end
function callback4 (config)
    if config then
           sim.setJointTargetPosition(jointHandles[4],config.data)
    end
end
function callback5 (config)
    if config then
           sim.setJointTargetPosition(jointHandles[5],config.data)
    end
end
function callback6 (config)
    if config then
           sim.setJointTargetPosition(jointHandles[6],config.data)
    end
end
function callback7 (config)
    if config then
           sim.setJointTargetPosition(gripper_roll_handle,config.data)
    end
end

[coppelia]

Hello,

can you confirm that the callbacks are effectively called?

Cheers

[MrHide]

I tried to put a print in the callback functions and I don't actually get anything printed. It seems strange to me because I tried the same algorithm for another robot (made with simple shapes) and it worked.

[coppelia]

Well, then you need to check what the differences are, and if some error messages are being printed..

Cheers

[MrHide]

Currently, after some changes to the kukakr16.urdf file and the published algorithm, there are the following problems:
- The robot collapses to the ground. This problem concerns in particular the coupling_2 which makes the whole arm move vertically. Unfortunately I can't put an image of the simulator otherwise I would show it to you.

- Now the arguments are not generated contrary to some previous attempts, where the problem was that the V-REP was not among the subscribers of the arguments created in the algorithm (verified with rostopic information).

I really don't understand where I'm wrong, and yet it seems to me a simple algorithm besides working for another robot.

PS: is it possible to control a robot defined with an .urdf file in RVIZ and a robot defined with a .ttm file in VREP?

[coppelia]

If the robot collapses to the ground this means your model is ill-defined. Make sure to carefully follow the design considerations here. And maybe also this tutorial about building a clean simulation model.

URDF models do not always include dynamic aspects, and/or are meant to be run within a different environment.

Also, in CoppeliaSim you most of the time have some script code that does the link between the model and the functionality. So object names (notably joint names) should always be specific, so that script code can access thos objects by querying their handles based on their names.

But in your case I am sure CoppeliaSim outputs a lot of errors, doesn't it? What are thos errors?

Cheers