Trajectory control

[akihikoy]

Although this was mentioned in #27, I would like to keep this issue opened.

At this moment, we cannot control trajectory of Gen3, which is a lack of fundamental feature of robots. Everyone should be able to know this fact before buying the robot. So, I would like to keep this open.

[simonferronforget]

Hello,
The FollowJointTrajectory server in ros_kortex was introduced in last release. Did you have the chance to try it ? Does it fit your needs ?

Regards,
Simon

[akihikoy]

@simonferronforget

As mentioned in Kinovarobotics/Kinova-kortex2_Gen3_G3L#16 by @alexvannobel :

• The Base service now supports the PlayPreComputedJointTrajectory function.
• ROS version can be found in the ROS FollowJointTrajectory server of ros_kortex.
• The note is that there are limitations to use it as described here.

I explored this action server from ROS.

My input is a set of via points of a trajectory (set of (joint angles, time from start) pairs) as usual ROS follow_joint_trajectory.
We use the /my_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory action server to follow a joint angular trajectory. However, the trajectory should have 1msec timesteps intervals between all trajectory points. In order to satisfy this limitation, I used cubic Hermite splines to generate 1 msec interval trajectory from my discrete via points.

The test code can be found: https://github.com/akihikoy/ay_test/blob/master/ros/py_ros/gen3/follow_q_traj3.py
The video: https://youtu.be/CUByCHOaKcE

It is almost working.
However there is still an issue.
When the goal pose (the final target) is the same as the current pose (or within the goal_tolerance), it cancels the trajectory with the message: "We already reached the goal position : nothing to do."
This should be a bug.
We cannot make cyclic trajectories.

[alexvannobel]

Hi @akihikoy ,

A couple of MoveIt bugs were flagged to us (bugs with the action server in fact) and we'll look into them very shortly. We will probably push the fixes to the kinetic branch in the coming weeks.
Meanwhile, you could comment out these lines in the action server, and recompile it. This will remove the check on the "goal already reached".

I'll update this issue when we have a definitive fix for that.
Hope this helps,
Alex

[akihikoy]

Hi @alexvannobel ,

Thanks. By commenting those lines, I was able to create cyclic motions.

Let's keep this issue open until it is officially fixed.
Akihiko

[akihikoy]

Hello @alexvannobel ,

I would like to know whether the hard limits of /my_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory are removed.

Especially the limitation of time increments (Time increments must be 0.001 seconds between trajectory points) is bothering me.

Thanks,
--Akihiko

[akihikoy]

I also noticed that there is an interval (1.0-2.0 sec) between the trajectory command request and the actual motion start.

More clearly, after the command

client.send_goal(goal)

is executed (client: SimpleActionClient, goal:FollowJointTrajectoryGoal), it takes 1.0-2.0 sec to start the motion.

Perhaps the trajectory controller takes time to process the large amount of data (as the trajectory points are generated at each 0.001 sec). But anyway, this is not good for many of robot applications.

Any solutions, or did you already solve this issue?

Thanks,
--Akihiko

[elevenjiang1]

I find that use FollowJointTrajectory to control robot may occur the robot block.
I just simply let the robot move repeatedly ,after 10min,the robot can not move,and the webapp can not connect.

[alexvannobel]

Hi to all,

The latest release (v2.3.0) adds Waypoints API to control the Kinova Gen3 and Gen3 lite.

The FollowJointTrajectory action server was modified to use this API and support via points that have arbitrary durations between themselves.

I'll close this issue since the initial request was fulfilled.

Best,
Alex