[Fetch] add servo-off method to fetch-interface.l #1405

tkmtnt7000 · 2021-11-02T12:55:58Z

I added :servo-off method to fetch-interface.
As we do from a joystick, this enables us to turn off arm, gripper and head servos respectively and shift to gravity compensation mode from euslisp.

I use this PR in Fetch-door-open-close demo jsk-ros-pkg/jsk_demos#1348.

fix unintended space

jsk_fetch_robot/fetcheus/fetch-interface.l

k-okada · 2021-11-04T01:22:11Z

once we send :servo-off command to robot, how do we servo-on the arm/grippers ? -- ◉ Kei Okada 2021年11月3日(水) 12:16 Shingo Kitagawa ***@***.***>:

…

***@***.**** approved this pull request. — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#1405 (review)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXGQRPGKROCN3MURAJTUKCSM7ANCNFSM5HGMRO3A> .

tkmtnt7000 · 2021-11-04T04:31:20Z

I tried servo-on from rostopic, but fetch's controller does not accept it.
So, when executing servo-on, I use the following commands.

For arm:
(send *ri* :angle-vector (send *ri* :state :potentio-vector))
For gripper:
(send *ri* :start-grasp) or (send *ri* :stop-grasp)

708yamaguchi · 2021-11-04T04:49:03Z

I think :servo-on method is also important and convenient.

  (:servo-on (&key (arm t) (gripper t) (head t))

You can move part of fetch by the following command.

(send *ri* :angle-vector (send *ri* :state :potentio-vector)) 3000 :head-controller)

jsk_robot/jsk_fetch_robot/fetcheus/fetch-interface.l

Lines 16 to 19 in e4262cb

    
           (send self :add-controller :arm-controller) 
        
           (send self :add-controller :torso-controller) 
        
           (send self :add-controller :head-controller) 
        
           ;; check if fetch_driver_msgs exists, fetch_ros in released on kinetic/melodic, but fetch_msgs is not released yet.

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/df0fc28a965168744c7d2bd1dd57e6fca8fb9d29/pr2eus/robot-interface.l#L377

You can get the gripper position by the following command.

jsk_robot/jsk_fetch_robot/fetcheus/fetch-interface.l

Line 210 in e4262cb

Example: (send self :gripper :position) => 0.00"

You can move the gripper by position.

(send *ri* :go-grasp :pos 0.00)

jsk_robot/jsk_fetch_robot/fetcheus/fetch-interface.l

Line 187 in e4262cb

