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

achernov
Participant
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 arm_commader.py. So, I ended up with next modifications to robot_commander.py:


 
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 = rospy.Time.now()
        self.arm_commander.joint_trajectory_publisher.publish(trajectory_msg)
 

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 6 months, 3 weeks ago by  achernov.