Trajectory Following for a Manipulator

Typically: "How do I... ", "How can I... " questions
Post Reply
mechatronics_eng
Posts: 2
Joined: 18 May 2020, 17:13

Trajectory Following for a Manipulator

Post by mechatronics_eng »

Hi everyone and sorry for my english. I have a robot manipulator correctly opened in RViz and VREP. I'm able to move the robot by publishing the position of the end effector on an apposit topic and see the motion of the whole manipulator in RViz. Meanwhile the trajectory of the robot is automatically published on a topic called /trajectory. Now I want to see the same motion also in VREP: so I created the following non-threaded child script:

Code: Select all

function sysCall_init()
    jointHandles={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jointHandles[i]=sim.getObjectHandle('prbt_joint_'..i)
    end
    sub=simROS.subscribe('/trajectory', 'trajectory_msgs/JointTrajectory', 'joint_callback')
    simROS.subscriberTreatUInt8ArrayAsString(sub)
end

function joint_callback(msg)
    targetPos1={msg.points[1].positions[1], msg.points[1].positions[2], msg.points[1].positions[3], msg.points[1].positions[4], msg.points[1].positions[5], msg.points[1].positions[6]}
    print(msg)
    for i=1,6,1 do
       sim.setJointTargetPosition(jointHandles[i], targetPos1[i])
    end
end

function sysCall_actuation()
end

function sysCall_cleanup()
    simROS.shutdownSubscriber(sub)
end
The problem is that, when I publish a certain configuration for the end effector on the topic, I always see in VREP the previous configuration of the robot and not the actual one. It is as if there is a delay configuration between what I publish and what I see in VREP. This is very strange because the printed msg in VREP and the content of the topic /trajectory (rostopic echo /trajectory) are the same. But why on VREP can't I see the actual motion of the manipulator?
Moreover, what should I write in the function sysCall_actuation if I already put the actuation part of the code in the function joint_callback()?
Thanks for your time!

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

Re: Trajectory Following for a Manipulator

Post by coppelia »

Hello,

so are you running in synchronous mode to be able to see that difference? The ROS interface normally calls spinOnce at the end of a simulation step, and the physics engine will then only run again in the next simulation step, in the actuation phase.

Or did I misunderstood something?

Cheers

Post Reply