search

iory / rcb4

MIT License
1 stars 8 forks source link

Add PressureControl ROS server and client #69

Closed 708yamaguchi closed 3 months ago

708yamaguchi commented 4 months ago

Server usage

Set control_pressure as true.

<launch>

  <include file="$(find kxr_controller)/launch/kxr_controller.launch">
    <arg name="urdf_path" value="$(find yamaguchi_7axis_arm_no_worm_with_vacuum_base_ver3)/urdf/yamaguchi_7axis_arm_no_worm_with_vacuum_base_ver3_color.urdf" />
    <arg name="servo_config_path" value="$(find yamaguchi_7axis_arm_no_worm_with_vacuum_base_ver3)/config/servo_config.yaml" />
    <arg name="publish_sensor" value="false" />
    <arg name="control_pressure" value="true" />
    <arg name="control_loop_rate" value="20" />
  </include>

</launch>

Client usage

keep pressure under -10[kPa]

Set threshold field as -10 and release field as false.

Shell

$ rostopic pub /kxr_fullbody_controller/pressure_control_interface/goal kxr_controller/PressureControlActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  board_idx: 39
  threshold: -10.0
  release: false"

Python

roscd kxr_controller/scripts
python3 interface.py
In [1]: ri.send_pressure_control(39, threshold=-10, release=False)

Euslisp

roscd kxreus/euslisp
roseus kxr-interface.l
(kxr-init)
(send *ri* :send-pressure-control :board-idx 39 :threshold -10 :release nil)

Connect to atmosphere

Set release field as true.

Shell

$ rostopic pub /kxr_fullbody_controller/pressure_control_interface/goal kxr_controller/PressureControlActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  board_idx: 39
  threshold: -10.0
  release: true"

Python

roscd kxr_controller/scripts
python3 interface.py
In [1]: ri.send_pressure_control(39, threshold=-10, release=True)

Euslisp

roscd kxreus/euslisp
roseus kxr-interface.l
(kxr-init)
(send *ri* :send-pressure-control :board-idx 39 :threshold -10 :release t)

Get current pressure

Shell

rostopic echo /kxr_fullbody_controller/pressure/39
data: 2.004485607147217
---                      
data: -0.006360102444887161
---                    
data: 0.9152774810791016
---

Python

roscd kxr_controller/scripts
python3 interface.py
In [1]: ri.read_pressure(39)
Out[1]: data: -16.13501739501953

Euslisp

roscd kxreus/euslisp
roseus kxr-interface.l
(kxr-init)
(send (send *ri* :read-pressure :board-idx 39) :data)
-11.8201

elf files

v0.6.3 https://github.com/nakane11/control_boards/releases/tag/v0.6.3.elf v0.6.4 https://github.com/708yamaguchi/rcb4/releases/tag/v0.6.4.elf

708yamaguchi commented 4 months ago

This PR is extended version of https://github.com/iory/rcb4/pull/67.

708yamaguchi commented 4 months ago

TODO:

    def search_air_board_ids(self):
        air_boards = self.all_air_boards()
        # TODO: How to decide (id*2) or (id*2+1)
        return np.array([air_board.id*2+1 for air_board in air_boards])

https://github.com/iory/rcb4/pull/69/files#diff-812e3b28b869795e6319a6a777f6129e0e230c7970e987a7c401e4a9325b8c78R627

nakane11 commented 4 months ago

Regarding search_air_board_ids(), what about filtering self.id_vector using j.board_revision as is done in all_air_boards()? Maybe self.id_vector needs to be updated with read_jointbase_sensor_ids()

708yamaguchi commented 4 months ago

@nakane11 Thank you for your review. Does the last commit match your idea?

nakane11 commented 4 months ago

@708yamaguchi Thank you, that's exactly right.

708yamaguchi commented 3 months ago

Resolve conflict and merge master branch

iory commented 3 months ago

LGTM