contact_states_observer

Euslisp server to check grasping and contact states.

Grasp state checking

For grasping state checking, use hand joint angle topics.
First, users should configure setting using :set-grasp-state-param such as joint angles for success-candidates and failure-candidates and checked joint names.
Then, observer program checks the difference between candidates and current hand joint angles and find the nearest candidate.
Finally, observer program publishes results and users obtain them from :get-contact-states. Setting parameter is as follows:

(list
  (list :rarm
      ;; Success and failure candidates
      (list
       (list :success #f(0.0 0.0 0.0 0.0 0.0 0.0))
       (list :fail #f(90.0 90.0 0.0 10.0 -20.0 -20.0))
       )
      ;; Checked joint names
      (list :t-1y :t-1p :f2-2r)
    ) ;; end of rarm setting
  (list :larm
    ...
    ) ;; end of larm setting
 ) ;; end of list

Contact force checking

For contact force state checking, use force sensor value and thresholding.
Observer program checks whether current force exceeds the threshold.
Observer program publishes results and users obtain them from :get-contact-states.

Use with real robot

• Start real robot (rtcd + ros bridge + servoOn)
• Run the following program on roseus

(progn
  (ros::roseus "test_grasp_check")
  ;; ROBOT Initialization. This is HRP2JSKNTS sample
  (load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")
  (unless (boundp '*robot*)
    (hrp2jsknts-init)
    (setq *robot* *hrp2jsknts*))
  ;; Loading of Contact State Client
  (load "package://contact_states_observer/euslisp/contact-states-client.l")
  (setq csc (instance contact-state-client :init))
  ;; Set grasp parameter
  (send csc :set-grasp-state-param
        (list
         ;; Do not check larm. Always returns "Off"
         (list :larm)
         ;; Set rarm parameter.
         ;;   :success is hand's reset-pose, corresponding to "On" flag
         ;;   :fail is hand's hook-pose, corresponding to "Off" flag
         ;;   Check joint angles for t-1y, t-1p, f2-2r joints
         (list :rarm
               (list
                (list :success #f(0.0 0.0 0.0 0.0 0.0 0.0))
                (list :fail #f(90.0 90.0 0.0 10.0 -20.0 -20.0))
                )
               (list :t-1y :t-1p :f2-2r))))
  ;; Test1. Set reset-pose. This should return '("On" "Off")
  (send *robot* :hand :rarm :reset-pose)
  (send *ri* :hand-angle-vector (send *robot* :hand-angle-vector) 2000)
  (send *ri* :hand-wait-interpolation)
  (unix:usleep 200000) ;; Wait for hand motion stopping
  (format t ";; '(:rarm :larm) = ~A~%" (send csc :get-contact-states '(:rarm :larm)))
  ;; Test2. Set hook-pose. This should return '("Off" "Off")
  (send *robot* :hand :rarm :hook-pose)
  (send *ri* :hand-angle-vector (send *robot* :hand-angle-vector) 2000)
  (send *ri* :hand-wait-interpolation)
  (unix:usleep 200000) ;; Wait for hand motion stopping
  (format t ";; '(:rarm :larm) = ~A~%" (send csc :get-contact-states '(:rarm :larm)))
  )

Publishing Topics

• ore_contact_states (contact_states_observer/OREContactStates)
  Array of object, robot, environment contact state.
  A contact state consists of
  • name : for example, rleg, rarm,..
  • state : On/Off (for grasping, foot, and hand contact)
  • mode : grasp, contact-force, …
• contact_grasp_states (hrpsys_ros_bridge/ContactStatesStamped)
  Contact states for all contact point (for example, rleg, lleg, rarm, larm)
• grasp_states (contact_states_observer/GraspState) Grasp state

Changelog for package contact_states_observer

0.1.17 (2023-05-28)

0.1.16 (2022-10-27)

0.1.15 (2018-05-16)

0.1.14 (2018-01-15)

0.1.13 (2017-04-18)

0.1.12 (2017-02-22)

• [contact_states_observer/euslisp/contact-states-observer.l] Check length of data list in contact states observer publishing parts.
• Contributors: Shunichi Nozawa

0.1.11 (2017-02-09)

• [contact_states_observer/euslisp/contact-states-observer.l] Use robot-init and update for latest jaxon methods
• Contributors: Shunichi Nozawa

0.1.10 (2016-12-15)

• [contact_states_observer/README.md] Add README for contact state observer.
• [contact_states_observer/euslisp/contact-states-observer.l] Update contact state observer rate.
• [contact_states_observer/euslisp/contact-states-observer.l, launch/contact_grasp_states_publisher.launch] Use robot/type instead of robot_name.
• Contributors: HRP2, Shunichi Nozawa

0.1.9 (2016-03-23)

• [contact-states-observer.l] Add :init for contact state topic initialization.
• [contact_states_observer/euslisp/contact-states-observer.l] Update for thk hand for jaxon.
• Contributors: Shunichi Nozawa

0.1.8 (2015-11-02)

0.1.7 (2015-11-01)

• [contact_states_observer] Support multiple success states. Check result symbol which has "success" in its name
• [contact_states_observer] Visualize off states as blue
• [contact_stats_observer] Fix for jaxon, which has no :hand-state methods
• [contact_states_publisher] Resolve robot_name parameter locally
• [contact_states_observer] Fix version and dependency
• [contact_states_observer] Move contact state observer package from jsk-ros-pkg-unreleased
• Contributors: Ryohei Ueda, Shunichi Nozawa

0.1.6 (2015-06-11)

0.1.5 (2015-01-08)

0.1.4 (2014-10-21)

0.1.3 (2014-10-10)

0.1.2 (2014-09-08)

0.1.1 (2014-09-04)