Forums Niryo One Programming Niryo One Queuing move goals Reply To: Queuing move goals

Post count: 7

Thank you, Edouard, for your answer!

I followed your advice and tried to hook up with Joint Trajectory Controller via /niryo_one_follow_joint_trajectory_controller/command topic.
Seems like this topic is also used in So, I ended up with next modifications to

class RobotCommander   
     def __init__(self, position_manager, trajectory_manager):
        self.arm_trajectory_subscriber = rospy.Subscriber(
                    '/niryo_one/commander/trajectory', RobotMoveCommand, self.callback_trajectory_command)
    def callback_trajectory_command(self, msg):
        rospy.loginfo("Robot Action Server - Trajectory command %s", msg)
        cmd = msg
        self.arm_commander.set_pose_target(cmd.position.x, cmd.position.y, cmd.position.z,
                                           cmd.rpy.roll, cmd.rpy.pitch, cmd.rpy.yaw)
        plan = self.move_group_arm.compute_plan()
        if not plan:
            raise RobotCommanderException(
                CommandStatus.PLAN_FAILED, "Moveit failed to compute the plan.")
        rospy.loginfo("Robot Action Server - Trajectory plan is ready")
        rospy.logwarn("Send trajectory direct message %s", plan)
        trajectory_msg = plan.joint_trajectory
        trajectory_msg.header.stamp =

Basically, now Niryo listens to the new topic /niryo_one/commander/trajectory for RobotMoveCommand message. Then plans trajectory with MoveGroupArm.compute_plan
and publishes it to the /niryo_one_follow_joint_trajectory_controller/command topic.
Unfortunately, it does not work. Niryo does not move. I can see in logs, that RobotMoveCommand message was received, trajectory was computed, and Joint Trajectory Controller started to process the new trajectory. After that is silence, i.e. no other log activity, the arm does not move. Could it be that niryo_one_driver rejects trajectory or does not understand it?

How can I troubleshoot the chain after joint_trajectory_controller?

Looking forward for your thoughts and hints!

  • This reply was modified 1 week, 6 days ago by  achernov.