Forum Replies Created

Viewing 4 posts - 1 through 4 (of 4 total)
  • Author
    Posts
  • achernov
    Participant
    Post count: 7
    in reply to: Queuing move goals #4697

    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 8 months, 1 week ago by  achernov.
    • This reply was modified 8 months, 1 week ago by  achernov.
    achernov
    Participant
    Post count: 7
    in reply to: Queuing move goals #4693

    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
    in reply to: Queuing move goals #4690

    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
    in reply to: Queuing move goals #4659

    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 8 months, 2 weeks ago by  achernov.
Viewing 4 posts - 1 through 4 (of 4 total)