Revision history [back]

I suppose Vrep has to publish some message to the /follow_joint_trajectory/goal topic but i have some problem understanding how to compose the message to publish.

Provided you are using ur_gazebo (or a similar package) which exposes a FollowJointTrajectory action server, you'd have to write something that instantiates and populates a control_msgs/FollowJointTrajectory goal and then submits it to the action server.

The goal is not that difficult to create. Something like this would do it:

from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

...

# using the action namespace you mentioned in your question here
client = actionlib.SimpleActionClient('follow_joint_trajectory',
  FollowJointTrajectoryAction)
client.wait_for_server()

goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()

# NOTE: order matters here! This HAS to correspond to how the
#       values in your VREP trajectory are ordered
goal.trajectory.joint_names = [
  'shoulder_pan_joint',
  'shoulder_lift_joint',
  'elbow_joint',
  'wrist_1_joint',
  'wrist_2_joint',
  'wrist_3_joint'
]

# not entirely sure how your 'points' list (array?) is built up. I'm
# assuming a list of tuple(float, float, float, float, float, float)
# here. Adjust as necessary.

for p in points:
  goal.trajectory.points.append(
    JointTrajectoryPoint(
      positions=list(p),
      velocities=...,
      accelerations=...,
      effort=...,
      time_from_start=rospy.Duration(...)))

# finally: submit the goal for execution
client.send_goal(goal)

# optional
client.wait_for_result()

You'll want to make sure to specify proper velocities, as I expect the controller to stop at each point in your trajectory if you don't.

I suppose Vrep has to publish some message to the /follow_joint_trajectory/goal topic but i have some problem understanding how to compose the message to publish.

Provided you are using ur_gazebo (or a similar package) which exposes a FollowJointTrajectory action server, you'd have to write something that instantiates and populates a control_msgs/FollowJointTrajectory goal and then submits it to the action server.

The goal is not that difficult to create. Something like this would do it:

from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

...

# using the action namespace you mentioned in your question here
client = actionlib.SimpleActionClient('follow_joint_trajectory',
  actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()

goal = FollowJointTrajectoryGoal()
goal.trajectory = JointTrajectory()

# NOTE: order matters here! This HAS to correspond to how the
#       values in your VREP trajectory are ordered
goal.trajectory.joint_names = [
  'shoulder_pan_joint',
  'shoulder_lift_joint',
  'elbow_joint',
  'wrist_1_joint',
  'wrist_2_joint',
  'wrist_3_joint'
]

# not entirely sure how your 'points' list (array?) is built up. I'm
# assuming a list of tuple(float, float, float, float, float, float)
# here. Adjust as necessary.

for p in points:
  goal.trajectory.points.append(
    JointTrajectoryPoint(
      positions=list(p),
      velocities=...,
      accelerations=...,
      effort=...,
      time_from_start=rospy.Duration(...)))

# finally: submit the goal for execution
client.send_goal(goal)

# optional
client.wait_for_result()

You'll want to make sure to specify proper velocities, as I expect the controller to stop at each point in your trajectory if you don't.