j2s6s300 stop in the middle of the trajectory

[WH-0501]

I‘m using moveit! to control j2s6s300. When i set an goal in RViz by drag interactive_marker, and click 'Plan and Execute' button, i can see moveit plan success, but the arm stop in the middle of the trajectory with any error.

roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2s6s300
roslaunch j2s6s300_moveit_config j2s6s300_demo.launch

[Tzxtoiny]

I met the same problem!! The arm stops at the middle of the trajectory in moveit.

[WH-0501]

I met the same problem!! The arm stops at the middle of the trajectory in moveit.

Did you fix this？

[Tzxtoiny]

I met the same problem!! The arm stops at the middle of the trajectory in moveit.

Did you fix this？

no, I have no idea how to do it.

[felixmaisonneuve]

Hi @WH-0501,

Like @Tzxtoiny said, there is #384 that covers this issue. You will start the trajectory and it will stop with the message "Aborting because we wound up outside the goal constraints" somewhere in the middle.

We have tried quite a bit and nothing seems to do anything. I do not know what is causing this and I haven't got the time to investigate it. I still have the issue open and I plan to look at it, but I don't know when.

From what I have seen, this problem is present on most (all?) of the arms I tested, but the point where it stops varies from one arm to another. Some arms will get very close to the final pose on the first try and other arms will only get half way.

Best regards, Felix

[WH-0501]

Hi @felixmaisonneuve

My situation is a little different from this, the prompt execution is successful, instead of "Aborting because we wound up outside the goal constraints"

[Tzxtoiny]

Hi @felixmaisonneuve

My situation is a little different from this, the prompt execution is successful, instead of "Aborting because we wound up outside the goal constraints"

I know what you said, I also met the problems as you said. The trajectory is planned and sent to the corresponding service, but it will stop at the middle, sometimes half, sometimes close to the target pose.

[felixmaisonneuve]

@Tzxtoiny is this something that was covered in your other issue as well? I do not remember ever earing about this.

@WH-0501 can you print the full trajectory sent by moveit to see if it is complete? If you arm only reaches half the trajectory, I want to know if it is because MoveIt is only sending the first half of the trajectory or if it is the arm who ignores the last half.

[WH-0501]

@Tzxtoiny is this something that was covered in your other issue as well? I do not remember ever earing about this.

@WH-0501 can you print the full trajectory sent by moveit to see if it is complete? If you arm only reaches half the trajectory, I want to know if it is because MoveIt is only sending the first half of the trajectory or if it is the arm who ignores the last half.

yes i tried it and received the full trajectory

[felixmaisonneuve]

Do you know what is the last trajectory point the arm reaches? For example, does the arm always stop atthe 1000th trajectory point.

[WH-0501]

Do you know what is the last trajectory point the arm reaches? For example, does the arm always stop atthe 1000th trajectory point.

I checked it, the trajectory is published normally, and the kinova driver also prompts execute each trajectory point,。

I checked the driver, it use the trajectory time to determine whether it is finished. The execution time of the robotic arm is satisfactory, but it does not actually reach the target point. For example: through the trajectory time_from_start param, the total duration of the trajectory can be calculated as 5s, and the robotic arm has indeed executed 5s, but at this time the robotic arm has not reached the target point and it is judged that the execution is completed

[felixmaisonneuve]

This is new information to me, but what you are saying makes a lot of sense. The way I remember the code in the ROS driver it is indeed checking if the trajectory is completed based on the time left. It might be worth to change this implementation to something else and see if your arm is able to reach the end pose.

This is where the validation happens if I am not mistaking : https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp#L235-L269

[WH-0501]

This is new information to me, but what you are saying makes a lot of sense. The way I remember the code in the ROS driver it is indeed checking if the trajectory is completed based on the time left. It might be worth to change this implementation to something else and see if your arm is able to reach the end pose.

This is where the validation happens if I am not mistaking :

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp#L235-L269

I try to make some change. First, I modified controllerStateCB in joint_trajectory_action_server.cpp to

void JointTrajectoryActionController::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
{
    ......

    int last = current_traj_.points.size() - 1;
    ros::Time end_time = start_time_ + current_traj_.points[last].time_from_start;

    bool inside_goal_constraints = true;
    for (size_t i = 0; i < msg->joint_names.size(); ++i) {
        double abs_error = fabs(msg->actual.positions[i] - current_traj_.points[last].positions[i]);
        double goal_constraint = 0.01; //goal_constraints_[msg->joint_names[i]];
        ROS_INFO("Joint %ld abs_error: %lf, and goal constraint is: %lf. (%lf - %lf)", i, abs_error, goal_constraint, msg->actual.positions[i], current_traj_.points[last].positions[i]);
        if (goal_constraint >= 0 && abs_error > goal_constraint)
            inside_goal_constraints = false;
        // It's important to be stopped if that's desired.
        if ( !(msg->desired.velocities.empty()) && (fabs(msg->desired.velocities[i]) < 1e-6) )
        {
            if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
                inside_goal_constraints = false;
        }
    }
    if (inside_goal_constraints) {
        active_goal_.setSucceeded();
        has_active_goal_ = false;
        first_fb_ = true;
    } 
}

