Need suggestions in debugging Descartes & Niryo (see video)

edit

Hello everybody,

I've adapted the Descartes tutorial1 file. I've replaced the ABB robot with the Niryo One with an IKFast plugin.

The trajectory looks fine when the tool is facing the X-axis. However, it doesn't seem to work when I ask the tool to point up (pos. Z-axis) or point down (neg. Z-axis). The video of this problem can be seen here: https://www.youtube.com/watch?v=7D3o2... . There are 4 trajectories:

1. translate along +Y-axis, with no tool rotation
2. translate along -Y-axis, with no tool rotation
3. translate along +Y-axis, with tool pointing up
4. translate along -Y-axis, with tool pointing down

The code and the diff to the original tutorial are shown below. The question is what are your suggestions in debugging this? Where should I be looking at first? Thank you. Thank you. Thank you.

Env: Ubuntu 16.04.07 LTS, Kinetic from apt-package, Niryo-one ROS pulled from git, Descartes pulled from git.

Diff between my modified tutorial and the original tutorial1.cpp (I don't know why the left-facing-arrow marks are not showing up:

20a21,25
> 
> #include <tutorial_utilities path_generation.h="">
> #include <tutorial_utilities collision_object_utils.h="">
> #include <tutorial_utilities visualization.h="">
> 
30a36,46
> EigenSTL::vector_Isometry3d makePath1a();
> descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Isometry3d& pose, double dt);
> descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Isometry3d& pose, double dt);
> 
> 
> /**
>  * Waits for a subscriber to subscribe to a publisher
>  */
> bool waitForSubscribers(ros::Publisher &pub, ros::Duration timeout);
> 
> 
34c50
<   ros::init(argc, argv, "descartes_tutorial");
---
>   ros::init(argc, argv, "move_arm_node");
54a71
> 
63c80
<   const std::string group_name = "manipulator";
---
>   const std::string group_name = "arm";
70c87
<   const std::string tcp_frame = "tool0";
---
>   const std::string tcp_frame = "tool_link";
86c103,168
<   std::vector<descartes_core::trajectoryptptr> points = makePath();
---
>   // MYCOMMENT: Re-integrated the helper for visualisation
>   std::vector<descartes_core::trajectoryptptr> points;// = makePath();
> 
>   // In Descartes, trajectories are composed of "points". Each point describes what joint positions of the robot can
>   // satisfy it. You can have a "joint point" for which only a single solution is acceptable. You might have a
>   // fully defined cartesian point for which many (8 or 16) different robot configurations might work. You could
>   // allow extra tolerances in any of these and even more points satisfy the constraints.
> 
>   // In this first tutorial, we're just going to describe a simple cartesian trajectory that moves the robot
>   // along a line in the XY plane.
> 
>   // Step 1: Let's start by just doing the math to generate the poses we want.
> 
>   // First thing, let's generate a pattern with its origin at zero. We'll define another transform later that
>   // can move it to somewere more convenient.
>   const static double step_size = 0.02;
>   const static int num_steps = 20;
>   const static double time_between_points = 1.0;
> 
>   EigenSTL::vector_Isometry3d pattern_poses;
> 
>   pattern_poses = makePath1a();
> 
>   for (const auto& pose : pattern_poses)
>   {
> ...