Thank you, Edouard, for your answer!
I followed your advice and tried to hook up with Joint Trajectory Controller via
Seems like this topic is also used in
arm_commader.py. 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 = rospy.Time.now() self.arm_commander.joint_trajectory_publisher.publish(trajectory_msg)
Basically, now Niryo listens to the new topic
RobotMoveCommand message. Then plans trajectory with
and publishes it to the
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
Looking forward for your thoughts and hints!
- This reply was modified 2 months, 2 weeks ago by achernov.