then modified pub_joint_vel in kinova_joint_trajectory_controller.cpp to

void JointTrajectoryController::pub_joint_vel(const ros::TimerEvent&)
{
    kinova_msgs::JointVelocity joint_velocity_msg;

    if (traj_command_points_index_ <  kinova_angle_command_.size() && ros::ok())
    {
        ......
        pub_joint_velocity_.publish(joint_velocity_msg);

        bool inside_goal_constraints = true;
        for (size_t j = 0; j < joint_names_.size(); j++)
        {
            ROS_INFO("Joint %ld position error: %lf", j, traj_feedback_msg_.error.positions[j]);
            if (fabs(traj_feedback_msg_.error.positions[j]) > 0.001 && traj_command_points_index_ > 0) {
                inside_goal_constraints = false;
            }
        }
        if (inside_goal_constraints) {
            ROS_INFO_STREAM("Moved to point " << traj_command_points_index_++);
        }
    }
   else // if come accross all the points, then stop timer.
    {
        joint_velocity_msg.joint1 = 0;
        joint_velocity_msg.joint2 = 0;
        joint_velocity_msg.joint3 = 0;
        joint_velocity_msg.joint4 = 0;
        joint_velocity_msg.joint5 = 0;
        joint_velocity_msg.joint6 = 0;
        joint_velocity_msg.joint7 = 0;

        traj_command_points_.clear();

        traj_command_points_index_ = 0;
        timer_pub_joint_vel_.stop();
    }
}

Then the arm can run continuously, but the stop condition seems wrong. I use traj_feedback_msg_.error.positions as the deviation of the expected joint position and the actual joint position, but it doesn't look like that.

Can you suggest some changes?

[AndrejOrsula]

This issue also occurs with j2s7s300, as demonstrated in the video below. I do not believe there is any issue with MoveIt (works in simulation and other robots) or the hardware (robot reaches the command joint positions in the Kinova SDK).

https://user-images.githubusercontent.com/22929099/182180690-50b81420-0cff-463a-84a9-88d0ba6e7075.mp4

I have not investigated the source code in depth, but your observation with the controller checking the remaining time instead of reaching the goal joint configuration of trajectory waypoints might be worth improving. @WH-0501 seems to be on the right track.

https://github.com/Kinovarobotics/kinova-ros/blob/1c4b2fdf05bc929fda26aac4e420276129375375/kinova_driver/src/kinova_joint_trajectory_controller.cpp#L222-L225

---

Also, there is a bunch of unused variables/computations all over the code due to disabled logging. For example, durations, trajectory_duration and previous_pub_ in kinova_joint_trajectory_controller.cpp. I am not sure why that is the case.

[Tzxtoiny]

This is new information to me, but what you are saying makes a lot of sense. The way I remember the code in the ROS driver it is indeed checking if the trajectory is completed based on the time left. It might be worth to change this implementation to something else and see if your arm is able to reach the end pose. This is where the validation happens if I am not mistaking : https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp#L235-L269

I try to make some change. First, I modified controllerStateCB in joint_trajectory_action_server.cpp to

void JointTrajectoryActionController::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
{
    ......

    int last = current_traj_.points.size() - 1;
    ros::Time end_time = start_time_ + current_traj_.points[last].time_from_start;

    bool inside_goal_constraints = true;
    for (size_t i = 0; i < msg->joint_names.size(); ++i) {
        double abs_error = fabs(msg->actual.positions[i] - current_traj_.points[last].positions[i]);
        double goal_constraint = 0.01; //goal_constraints_[msg->joint_names[i]];
        ROS_INFO("Joint %ld abs_error: %lf, and goal constraint is: %lf. (%lf - %lf)", i, abs_error, goal_constraint, msg->actual.positions[i], current_traj_.points[last].positions[i]);
        if (goal_constraint >= 0 && abs_error > goal_constraint)
            inside_goal_constraints = false;
        // It's important to be stopped if that's desired.
        if ( !(msg->desired.velocities.empty()) && (fabs(msg->desired.velocities[i]) < 1e-6) )
        {
            if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
                inside_goal_constraints = false;
        }
    }
    if (inside_goal_constraints) {
        active_goal_.setSucceeded();
        has_active_goal_ = false;
        first_fb_ = true;
    } 
}

then modified pub_joint_vel in kinova_joint_trajectory_controller.cpp to

