Forums Niryo One Programming Niryo One Queuing move goals

Viewing 8 posts - 1 through 8 (of 8 total)
  • Author
    Posts
  • achernov
    Participant
    Post count: 7

    Hi,

    We’re trying to move the arm by sending the end position with roslibpy.Actionlib.Goal

    `
    ActionClient(self.ros, ‘/niryo_one/commander/robot_action’, ‘niryo_one_msgs/RobotMoveAction’)
    `

    We want to constantly change the goal by providing a new target position (the robot should follow human hand movement). When we send a new goal with the new position we get an error:

    `
    {‘status’: 10, ‘message’: ‘Current command is still active. Cancel it if you want to execute a new one’}
    `

    and the next command get dropped.

    Can we use some queuing to queue the changing target goal?

    Edouard Renard
    Keymaster
    Post count: 179

    Hi,

    The RobotMoveAction action server does not allow for overriding trajectories. You can cancel a trajectory, but not sending a new one while there is still one active. So, you have to wait until there is no trajectory being processed to start a new one.

    However, you can still achieve what you want to do. You’ll have to write some more code and use the MoveGroup functionality from Moveit. Check out this code here to see how we use the MoveGroupCommander in Python.

    So, to do what you want to do:

    1. Don’t use the action server we provide (/niryo_one/commander/robot_action)

    2. Use the MoveGroupCommander to compute trajectories, and send them to the controller (see this step by step guide)

    3. Once planned, the trajectory is sent to ros_control (more specifically a Joint Trajectory Controller). And here, good news, this controller can already replace trajectories (see here for more info on that). So basically, no work needed from you on this part

    With that, you can setup your own policy about new trajectories, preemption, etc.

    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 2 months, 2 weeks ago by  achernov.
    Edouard Renard
    Keymaster
    Post count: 179

    Nice progress!

    I guess I found why it’s not working:

    The Joint Trajectory Controller ‘/niryo_one_follow_joint_trajectory_controller/command’ topic is used to send direct command to the hardware. If you use this interface you’d have to compute or interpolate each points of the trajectory before sending them to the ros_control controller.

    You can think of this ‘/niryo_one_follow_joint_trajectory_controller/command’ interface as an interface most suited for ‘jog’ commands. And that’s exactly what we use for moving the robot with a Joystick controller (see code on github)

    What you need here is not the ‘/niryo_one_follow_joint_trajectory_controller/command’ topic, but the ‘/niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory’ action. (see more about this controller here)

    How things are processed with Niryo One (quick recap):

    – The RobotCommander will act as a global gateway, to ensure only one command is processed at the same time

    – It contains a MoveGroup interface to plan and send trajectories

    – The trajectory is sent directly from Moveit with its simple_controller_manager functionality

    – ros_control receives the trajectory, interpolates between points, and send commands to hardware.

    For your application, you can rewrite the first part (as you did here), keep the MoveGroup interface to only compute trajectories. Then send the trajectory yourself instead of using the Moveit functionality (self.arm.execute() or self.arm.move()), and finally, nothing to do on the ros_control side.

    So:

    – In your RobotCommander class, create an ActionClient, or the simpler-to-use SimpleActionClient (the simple action client can only handle one goal from the client side, but for your application that’s just what you need).

    – Keep your callback_trajectory_command function with the ‘/niryo_one/commander/trajectory’ subscriber.

    – In the callback, send a new ActionGoal through your SimpleActionServer (and not through the topic interface)

    And that should be it! Everytime your subscriber receives a new command, it will compute a plan and send it to the controller. The controller will then use its trajectory replacement feature.

    Let me know if it doesn’t make sense 🙂

    achernov
    Participant
    Post count: 7

    Alright, we tried the approach with SimpleActionClient you suggested, but unfortunately the result is the same – no movement. Attaching rosout.log for investigation. In the log you can see that topic message is still used, but actually with goal the output is identical.

    
    
    1549558269.605691035  Node Startup
    1549558271.531095981 INFO [niryo_one_ros_setup.py:212(start_process) [topics: /niryo_one/rpi/process_state, /rosout] Start process : robot_commander
    1549558271.863245010 INFO [rosbridge_websocket:224(<module>) [topics: /rosout] Rosbridge WebSocket server started on port 9090
    1549558272.940612077 INFO [rosapi_node:194(<module>) [topics: /rosout] Rosapi started
    1549558275.529162036 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:148(init) [topics: /rosout] 3 motors should be connected to CAN bus
    1549558275.542890203 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_driver/niryo_one_can_driver.cpp:50(init) [topics: /rosout] Result begin can : 0
    1549558275.549954891 INFO [niryo_one_ros_setup.py:209(start_process) [topics: /niryo_one/rpi/process_state, /rosout] Handle process : user_interface
    1549558275.551881074 INFO [niryo_one_ros_setup.py:212(start_process) [topics: /niryo_one/rpi/process_state, /rosout] Start process : user_interface
    1549558275.628074136 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:66(init) [topics: /rosout] Start Dxl communication (100.000000 Hz)
    1549558275.628778612 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:67(init) [topics: /rosout] Writing data on Dxl at 50.000000 Hz
    1549558275.629420849 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:68(init) [topics: /rosout] Reading data from Dxl at 15.000000 Hz
    1549558275.630011888 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:69(init) [topics: /rosout] Reading hardware error status from Dxl at 0.500000 Hz
    1549558275.638224350 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:183(setupCommunication) [topics: /rosout] Dxl : set port name (/dev/serial0), baudrate(1000000)
    1549558275.740333675 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:132(NiryoOneDriver) [topics: /rosout] NiryoOne communication has been successfully started
    1549558275.841226079 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:137(NiryoOneDriver) [topics: /rosout] Start hardware control loop
    1549558275.842100814 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:107(manageCanConnectionLoop) [topics: /rosout] Stop Can hw control
    1549558275.842230762 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:234(stopHardwareControlLoop) [topics: /rosout] CanComm : Stop hardware control loop
    1549558276.192944211 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:117(manageCanConnectionLoop) [topics: /rosout] Set calibration flag
    1549558276.194608839 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:212(startHardwareControlLoop) [topics: /rosout] CAN : Start hardware control loop
    1549558276.194786129 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:227(startHardwareControlLoop) [topics: /rosout] START ctrl loop thread can
    1549558276.264389457 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1193(detectVersion) [topics: /rosout] Dynamixel broadcast ping - Found IDs:
    1549558276.264680289 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1195(detectVersion) [topics: /rosout] - 2
    1549558276.264767268 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1195(detectVersion) [topics: /rosout] - 3
    1549558276.264840913 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1195(detectVersion) [topics: /rosout] - 6
    1549558276.285623943 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:209(checkHardwareVersionFromDxlMotors) [topics: /rosout] Detected version from hardware : 2
    1549558276.285868734 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:235(manageDxlConnectionLoop) [topics: /rosout] Stop Dxl hw control
    1549558276.343995864 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:141(NiryoOneDriver) [topics: /rosout] Start hardware interface
    1549558276.344553570 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_hardware_interface.cpp:25(NiryoOneHardwareInterface) [topics: /rosout] Starting NiryoOne Hardware Interface...
    1549558276.345264765 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_hardware_interface.cpp:59(NiryoOneHardwareInterface) [topics: /rosout] Interfaces registered.
    1549558276.345676742 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:144(NiryoOneDriver) [topics: /rosout] Create controller manager
    1549558276.355998992 INFO [spawner:116(main) [topics: /rosout] Controller Spawner: Waiting for service controller_manager/load_controller
    1549558276.587620271 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:148(NiryoOneDriver) [topics: /rosout] Starting ros control thread...
    1549558276.588396518 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:152(NiryoOneDriver) [topics: /rosout] Start Rpi Diagnostics...
    1549558276.588872609 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:155(NiryoOneDriver) [topics: /rosout] Starting ROS interface...
    1549558276.597320904 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/ros_interface.cpp:39(RosInterface) [topics: /rosout] Ros interface started.
    1549558276.667877912 INFO [spawner:121(main) [topics: /rosout] Controller Spawner: Waiting for service controller_manager/switch_controller
    1549558276.674745082 INFO [spawner:129(main) [topics: /rosout] Controller Spawner: Waiting for service controller_manager/unload_controller
    1549558276.681794881 INFO [spawner:184(main) [topics: /rosout] Loading controller: joint_state_controller
    1549558276.729506015 INFO [spawner:184(main) [topics: /rosout] Loading controller: niryo_one_follow_joint_trajectory_controller
    1549558276.807495308 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1096(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] Dynamixel broadcast ping - Found IDs:
    1549558276.807902285 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1098(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] - 2
    1549558276.808345981 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1098(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] - 3
    1549558276.808831969 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1098(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] - 6
    1549558276.808974780 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:244(manageDxlConnectionLoop) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] Resume Dxl hw control
    1549558276.810454357 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:222(startHardwareControlLoop) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] DXL : Start hardware control loop
    1549558276.810941855 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:235(startHardwareControlLoop) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] START ctrl loop thread dxl
    1549558276.866338968 INFO [tool_ros_command_interface.py:50(__init__) [topics: /rosout] Interface between Tools Controller and Ros Control has been started.
    1549558276.914160013 INFO [tool_controller.py:74(create_tools) [topics: /niryo_one/current_tool_id, /rosout] Detected tools : (id + available commands)
    1549558276.916560888 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 11 : [1, 2]
    1549558276.918932914 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 12 : [1, 2]
    1549558276.921161890 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 13 : [1, 2]
    1549558276.923299074 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 30 : [21, 22, 20]
    1549558276.925488948 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 31 : [10, 11]
    1549558277.024730920 INFO [tool_controller.py:184(start) [topics: /niryo_one/current_tool_id, /niryo_one/tool_action/goal, /niryo_one/tool_action/cancel, /niryo_one/tool_action/result, /rosout, /niryo_one/tool_action/feedback, /niryo_one/tool_action/status] Action server started (ToolController)
    1549558277.079272031 INFO [spawner:192(main) [topics: /rosout] Controller Spawner: Loaded controllers: joint_state_controller, niryo_one_follow_joint_trajectory_controller
    1549558277.090075016 INFO [spawner:201(main) [topics: /rosout] Started controllers: joint_state_controller, niryo_one_follow_joint_trajectory_controller
    1549558280.560875391 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
    1549558280.918588496 INFO [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:93(core::RobotModel::buildModel) [topics: /rosout] Loading robot model 'niryo_one'...
    1549558280.919279378 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
    1549558281.083474461 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:331(PlanningSceneMonitor::startPublishingPlanningScene) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Publishing maintained planning scene on 'monitored_planning_scene'
    1549558281.107437528 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group.cpp:210(main) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] MoveGroup debug mode is OFF
    1549558281.107718673 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:964(PlanningSceneMonitor::startSceneMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Starting scene monitor
    1549558281.127478583 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:970(PlanningSceneMonitor::startSceneMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/planning_scene'
    1549558281.127767019 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1041(PlanningSceneMonitor::startWorldGeometryMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Starting world geometry monitor
    1549558281.148004999 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1057(PlanningSceneMonitor::startWorldGeometryMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/collision_object' using message notifier with target frame '/ground_link '
    1549558281.176345963 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1072(PlanningSceneMonitor::startWorldGeometryMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/planning_scene_world' for planning scene world geometry
    1549558281.254341805 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1134(PlanningSceneMonitor::startStateMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/attached_collision_object' for attached collision objects
    1549558281.584281703 INFO [/tmp/binarydeb/ros-kinetic-moveit-planners-ompl-0.9.15/ompl_interface/src/ompl_interface.cpp:55(OMPLInterface::OMPLInterface) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Initializing OMPL interface using ROS parameters
    1549558281.815234033 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:119(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning interface 'OMPL'
    1549558281.874300377 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp:53(FixWorkspaceBounds::FixWorkspaceBounds) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'default_workspace_bounds' was not set. Using default value: 10
    1549558281.878841606 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp:60(FixStartStateBounds::FixStartStateBounds) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'start_state_max_bounds_error' was set to 0.3
    1549558281.887674951 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp:65(FixStartStateBounds::FixStartStateBounds) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'start_state_max_dt' was not set. Using default value: 0.5
    1549558281.893030552 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_collision.cpp:57(FixStartStateCollision::FixStartStateCollision) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'start_state_max_dt' was not set. Using default value: 0.5
    1549558281.897912769 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_collision.cpp:68(FixStartStateCollision::FixStartStateCollision) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'jiggle_fraction' was set to 0.05
    1549558281.902076969 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_collision.cpp:73(FixStartStateCollision::FixStartStateCollision) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'max_sampling_attempts' was not set. Using default value: 100
    1549558281.902636810 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Add Time Parameterization'
    1549558281.902838945 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Workspace Bounds'
    1549558281.902994621 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Start State Bounds'
    1549558281.903144777 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Start State In Collision'
    1549558281.903278422 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Start State Path Constraints'
    1549558282.291514043 INFO [/tmp/binarydeb/ros-kinetic-moveit-simple-controller-manager-0.9.15/src/moveit_simple_controller_manager.cpp:146(MoveItSimpleControllerManager::MoveItSimpleControllerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel] Added FollowJointTrajectory controller for niryo_one_follow_joint_trajectory_controller
    1549558282.292304664 INFO [/tmp/binarydeb/ros-kinetic-moveit-simple-controller-manager-0.9.15/src/moveit_simple_controller_manager.cpp:197(MoveItSimpleControllerManager::getControllersList) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel] Returned 1 controllers in list
    1549558282.401289114 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/trajectory_execution_manager/src/trajectory_execution_manager.cpp:188(TrajectoryExecutionManager::initialize) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates] Trajectory execution is managing controllers
    1549558283.117929432 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group.cpp:176(MoveGroupExe::configureCapabilities) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] 
    
    ********************************************************
    * MoveGroup using: 
    *     - ApplyPlanningSceneService
    *     - ClearOctomapService
    *     - CartesianPathService
    *     - ExecuteTrajectoryAction
    *     - GetPlanningSceneService
    *     - KinematicsService
    *     - MoveAction
    *     - PickPlaceAction
    *     - MotionPlanService
    *     - QueryPlannersService
    *     - StateValidationService
    ********************************************************
    
    1549558283.118471044 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group_context.cpp:84(MoveGroupContext::status) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] MoveGroup context using planning plugin ompl_interface/OMPLPlanner
    1549558283.118763282 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group_context.cpp:85(MoveGroupContext::status) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] MoveGroup context initialization complete
    1549558284.178380886 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
    1549558284.496585161 INFO [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:93(core::RobotModel::buildModel) [topics: /rosout] Loading robot model 'niryo_one'...
    1549558284.497022919 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
    1549558284.629981040 INFO [moveitjoy_module.py:408(updatePlanningGroup) [topics: /tf_static, /rviz/moveit/select_planning_group, /rosout, /tf] Changed planning group to arm
    1549558284.633936882 INFO [moveitjoy_module.py:421(updatePoseTopic) [topics: /tf_static, /rviz/moveit/select_planning_group, /rosout, /tf] Changed controlled end effector to tool_link
    1549558284.751899957 INFO [user_interface.py:38(__init__) [topics: /rviz/moveit/move_marker/goal_tool_link, /rviz/moveit/execute, /joy_pose, /rviz/moveit/update_start_state, /rosout, /tf, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /tf_static, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /joy, /rviz/moveit/select_planning_group] ===========MoveitJoy loaded==============
    1549558284.883557081 INFO [blockly_code_generator.py:66(__init__) [topics: /rviz/moveit/move_marker/goal_tool_link, /rviz/moveit/execute, /joy_pose, /rviz/moveit/update_start_state, /rosout, /tf, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /tf_static, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /joy, /rviz/moveit/select_planning_group] Blockly code generator started
    1549558284.787094145 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
    1549558285.012191057 INFO [sequence_action_server.py:54(start) [topics: /rviz/moveit/execute, /niryo_one/sequences/execute/cancel, /rosout, /tf, /tf_static, /niryo_one/sequences/execute/goal, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /rviz/moveit/select_planning_group, /niryo_one/sequences/execute/result, /rviz/moveit/move_marker/goal_tool_link, /niryo_one/sequences/execute/feedback, /rviz/moveit/update_start_state, /joy_pose, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /joy, /niryo_one/sequences/execute/status] Action Server started (Sequence Action)
    1549558285.219535112 INFO [sequence_autorun.py:215(__init__) [topics: /rviz/moveit/execute, /niryo_one/sequences/execute/cancel, /rosout, /tf, /tf_static, /niryo_one/sequences/execute/goal, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /rviz/moveit/select_planning_group, /niryo_one/sequences/execute/result, /rviz/moveit/move_marker/goal_tool_link, /niryo_one/learning_mode, /niryo_one/sequences/execute/feedback, /rviz/moveit/update_start_state, /joy_pose, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /joy, /niryo_one/hardware_status, /niryo_one/sequences/execute/status, /niryo_one/sequences/sequence_autorun_status] Init Sequence autorun OK
    1549558285.233821842 INFO [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:93(core::RobotModel::buildModel) [topics: /rosout] Loading robot model 'niryo_one'...
    1549558285.234876577 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
    1549558285.250288963 INFO [matlab_manager.py:62(__init__) [topics: /rviz/moveit/execute, /rviz/moveit/move_marker/goal_tool_link, /niryo_one/sequences/execute/cancel, /rosout, /niryo_one_matlab/command, /tf, /niryo_one/sequences/execute/goal, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /rviz/moveit/select_planning_group, /rviz/moveit/update_goal_state, /niryo_one/sequences/sequence_autorun_status, /rviz/moveit/plan, /niryo_one/learning_mode, /niryo_one_matlab/result, /rviz/moveit/update_start_state, /joy_pose, /niryo_one/sequences/execute/result, /tf_static, /joy, /niryo_one/hardware_status, /niryo_one/sequences/execute/status, /niryo_one/sequences/execute/feedback] ..............Matlab OK.................
    1549558285.570365905 INFO [wifi_connection.py:89(__init__) [topics: /niryo_one/rpi/process_state, /rosout] Starting wifi manager...
    1549558286.343882083 INFO [wifi_connection.py:121(__init__) [topics: /niryo_one/rpi/process_state, /rosout] Current ssid : Guest
    1549558286.634478929 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-interface-0.9.15/move_group_interface/src/move_group_interface.cpp:180(planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::MoveGroupInterfaceImpl) [topics: /rosout, /trajectory_execution_event, /attached_collision_object, /move_group/goal, /move_group/cancel, /pickup/goal, /pickup/cancel, /place/goal, /place/cancel, /execute_trajectory/goal, /execute_trajectory/cancel] Ready to take commands for planning group arm.
    1549558286.635123926 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-interface-0.9.15/move_group_interface/src/move_group_interface.cpp:1085(planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowReplanning) [topics: /rosout, /trajectory_execution_event, /attached_collision_object, /move_group/goal, /move_group/cancel, /pickup/goal, /pickup/cancel, /place/goal, /place/cancel, /execute_trajectory/goal, /execute_trajectory/cancel] Replanning: yes
    1549558286.665504932 INFO [move_group_arm.py:65(__init__) [topics: /tf_static, /niryo_one/robot_state, /rosout, /tf] Successfully connected to move_group.
    Started group     : arm
    Planning_frame    : /ground_link
    Reference frame   : ground_link
    End effector link : tool_link
    1549558286.673681974 INFO [move_group_arm.py:67(__init__) [topics: /tf_static, /niryo_one/robot_state, /rosout, /tf] Arm Moveit Commander has been started
    1549558287.156985044 INFO [wifi_connection.py:125(__init__) [topics: /niryo_one/rpi/process_state, /rosout] Already connected to a Wifi or in Hotspot mode
    1549558287.181430101 INFO [wifi_connection.py:140(__init__) [topics: /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot] Wifi manager started
    1549558287.194025993 INFO [tool_commander.py:34(__init__) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /tf_static, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /tf, /niryo_one/tool_action/status, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/tool_action/feedback] Waiting for action server : niryo_one/tool_action...
    1549558287.233474016 INFO [fans_manager.py:36(setup_fans) [topics: /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot] ------ RPI FANS SETUP OK ------
    1549558287.240040063 INFO [tool_commander.py:36(__init__) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /tf_static, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /tf, /niryo_one/tool_action/status, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/tool_action/feedback] Found action server : niryo_one/tool_action
    1549558287.242249965 INFO [tool_commander.py:38(__init__) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /tf_static, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /tf, /niryo_one/tool_action/status, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/tool_action/feedback] Tool Commander has been started
    1549558287.448247909 INFO [robot_commander.py:221(start) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Action Server started (Robot Commander)
    1549558287.658555984 INFO [arm_commander.py:106(stop_current_plan) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Send STOP to arm
    1549558287.660652310 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/trajectory_execution_manager/src/trajectory_execution_manager.cpp:243(TrajectoryExecutionManager::receiveEvent) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Received event 'stop'
    1549558290.841027021 INFO [websocket_handler.py:77(open) [topics: /rosout] Client connected.  1 clients total.
    1549558292.287158012 INFO [ros_log_manager.py:147(__init__) [topics: /niryo_one/rpi/process_state, /niryo_one/learning_mode, /rosout, /niryo_one/rpi/ros_log_status, /niryo_one/wifi/hotspot] Init Ros Log Manager OK
    1549558292.295151948 INFO [shutdown_manager.py:49(__init__) [topics: /niryo_one/rpi/process_state, /niryo_one/learning_mode, /rosout, /niryo_one/rpi/ros_log_status, /niryo_one/wifi/hotspot] Shutdown Manager OK
    1549558292.438963890 INFO [led_manager.py:169(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] LED manager has been started.
    1549558292.449472904 INFO [niryo_one_button.py:41(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] GPIO setup : 4 as input
    1549558292.553144931 INFO [niryo_one_button.py:54(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] Niryo One Button started
    1549558292.693665027 INFO [motor_debug.py:32(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /niryo_one/rpi/digital_io_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] Init motor debug OK
    1549558292.715090036 INFO [modbus_server.py:57(start) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /niryo_one/rpi/digital_io_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] Start Modbus Server
    1549558322.431951045 INFO [robot_commander.py:249(callback_trajectory_command) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Robot Action Server - Trajectory command x: 0.225
    y: -0.22
    z: 0.1 roll: 0.0
    pitch: 1.55
    yaw: 1.63
    1549558322.499579904 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/default_capabilities/move_action_capability.cpp:163(MoveGroupMoveAction::executeMoveCallback_PlanOnly) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Planning request received for MoveGroup action. Forwarding to planning pipeline.
    1549558322.507166067 INFO [/tmp/binarydeb/ros-kinetic-moveit-planners-ompl-0.9.15/ompl_interface/src/model_based_planning_context.cpp:335(ModelBasedPlanningContext::useConfig) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
    1549558322.510276418 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:185(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] RRTConnect: Starting planning with 1 states already in datastructure
    1549558322.522835579 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:292(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] RRTConnect: Created 5 states (2 start + 3 goal)
    1549558322.522967453 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/src/SimpleSetup.cpp:143(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Solution found in 0.013272 seconds
    1549558322.533046470 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/src/SimpleSetup.cpp:185(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] SimpleSetup: Path simplification took 0.009394 seconds and changed from 4 to 2 states
    1549558322.548414004 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:180(callbackTrajectoryGoal) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states, /niryo_one_follow_joint_trajectory_controller/state, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status] Received trajectory GOAL
    1549558322.552408933 INFO [robot_commander.py:260(callback_trajectory_command) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Robot Action Server - Trajectory plan is ready
    1549558322.585071086 WARN [arm_commander.py:94(send_trajectory_direct) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Joint Trajectory Publisher msg: header: 
      seq: 0
      stamp: 
        secs: 1549558322
        nsecs: 553577899
      frame_id: ''
    joint_names: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]
    points: 
      - 
        positions: [-0.3433080631757919, -0.37415659874332574, 0.28124734232137194, 0.0767944870877505, 0.01689478715930511, 0.010122909661567111]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [-0.17109186245792865, 0.0, 0.0, 0.0, 0.0, 0.0]
        effort: []
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        positions: [-0.35601449063967366, -0.3927600931001226, 0.27286370069659466, 0.07526152967251763, -0.0005283144017771274, -0.061506224863800286]
        velocities: [-0.05647721686756443, -0.08268835502897148, -0.03726340448787656, -0.0068136514874022665, -0.07744177410746675, -0.3183754187496277]
        accelerations: [-0.17276868326339648, -0.25295081825776583, -0.113991972061556, -0.020843548265747486, -0.23690107417124695, -0.9739378979985842]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 385400303
      - 
        positions: [-0.3687209181035554, -0.4113635874569195, 0.2644800590718173, 0.07372857225728478, -0.017951415962859364, -0.13313535938916768]
        velocities: [-0.09210891200311652, -0.1348567588751023, -0.06077303088356602, -0.011112410633560763, -0.12630008970442894, -0.5192397050713495]
        accelerations: [-0.17272710970222613, -0.2528899503613526, -0.11396454201963516, -0.02083853265462873, -0.23684406834638802, -0.9737035380091625]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 544260421
      - 
        positions: [-0.3814273455674372, -0.4299670818137164, 0.25609641744704004, 0.07219561484205192, -0.03537451752394161, -0.2047644939145351]
        velocities: [-0.11319366145181567, -0.16572696361977413, -0.07468465030834745, -0.013656164423332666, -0.1552115781679558, -0.6380994206749324]
        accelerations: [-0.15865263132101015, -0.23228349115918584, -0.10467826677515192, -0.019140527762117006, -0.21754509017552795, -0.8943623771511894]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 666164709
      - 
        positions: [-0.3941337730313189, -0.44857057617051327, 0.24771277582226273, 0.07066265742681906, -0.05279761908502384, -0.2763936284399025]
        velocities: [-0.13010673795543767, -0.19048941743974662, -0.08584381936525659, -0.015696629858194356, -0.17840285285713078, -0.7334424299951676]
        accelerations: [-0.16224449471014624, -0.23754234227843132, -0.10704816150009812, -0.019573865427838982, -0.2224702668863049, -0.9146105599411883]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 770184020
      - 
        positions: [-0.4068402004952007, -0.46717407052731014, 0.23932913419748542, 0.06912970001158619, -0.07022072064610607, -0.34802276296526996]
        velocities: [-0.14582695964797388, -0.21350541122525363, -0.09621594837688455, -0.017593184218668775, -0.19995848050230008, -0.8220610348377976]
        accelerations: [-0.1777947179256261, -0.26030944110764603, -0.11730812630881818, -0.021449910449498396, -0.2437927919745864, -1.0022708431929517]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 862220252
      - 
        positions: [-0.41954662795908243, -0.48577756488410706, 0.2309454925727081, 0.06759674259635334, -0.08764382220718832, -0.41965189749063736]
        velocities: [-0.15915443012155453, -0.2330181753322696, -0.10500935128520816, -0.019201066902204047, -0.21823315859522335, -0.8971911355800024]
        accelerations: [-0.13910070708675726, -0.2036574974886079, -0.09177799828341077, -0.016781700521166797, -0.1907354174633074, -0.7841435595341105]
        effort: []
        time_from_start: 
          secs: 0
          nsecs: 944947110
      - 
        positions: [-0.4322530554229642, -0.5043810592409039, 0.2225618509479308, 0.06606378518112048, -0.10506692376827054, -0.49128103201600465]
        velocities: [-0.16974196315576812, -0.24851939403550194, -0.11199495623999528, -0.020478391887527925, -0.23275082407285422, -0.9568755614467658]
        accelerations: [-0.13421906273648548, -0.1965102766525497, -0.08855711209108486, -0.01619275819835032, -0.1840416882037496, -0.756624576652197]
        effort: []
        time_from_start: 
          secs: 1
          nsecs:  22089516
      - 
        positions: [-0.44495948288684595, -0.5229845535977008, 0.2141782093231535, 0.06453082776588762, -0.12249002532935278, -0.562910166541372]
        velocities: [-0.17926461436820373, -0.2624615180979779, -0.11827795713145317, -0.02162724500381672, -0.24580831955509283, -1.0105569968205241]
        accelerations: [-0.1267415652745897, -0.18556246443458776, -0.08362349411318505, -0.015290641123015954, -0.17378851530584713, -0.7144721563015299]
        effort: []
        time_from_start: 
          secs: 1
          nsecs:  94793220
      - 
        positions: [-0.45766591035072773, -0.5415880479544977, 0.20579456769837617, 0.06299787035065477, -0.139913126890435, -0.6345393010667395]
        velocities: [-0.18855811914983556, -0.2760681486203172, -0.12440976827575961, -0.022748452920754822, -0.25855160858177095, -1.0629466796096363]
        accelerations: [-0.14233526023773266, -0.20839321029712307, -0.09391214137839667, -0.01717193076108833, -0.1951706490195083, -0.8023775000685422]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 163940378
      - 
        positions: [-0.4703723378146095, -0.5601915423112945, 0.1974109260735989, 0.0614649129354219, -0.15733622845151723, -0.706168435592107]
        velocities: [-0.19728255910603715, -0.2888416107089154, -0.1301661131002661, -0.023801006438463196, -0.27051459376014286, -1.1121284094906456]
        accelerations: [-0.12184904369444477, -0.17839931823436772, -0.08039543117528439, -0.014700386524961856, -0.1670798711474179, -0.6868918559041631]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 229655226
      - 
        positions: [-0.48307876527849125, -0.5787950366680914, 0.18902728444882158, 0.05993195552018904, -0.1747593300125995, -0.7777975701174743]
        velocities: [-0.20529302210880382, -0.30056973835855727, -0.13545137925825063, -0.02476742274190056, -0.2814985710303256, -1.157285282550888]
        accelerations: [-0.13194505403890225, -0.19318089802957705, -0.08705673175000386, -0.01591841212387653, -0.18092355884742942, -0.7438054522071391]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 292805904
      - 
        positions: [-0.495785192742373, -0.5973985310248884, 0.18064364282404427, 0.05839899810495618, -0.19218243157368173, -0.8494267046428418]
        velocities: [-0.2125501304759147, -0.31119487866160156, -0.14023958553849497, -0.025642951140117363, -0.2914495455650136, -1.1981953174895474]
        accelerations: [-0.10610334469689399, -0.15534602309862378, -0.07000649235651761, -0.0128007584741229, -0.1454893089248905, -0.5981296294720048]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 353492464
      - 
        positions: [-0.5084916202062547, -0.6160020253816851, 0.172260001199267, 0.05686604068972333, -0.20960553313476393, -0.921055839168209]
        velocities: [-0.21899061197946654, -0.3206243946799271, -0.14448898521989728, -0.02641995820261408, -0.3002807582451793, -1.2345018338047984]
        accelerations: [-0.11263083844193637, -0.16490293383502805, -0.07431330230935328, -0.013588263063256212, -0.15443983312078174, -0.6349266543560576]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 412394241
      - 
        positions: [-0.5211980476701366, -0.6346055197384821, 0.16387635957448965, 0.05533308327449047, -0.22702863469584617, -0.9926849736935763]
        velocities: [-0.22562624651305602, -0.330339634463123, -0.14886714596104517, -0.02722050935611145, -0.30937956550076656, -1.2719084738709177]
        accelerations: [-0.11955990458512829, -0.17504778715896163, -0.0788850678590457, -0.014424215053313429, -0.1639409949131628, -0.6739874377526204]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 469563725
      - 
        positions: [-0.5339044751340183, -0.6532090140952789, 0.15549271794971237, 0.053800125859257614, -0.24445173625692843, -1.0643141082189438]
        velocities: [-0.23246294740864692, -0.34034925572698993, -0.15337796935081044, -0.02804531801011103, -0.3187540757150053, -1.3104485725376804]
        accelerations: [-0.12691524792109024, -0.18581675338751316, -0.08373808911393643, -0.015311594935697755, -0.17402666960989932, -0.7154512464263463]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 525051863
      - 
        positions: [-0.5466109025979, -0.6718125084520759, 0.14710907632493506, 0.05226716844402475, -0.2618748378180107, -1.1359432427443112]
        velocities: [-0.2383034344072293, -0.3489003191339101, -0.15723149545399495, -0.028749939185378305, -0.32676257365289785, -1.3433727780316542]
        accelerations: [-0.08892980239052335, -0.13020222101179377, -0.058675469176814106, -0.010728869337762582, -0.12194088253848114, -0.5013183128658224]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 578908104
      - 
        positions: [-0.5593173300617817, -0.6904160028088727, 0.13872543470015775, 0.05073421102879189, -0.2792979393790929, -1.2075723772696787]
        velocities: [-0.23830343440723034, -0.34890031913390906, -0.15723149545399495, -0.02874993918537824, -0.3267625736528975, -1.3433727780316542]
        accelerations: [0.08892980239048431, 0.13020222101183335, 0.058675469176814106, 0.010728869337765056, 0.12194088253849154, 0.5013183128658224]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 631703164
      - 
        positions: [-0.5720237575256636, -0.7090194971656696, 0.13034179307538044, 0.04920125361355903, -0.29672104094017515, -1.2792015117950462]
        velocities: [-0.23246294740864798, -0.3403492557269908, -0.1533779693508107, -0.02804531801011096, -0.3187540757150048, -1.3104485725376804]
        accelerations: [0.12691524792112832, 0.18581675338740147, 0.0837380891139273, 0.015311594935695344, 0.1740266696098983, 0.7154512464263463]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 685559404
      - 
        positions: [-0.5847301849895453, -0.7276229915224666, 0.12195815145060313, 0.047668296198326174, -0.3141441425012574, -1.3508306463204136]
        velocities: [-0.22562624651305585, -0.3303396344631228, -0.14886714596104506, -0.02722050935611149, -0.30937956550076606, -1.2719084738709177]
        accelerations: [0.11955990458513459, 0.1750477871591103, 0.07888506785906732, 0.014424215053312, 0.16394099491316266, 0.6739874377526198]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 741047542
      - 
        positions: [-0.5974366124534272, -0.7462264858792633, 0.11357450982582581, 0.04613533878309331, -0.3315672440623396, -1.422459780845781]
        velocities: [-0.21899061197946634, -0.32062439467992676, -0.14448898521989717, -0.02641995820261412, -0.30028075824517975, -1.2345018338047984]
        accelerations: [0.11263083844193007, 0.16490293383488733, 0.07431330230933314, 0.013588263063257578, 0.1544398331207491, 0.6349266543560571]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 798217027
      - 
        positions: [-0.6101430399173089, -0.7648299802360603, 0.1051908682010485, 0.044602381367860454, -0.3489903456234219, -1.4940889153711483]
        velocities: [-0.2125501304759147, -0.31119487866160256, -0.14023958553849497, -0.025642951140117422, -0.29144954556501435, -1.1981953174895474]
        accelerations: [0.10610334469689399, 0.15534602309871753, 0.07000649235653246, 0.012800758474120984, 0.1454893089249137, 0.5981296294720048]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 857118804
      - 
        positions: [-0.6228494673811906, -0.7834334745928572, 0.09680722657627122, 0.04306942395262759, -0.36641344718450414, -1.5657180498965158]
        velocities: [-0.20529302210880368, -0.3005697383585561, -0.13545137925825032, -0.024767422741900545, -0.2814985710303252, -1.157285282550888]
        accelerations: [0.13194505403890708, 0.1931808980295545, 0.08705673174999976, 0.015918412123880718, 0.1809235588474427, 0.7438054522071386]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 917805364
      - 
        positions: [-0.6355558948450724, -0.802036968949654, 0.08842358495149391, 0.041536466537394734, -0.38383654874558637, -1.6373471844218832]
        velocities: [-0.19728255910603654, -0.2888416107089152, -0.1301661131002662, -0.02380100643846312, -0.2705145937601424, -1.1121284094906456]
        accelerations: [0.12184904369445328, 0.17839931823436067, 0.08039543117527484, 0.014700386524959582, 0.16707987114740483, 0.6868918559041626]
        effort: []
        time_from_start: 
          secs: 1
          nsecs: 980956041
      - 
        positions: [-0.6482623223089541, -0.8206404633064509, 0.0800399433267166, 0.04000350912216187, -0.4012596503066686, -1.7089763189472507]
        velocities: [-0.1886647987419579, -0.2762243383279022, -0.12448015497348199, -0.022761323200167626, -0.2586978880433527, -1.0635480576820304]
        accelerations: [0.13925395300377505, 0.20388186500349373, 0.09187910922530709, 0.016800188759938613, 0.19094554884636003, 0.7850074429851691]
        effort: []
        time_from_start: 
          secs: 2
          nsecs:  46670889
      - 
        positions: [-0.6609687497728359, -0.8392439576632478, 0.07165630170193929, 0.038470551706929014, -0.41868275186775084, -1.7806054534726181]
        velocities: [-0.17950808246220562, -0.2628179800008774, -0.11843859624523884, -0.021656618029486943, -0.2461421639295826, -1.0119294840052548]
        accelerations: [0.1260647870437986, 0.18457159268611859, 0.0831769589904781, 0.015208991720748972, 0.17286051442729244, 0.7106569974713725]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 115737855
      - 
        positions: [-0.6736751772367175, -0.8578474520200446, 0.06327266007716201, 0.03693759429169616, -0.436105853428833, -1.8522345879979851]
        velocities: [-0.17018895394262218, -0.2491738337357579, -0.11228987867804618, -0.020532318755894022, -0.23336373953617906, -0.9593953541496469]
        accelerations: [0.12993949560915438, 0.19024455773756815, 0.08573347364460124, 0.015676453030715163, 0.17817352951724433, 0.7324996453649099]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 188327930
      - 
        positions: [-0.6863816047005994, -0.8764509463768415, 0.0548890184523847, 0.0354046368764633, -0.45352895498991525, -1.9238637225233526]
        velocities: [-0.1586410663440077, -0.23226655886369518, -0.10467063625721647, -0.019139132514136202, -0.2175292322351649, -0.8942971826426445]
        accelerations: [0.1668343386901454, 0.24426233787304624, 0.11007651916800863, 0.020127603713750403, 0.22876387837076353, 0.9404845951748545]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 265180865
      - 
        positions: [-0.6990880321644812, -0.8950544407336384, 0.046505376827607386, 0.033871679461230444, -0.4709520565509975, -1.9954928570487198]
        velocities: [-0.15054843451787073, -0.22041812774982739, -0.09933115549250535, -0.018162803014606314, -0.2064325847625366, -0.8486771045061041]
        accelerations: [0.03315747124016553, 0.04854588993288419, 0.021877145000153753, 0.004000258259249331, 0.04546565040157303, 0.18691650148965938]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 348804492
      - 
        positions: [-0.711794459628363, -0.9136579350904352, 0.038121735202830076, 0.03233872204599758, -0.4883751581120797, -2.0671219915740875]
        velocities: [-0.14203093324807642, -0.2079476448184898, -0.09371134784889229, -0.01713521545957559, -0.19475335469632832, -0.8006619369062224]
        accelerations: [0.15873130251038364, 0.23239867373364984, 0.104730173659267, 0.019150018988778664, 0.2176529644089601, 0.8948058652380679]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 433997306
      - 
        positions: [-0.7245008870922447, -0.9322614294472321, 0.029738093578052793, 0.030805764630764725, -0.5057982596731619, -2.138751126099455]
        velocities: [-0.12697088039128213, -0.18589820494793483, -0.08377479515849552, -0.015318306673353401, -0.17410295306415802, -0.7157648598084454]
        accelerations: [0.1581009291831485, 0.2314757434553038, 0.10431425627568354, 0.01907396807131059, 0.21678859411030837, 0.8912523018162128]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 528179816
      - 
        positions: [-0.7372073145561264, -0.9508649238040291, 0.021354451953275455, 0.02927280721553186, -0.5232213612342441, -2.2103802606248224]
        velocities: [-0.10981038428388298, -0.16077350381527616, -0.07245237980006641, -0.013247991485887192, -0.15057241567529317, -0.6190270877090148]
        accelerations: [0.15821279854780865, 0.23163953151456076, 0.10438806716115012, 0.019087464466941526, 0.21694198980767637, 0.8918829358629018]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 634930550
      - 
        positions: [-0.7499137420200082, -0.969468418160826, 0.012970810328498172, 0.027739849800299005, -0.5406444627953264, -2.28200939515019]
        velocities: [-0.08758949428180496, -0.12823987444291893, -0.057791140132932704, -0.010567168870829893, -0.12010304697316646, -0.4937626793018072]
        accelerations: [0.17530826265194444, 0.2566690192200563, 0.11566757470688871, 0.021149933917128404, 0.24038335506687486, 0.9882541071909164]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 761247255
      - 
        positions: [-0.7626201694838899, -0.9880719125176228, 0.004587168703720834, 0.02620689238506614, -0.5580675643564086, -2.3536385296755573]
        velocities: [-0.05380014310566368, -0.07876884840416946, -0.03549708369576823, -0.006490677930429152, -0.0737709604052919, -0.3032841212806425]
        accelerations: [0.14974974575155586, 0.21924876665291956, 0.0988041843666298, 0.018066445807180236, 0.20533741969503455, 0.8441747071761887]
        effort: []
        time_from_start: 
          secs: 2
          nsecs: 931604006
      - 
        positions: [-0.7753265969477717, -1.0066754068744197, -0.0037964729210564754, 0.024673934969833285, -0.5754906659174909, -2.4252676642009248]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: [0.1715456256673109, 0.25116013829272216, 0.1131850043594841, 0.020695993399007337, 0.23522401295382364, 0.9670432332840968]
        effort: []
        time_from_start: 
          secs: 3
          nsecs: 316494251
    1549558322.616762493 WARN [/tmp/binarydeb/ros-kinetic-joint-trajectory-controller-0.13.4/include/joint_trajectory_controller/init_joint_trajectory.h:324(initJointTrajectory) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states, /niryo_one_follow_joint_trajectory_controller/state, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status] Dropping first 1 trajectory point(s) out of 35, as they occur before the current time.
    First valid point will be reached in 0.321s.
    
    
    achernov
    Participant
    Post count: 7

    An update from our side: we have managed to get it working by switching to node and roslibjs for the command sender. We still get jerks at the time of the trajectory update, but probably it’s because of the points of the new trajectory – the first point always has 0 velocities. Probably the planner doesn’t consider the current state of the joints.

    Thanks from your help anyway, will keep you posted on the progress.

    achernov
    Participant
    Post count: 7

    Here (https://github.com/logogin/niryo_one_ros/compare/e3aae6d609a5d485c0ab89a2b864e0ea934db095…master) is the code that we’ve wrote.

    
    def send_trajectory_direct(self, plan):
            #rospy.logwarn("Send trajectory direct message %s", plan)
            #msg = plan.joint_trajectory
            msg = JointTrajectory()
            msg.header.stamp = rospy.Time.now()
            msg.joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
            msg.points = plan.joint_trajectory.points
            #rospy.logwarn("Joint Trajectory Publisher msg: %s", msg)
            self.joint_trajectory_publisher.publish(msg)
    

    If we send a new target position to send_trajectory_direct then the arm stops at first and only then moves along the new trajectory. Do you have an idea what could be the reason for it?

    • This reply was modified 2 months, 1 week ago by  achernov.
    • This reply was modified 2 months, 1 week ago by  achernov.
    Edouard Renard
    Keymaster
    Post count: 179

    I spent some time testing some stuff about trajectory replacement, and just found a much simpler way to do that (I don’t know how I didn’t think of that before):

    –> Here in this file, comment line 282 to 288 (starting with “if self.current_goal_handle is not None:”)

    You can now send a new trajectory on the “/niryo_one/commander/robot_action”, this will send a new planned trajectory to ros_control, which will replace the current traj with the new one (and try to smooth the transition).

    I also experienced some jerk when changing trajectory, but maybe not much as what you had. Please let me know if this change in the code makes things better for you.

    The jerk comes mainly from the NiryoSteppers, but for that I don’t think we can do more than what we have now.

Viewing 8 posts - 1 through 8 (of 8 total)

You must be logged in to reply to this topic.