(:go-grasp

708yamaguchi · 2021-11-04T04:51:46Z

If :servo-on and servo-off methods works well, please add these methods to README.md.
All fetch users will be happy.

tkmtnt7000 · 2021-11-04T04:57:56Z

@708yamaguchi
Thank you for advice.
I'll add :servo-on method, too.

708yamaguchi · 2021-11-04T05:00:30Z

By the way, what is the error message when you cannot servo on?

Could you tell me where is the cause of the error?
https://github.com/fetchrobotics/robot_controllers/blob/melodic-devel/robot_controllers_interface/src/controller_manager.cpp

tkmtnt7000 · 2021-11-04T05:25:12Z

By the way, what is the error message when you cannot servo on?

When I publish like following,

rostopic pub

rostopic pub /query_controller_states/goal robot_ctrollers_msgs/QueryControllerStatesActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  updates:
  - name: 'arm_controller/follow_joint_trajectory'
    type: ''
    state: 1"

this message is shown in /query_controller_states/status

header: 
  seq: 182161
  stamp: 
    secs: 1636002518
    nsecs: 612035174
  frame_id: ''
status_list: 
  - 
    goal_id: 
      stamp: 
        secs: 1636002502
        nsecs: 184446039
      id: "/robot_driver-1-1636002502.184446039"
    status: 4
    text: "Unable to start arm_controller/follow_joint_trajectory"

Could you tell me where is the cause of the error? fetchrobotics/robot_controllers@melodic-devel/robot_controllers_interface/src/controller_manager.cpp

I think this part causes messages above. https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers_interface/src/controller_manager.cpp#L320-L330

708yamaguchi · 2021-11-05T10:43:53Z

@tkmtnt7000

Thank you for your report.
I think controller manager log is outputted to /var/log/ros/robot.log.

If we cannot solve this problem by ourselves, let's report it to https://github.com/fetchrobotics/robot_controllers/issues

tkmtnt7000 · 2021-11-11T05:29:46Z

I found good test scripts in fetchrobotics/robot_controllers.
start_controller.py and stop_controller.py

I tried to stop and start controller with using these scripts, and the same error I previously mentioned occured.

fetch@fetch1075:~$ rosrun robot_controllers_interface stop_controller.py arm_controller/follow_joint_trajectory
[INFO] [WallTime: 1636607789.978238] [node:/stop_robot_controllers] [func:<module>]: Connecting to /query_controller_states...
[INFO] [WallTime: 1636607790.264265] [node:/stop_robot_controllers] [func:<module>]: Done.
[INFO] [WallTime: 1636607790.265782] [node:/stop_robot_controllers] [func:<module>]: Requesting that arm_controller/follow_joint_trajectory be stopped...
[INFO] [WallTime: 1636607790.268620] [node:/stop_robot_controllers] [func:<module>]: Done.
fetch@fetch1075:~$ rosrun robot_controllers_interface start_controller.py arm_controller/follow_joint_trajectory
[INFO] [WallTime: 1636607820.392276] [node:/start_robot_controllers] [func:<module>]: Connecting to /query_controller_states...
[INFO] [WallTime: 1636607820.649917] [node:/start_robot_controllers] [func:<module>]: Done.
[INFO] [WallTime: 1636607820.651295] [node:/start_robot_controllers] [func:<module>]: Requesting that arm_controller/follow_joint_trajectory be started...
[ERROR] [WallTime: 1636607820.656379] [node:/start_robot_controllers] [func:<module>]: Unable to start arm_controller/follow_joint_trajectory

/var/log/ros/robot.log output

[ERROR] [1636607820.653069079] [/robot_driver:ros.robot_controllers.FollowJointTrajectoryController]: Unable to start, action server is not active.

This log is outputted by https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers/src/follow_joint_trajectory.cpp#L145-L150

I wonder why the action server seems to be non-active...

knorth55 · 2021-11-11T06:07:34Z

https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers_interface/src/controller_manager.cpp#L170
https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers/src/follow_joint_trajectory.cpp#L166

FollowJointTrajectory is force-stopped and setAborted, which makes inactive.
I'm not checking deeply, but checking isActive() for starting seems not proper.

https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers/src/follow_joint_trajectory.cpp#L166
https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers/src/follow_joint_trajectory.cpp#L145-L150
https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers/include/robot_controllers/follow_joint_trajectory.h#L138
https://github.com/fetchrobotics/robot_controllers/blob/010855f0e6a8e058a1cbd2ad69c324cd97f0d815/robot_controllers/include/robot_controllers/follow_joint_trajectory.h#L67
https://github.com/ros/actionlib/blob/noetic-devel/actionlib/include/actionlib/server/simple_action_server_imp.h#L227-L236

can you check these lines?
you can ask if isActive() is proper or not by submitting a issue on Fetch repo.

tkmtnt7000 · 2021-11-11T07:23:37Z

Thanks for your advice.

ZebraDevs/robot_controllers@010855f/robot_controllers/src/follow_joint_trajectory.cpp#L166
ZebraDevs/robot_controllers@010855f/robot_controllers/src/follow_joint_trajectory.cpp#L145-L150
ZebraDevs/robot_controllers@010855f/robot_controllers/include/robot_controllers/follow_joint_trajectory.h#L138
ZebraDevs/robot_controllers@010855f/robot_controllers/include/robot_controllers/follow_joint_trajectory.h#L67
ros/actionlib@noetic-devel/actionlib/include/actionlib/server/simple_action_server_imp.h#L227-L236

can you check these lines?
you can ask if isActive() is proper or not by submitting a issue on Fetch repo.

I'll try checking these lines.

tkmtnt7000 · 2021-11-15T07:39:47Z

In my understanding, isActive() shows whether an active goal already exists or not, so checking isActive() when starting controllers is not proper. I think they have probably confused isActive() with representing the state of the action server itself.
I reported ZebraDevs/robot_controllers#72

tkmtnt7000 · 2021-11-16T08:55:52Z

ZebraDevs/robot_controllers#72 (comment)
I got a helpful reply from fetchrobotics. I learned a lot.
To start controllers, we need to send an action goal such as joint trajectory.
I left comment about it in :servo-on method.

Anyway to execute :servo-on,

(send self :angle-vector (send self :state :potentio-vector) 3000 :arm-controller) ;; for arm
...etc

is correct.

k-okada · 2021-11-19T06:26:55Z

jsk_fetch_robot/fetcheus/fetch-interface.l

+    ;; https://github.com/fetchrobotics/robot_controllers/issues/72
+    (when arm
+      (ros::ros-info "arm servo on")
+      (send self :angle-vector (send self :state :potentio-vector) 3000 :arm-controller)


@tkmtnt7000 If 3000 is the minimum number to re-start controller, then it is ok. Otherwise, I we'd better to set this in keyword option.

Thank you for reviewing.
I confirmed minimum time to restart contollers. It is about 60 [msec] as follows.
So I'll set :time as keyword option. I'll set default time as 500 [msec] for now.

checking minimum time to restart controller

16.irteusgl$ send *ri* :servo-on :time 0 [ INFO] [1637381001.316778487] [/fetch_eus_interface_1637380539071598730]: [arm servo on] [ INFO] [1637381001.525625578] [/fetch_eus_interface_1637380539071598730]: [;; Planned Trajectory Total Time 0.053 [sec]] [ INFO] [1637381001.525788490] [/fetch_eus_interface_1637380539071598730]: [;; Scaled Trajectory Total Time 0.053(0.053) [sec]] [ INFO] [1637381001.525845322] [/fetch_eus_interface_1637380539071598730]: [;; generated 2 points for 0.052928 sec using [arm_with_torso] group] [ INFO] [1637381001.525893210] [/fetch_eus_interface_1637380539071598730]: [;; will send to (torso_lift_joint shoulder_pan_joint shoulder_lift_joint upperarm_roll_joint elbow_flex_joint forearm_roll_joint wrist_flex_joint wrist_roll_joint)] [ INFO] [1637381001.525966171] [/fetch_eus_interface_1637380539071598730]: [;; send self :send-trajectory :arm-controller] [ INFO] [1637381001.909813113] [/fetch_eus_interface_1637380539071598730]: [;; Planned Trajectory Total Time 0.064 [sec]] [ INFO] [1637381001.909940000] [/fetch_eus_interface_1637380539071598730]: [;; Scaled Trajectory Total Time 0.064(0.064) [sec]] [ INFO] [1637381001.909974174] [/fetch_eus_interface_1637380539071598730]: [;; generated 2 points for 0.063665 sec using [arm_with_torso] group] [ INFO] [1637381001.910000743] [/fetch_eus_interface_1637380539071598730]: [;; will send to (torso_lift_joint shoulder_pan_joint shoulder_lift_joint upperarm_roll_joint elbow_flex_joint forearm_roll_joint wrist_flex_joint wrist_roll_joint)] [ INFO] [1637381001.910035502] [/fetch_eus_interface_1637380539071598730]: [;; send self :send-trajectory :torso-controller] [ INFO] [1637381001.913013224] [/fetch_eus_interface_1637380539071598730]: [head servo on] [ INFO] [1637381002.203155503] [/fetch_eus_interface_1637380539071598730]: [;; Planned Trajectory Total Time 0.066 [sec]] [ INFO] [1637381002.203274844] [/fetch_eus_interface_1637380539071598730]: [;; Scaled Trajectory Total Time 0.066(0.066) [sec]] [ INFO] [1637381002.203311426] [/fetch_eus_interface_1637380539071598730]: [;; generated 2 points for 0.065561 sec using [arm_with_torso] group] [ INFO] [1637381002.203357681] [/fetch_eus_interface_1637380539071598730]: [;; will send to (torso_lift_joint shoulder_pan_joint shoulder_lift_joint upperarm_roll_joint elbow_flex_joint forearm_roll_joint wrist_flex_joint wrist_roll_joint)] [ INFO] [1637381002.203419477] [/fetch_eus_interface_1637380539071598730]: [;; send self :angle-vector :head-controller (#<controller-action-client #X563271925ec0 /head_controller/follow_joint_trajectory>) (without planning)] [ INFO] [1637381002.210564802] [/fetch_eus_interface_1637380539071598730]: [;; send self :send-trajectory :head-controller] [ INFO] [1637381002.210622252] [/fetch_eus_interface_1637380539071598730]: [gripper servo on] #<control_msgs::grippercommandresult #X56327fd17318>

k-okada · 2021-11-21T08:38:55Z

jsk_fetch_robot/fetcheus/fetch-interface.l

+        (send (send goal-gripper-servo-off :command) :max_effort -1.0)
+        (send gripper-action :send-goal goal-gripper-servo-off))))
+  (:servo-on (&key (arm t) (gripper t) (head t) (time 500))
+    ;; the reason why :servo-off and :servo-on has different implementation is in the following issue


@tkmtnt7000 thank you, the last comment, please add method documentation like https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/ca2d919fe87f61605c10c966af32dcdf8c4f5147/pr2eus/robot-interface.l#L378-L391

(:servo-off (&key (arm t) (gripper t) (head t)) " Turn servo off for arm/gripper/head motors - arm : If you do not want to servo off the arm, set nil (default: t) - gripper : If you do not want to servo off the grippper, set nil (default: t) - head : If you do not want to servo off the head, set nil (default: t) " (:servo-on (&key (arm t) (gripper t) (head t) (time 500)) "Turm upperbody servo on - arm : If ... - gripper : ... - head : ... - time : [msec] duration to turn servo on, experimentally the minimal duration is about 60[msec] ;; the reason why :servo-off and :servo-on has different implementation is in the following issue

You can find better English explanation of each keyward...
If there is no reason for 500[msec], please set 200 or 300 as default. As you get used to robot experiments, you'll tend to set a lot of sleep and more duration time.
c.f. https://youtu.be/8o0eU_O3_OY?t=982, https://www.mis.mpg.de/fileadmin/pdf/slides_al_2616.pdf (p.81), https://hlr2016.sciencesconf.org/data/hlr_2016_ta.pdf (p.39), https://www.codyco.eu/images/pdf/2013_Humanoids/Asfour.pdf (p.17)

Thank you very much for advice.
I understand speed is important.
I add the explanation of each keyword and the usage examples of these methods.

tkmtnt7000 added 2 commits November 2, 2021 18:33

add fetch servo-off method

a6fcf1d

update variable define position

5551561

fix unintended space

tkmtnt7000 added fetch1075 fetch15 labels Nov 2, 2021

knorth55 reviewed Nov 2, 2021

View reviewed changes

jsk_fetch_robot/fetcheus/fetch-interface.l Outdated Show resolved Hide resolved

controller-action --> fetch-controller-action

dfa982d

knorth55 approved these changes Nov 3, 2021

View reviewed changes

tkmtnt7000 added 4 commits November 4, 2021 14:47

add :servo-on method

d32c84f

send current angle-vector to *fetch*

3941ccb

add explanation of servo-on and servo-off method to README.md

eb6a7ee

fix *ri* --> self

d246eb4

start torso-controller when arm servo is on

aa676c3

leave comment about servo-on

2654b9d

k-okada reviewed Nov 19, 2021

View reviewed changes

k-okada added the need fix label Nov 19, 2021

set time as keyword option

d61e00a

k-okada reviewed Nov 21, 2021

View reviewed changes

tkmtnt7000 added 3 commits November 22, 2021 13:40

update default duration time of servo-on 200[msec]

1e059b6

add docstring for servo-off and servo-on methods

6bd1695

update README.md to add keyword option in servo-on method

6200292

tkmtnt7000 mentioned this pull request Nov 22, 2021

[Fetch] Add servo off and on method for Fetch knorth55/jsk_robot#156

Merged

Merge branch 'master' into PR-fetch-add-servo-off-master

d0517ac

tkmtnt7000 removed the need fix label Apr 20, 2022

knorth55 mentioned this pull request Apr 21, 2022

[WIP] Merge Fetch15 branch #1236

Closed

Merge branch 'master' into PR-fetch-add-servo-off-master

1f6d329

k-okada added the MergeOK label Apr 25, 2022

k-okada merged commit 76a2ec6 into jsk-ros-pkg:master Apr 26, 2022

tkmtnt7000 deleted the PR-fetch-add-servo-off-master branch April 26, 2022 01:27

knorth55 mentioned this pull request Sep 11, 2022

[WIP] Merge develop/fetch branch #1593

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Fetch] add servo-off method to fetch-interface.l #1405

[Fetch] add servo-off method to fetch-interface.l #1405

tkmtnt7000 commented Nov 2, 2021

k-okada commented Nov 4, 2021 via email

tkmtnt7000 commented Nov 4, 2021

708yamaguchi commented Nov 4, 2021 •

edited

Loading

708yamaguchi commented Nov 4, 2021 •

edited

Loading

tkmtnt7000 commented Nov 4, 2021

708yamaguchi commented Nov 4, 2021

tkmtnt7000 commented Nov 4, 2021

708yamaguchi commented Nov 5, 2021

tkmtnt7000 commented Nov 11, 2021 •

edited

Loading

knorth55 commented Nov 11, 2021 •

edited

Loading

tkmtnt7000 commented Nov 11, 2021

tkmtnt7000 commented Nov 15, 2021 •

edited

Loading

tkmtnt7000 commented Nov 16, 2021

k-okada Nov 19, 2021

tkmtnt7000 Nov 20, 2021

k-okada Nov 21, 2021

tkmtnt7000 Nov 22, 2021

[Fetch] add servo-off method to fetch-interface.l #1405

[Fetch] add servo-off method to fetch-interface.l #1405

Conversation

tkmtnt7000 commented Nov 2, 2021

k-okada commented Nov 4, 2021 via email

tkmtnt7000 commented Nov 4, 2021

708yamaguchi commented Nov 4, 2021 • edited Loading

708yamaguchi commented Nov 4, 2021 • edited Loading

tkmtnt7000 commented Nov 4, 2021

708yamaguchi commented Nov 4, 2021

tkmtnt7000 commented Nov 4, 2021

708yamaguchi commented Nov 5, 2021

tkmtnt7000 commented Nov 11, 2021 • edited Loading

knorth55 commented Nov 11, 2021 • edited Loading

tkmtnt7000 commented Nov 11, 2021

tkmtnt7000 commented Nov 15, 2021 • edited Loading

tkmtnt7000 commented Nov 16, 2021

k-okada Nov 19, 2021

Choose a reason for hiding this comment

tkmtnt7000 Nov 20, 2021

Choose a reason for hiding this comment

k-okada Nov 21, 2021

Choose a reason for hiding this comment

tkmtnt7000 Nov 22, 2021

Choose a reason for hiding this comment

708yamaguchi commented Nov 4, 2021 •

edited

Loading

708yamaguchi commented Nov 4, 2021 •

edited

Loading

tkmtnt7000 commented Nov 11, 2021 •

edited

Loading

knorth55 commented Nov 11, 2021 •

edited

Loading

tkmtnt7000 commented Nov 15, 2021 •

edited

Loading