void JointTrajectoryController::pub_joint_vel(const ros::TimerEvent&)
{
    kinova_msgs::JointVelocity joint_velocity_msg;

    if (traj_command_points_index_ <  kinova_angle_command_.size() && ros::ok())
    {
        ......
        pub_joint_velocity_.publish(joint_velocity_msg);

        bool inside_goal_constraints = true;
        for (size_t j = 0; j < joint_names_.size(); j++)
        {
            ROS_INFO("Joint %ld position error: %lf", j, traj_feedback_msg_.error.positions[j]);
            if (fabs(traj_feedback_msg_.error.positions[j]) > 0.001 && traj_command_points_index_ > 0) {
                inside_goal_constraints = false;
            }
        }
        if (inside_goal_constraints) {
            ROS_INFO_STREAM("Moved to point " << traj_command_points_index_++);
        }
    }
   else // if come accross all the points, then stop timer.
    {
        joint_velocity_msg.joint1 = 0;
        joint_velocity_msg.joint2 = 0;
        joint_velocity_msg.joint3 = 0;
        joint_velocity_msg.joint4 = 0;
        joint_velocity_msg.joint5 = 0;
        joint_velocity_msg.joint6 = 0;
        joint_velocity_msg.joint7 = 0;

        traj_command_points_.clear();

        traj_command_points_index_ = 0;
        timer_pub_joint_vel_.stop();
    }
}

Then the arm can run continuously, but the stop condition seems wrong. I use traj_feedback_msg_.error.positions as the deviation of the expected joint position and the actual joint position, but it doesn't look like that.

Can you suggest some changes?

Have you found a solution to this problem now?

[WH-0501]

This is new information to me, but what you are saying makes a lot of sense. The way I remember the code in the ROS driver it is indeed checking if the trajectory is completed based on the time left. It might be worth to change this implementation to something else and see if your arm is able to reach the end pose. This is where the validation happens if I am not mistaking : https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp#L235-L269

I try to make some change. First, I modified controllerStateCB in joint_trajectory_action_server.cpp to

void JointTrajectoryActionController::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
{
    ......

    int last = current_traj_.points.size() - 1;
    ros::Time end_time = start_time_ + current_traj_.points[last].time_from_start;

    bool inside_goal_constraints = true;
    for (size_t i = 0; i < msg->joint_names.size(); ++i) {
        double abs_error = fabs(msg->actual.positions[i] - current_traj_.points[last].positions[i]);
        double goal_constraint = 0.01; //goal_constraints_[msg->joint_names[i]];
        ROS_INFO("Joint %ld abs_error: %lf, and goal constraint is: %lf. (%lf - %lf)", i, abs_error, goal_constraint, msg->actual.positions[i], current_traj_.points[last].positions[i]);
        if (goal_constraint >= 0 && abs_error > goal_constraint)
            inside_goal_constraints = false;
        // It's important to be stopped if that's desired.
        if ( !(msg->desired.velocities.empty()) && (fabs(msg->desired.velocities[i]) < 1e-6) )
        {
            if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
                inside_goal_constraints = false;
        }
    }
    if (inside_goal_constraints) {
        active_goal_.setSucceeded();
        has_active_goal_ = false;
        first_fb_ = true;
    } 
}

then modified pub_joint_vel in kinova_joint_trajectory_controller.cpp to

void JointTrajectoryController::pub_joint_vel(const ros::TimerEvent&)
{
    kinova_msgs::JointVelocity joint_velocity_msg;

    if (traj_command_points_index_ <  kinova_angle_command_.size() && ros::ok())
    {
        ......
        pub_joint_velocity_.publish(joint_velocity_msg);

        bool inside_goal_constraints = true;
        for (size_t j = 0; j < joint_names_.size(); j++)
        {
            ROS_INFO("Joint %ld position error: %lf", j, traj_feedback_msg_.error.positions[j]);
            if (fabs(traj_feedback_msg_.error.positions[j]) > 0.001 && traj_command_points_index_ > 0) {
                inside_goal_constraints = false;
            }
        }
        if (inside_goal_constraints) {
            ROS_INFO_STREAM("Moved to point " << traj_command_points_index_++);
        }
    }
   else // if come accross all the points, then stop timer.
    {
        joint_velocity_msg.joint1 = 0;
        joint_velocity_msg.joint2 = 0;
        joint_velocity_msg.joint3 = 0;
        joint_velocity_msg.joint4 = 0;
        joint_velocity_msg.joint5 = 0;
        joint_velocity_msg.joint6 = 0;
        joint_velocity_msg.joint7 = 0;

        traj_command_points_.clear();

        traj_command_points_index_ = 0;
        timer_pub_joint_vel_.stop();
    }
}

Then the arm can run continuously, but the stop condition seems wrong. I use traj_feedback_msg_.error.positions as the deviation of the expected joint position and the actual joint position, but it doesn't look like that. Can you suggest some changes?

Have you found a solution to this problem now?

Not yet. I don't have time to deal with it now.

[hbler99]

Have you found a solution to this problem now? I have met the same problem in controling j2n6s300.