From 9b56b0e9bc0a1c4b9345928c7e3dc755ce027db3 Mon Sep 17 00:00:00 2001 From: reachy2-pvt02 Date: Tue, 21 Oct 2025 17:04:38 +0200 Subject: [PATCH 1/9] inital implementation --- reachy2_gravity_compensation/package.xml | 18 ++ .../reachy2_gravity_compensation/__init__.py | 0 .../gravity_compensator.py | 234 ++++++++++++++++++ .../resource/reachy2_gravity_compensation | 0 reachy2_gravity_compensation/setup.cfg | 4 + reachy2_gravity_compensation/setup.py | 26 ++ .../test/test_copyright.py | 25 ++ .../test/test_flake8.py | 25 ++ .../test/test_pep257.py | 23 ++ 9 files changed, 355 insertions(+) create mode 100644 reachy2_gravity_compensation/package.xml create mode 100644 reachy2_gravity_compensation/reachy2_gravity_compensation/__init__.py create mode 100755 reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py create mode 100644 reachy2_gravity_compensation/resource/reachy2_gravity_compensation create mode 100644 reachy2_gravity_compensation/setup.cfg create mode 100644 reachy2_gravity_compensation/setup.py create mode 100644 reachy2_gravity_compensation/test/test_copyright.py create mode 100644 reachy2_gravity_compensation/test/test_flake8.py create mode 100644 reachy2_gravity_compensation/test/test_pep257.py diff --git a/reachy2_gravity_compensation/package.xml b/reachy2_gravity_compensation/package.xml new file mode 100644 index 0000000..bb27ccd --- /dev/null +++ b/reachy2_gravity_compensation/package.xml @@ -0,0 +1,18 @@ + + + + reachy2_gravity_compensation + 0.0.0 + TODO: Package description + reachy + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/reachy2_gravity_compensation/reachy2_gravity_compensation/__init__.py b/reachy2_gravity_compensation/reachy2_gravity_compensation/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py new file mode 100755 index 0000000..b3249a9 --- /dev/null +++ b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py @@ -0,0 +1,234 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64MultiArray + +import numpy as np +import pinocchio as pin +from pinocchio.robot_wrapper import RobotWrapper + +from rcl_interfaces.srv import GetParameters +from rcl_interfaces.msg import ParameterType + +# name: +# - l_elbow_yaw +# - r_elbow_yaw +# - r_hand_finger +# - r_elbow_pitch +# - r_shoulder_roll +# - antenna_left +# - neck_pitch +# - l_wrist_roll +# - neck_yaw +# - r_shoulder_pitch +# - drivewhl1_joint +# - drivewhl3_joint +# - l_wrist_yaw +# - tripod_joint +# - drivewhl2_joint +# - antenna_right +# - r_wrist_yaw +# - r_wrist_pitch +# - r_wrist_roll +# - l_elbow_pitch +# - l_shoulder_pitch +# - l_wrist_pitch +# - neck_roll +# - l_hand_finger +# - l_shoulder_roll + +class GravityCompensator(Node): + def __init__(self): + super().__init__('gravity_compensator') + + # payload masses (kg) + self.l_arm_object_mass = 0 + self.r_arm_object_mass = 0 + # Robot model pinocchio + self.robot = None + + # Subs and pubs + self.sub = self.create_subscription(JointState, '/joint_states', self.joint_state_cb, 10) + self.sub_object_mass = self.create_subscription(Float64MultiArray, '/set_payload_mass', self.object_mass_cb, 10) + + self.publ = self.create_publisher(Float64MultiArray, '/l_arm_forward_effort_controller/commands', 10) + self.pubr = self.create_publisher(Float64MultiArray, '/r_arm_forward_effort_controller/commands', 10) + self.pub_on = self.create_publisher(Float64MultiArray, '/forward_torque_controller/commands', 10) + self.pub_limit = self.create_publisher(Float64MultiArray, '/forward_torque_limit_controller/commands', 10) + + + # Get URDF from parameter server + self.cli = self.create_client(GetParameters, '/robot_state_publisher/get_parameters') + if not self.cli.wait_for_service(timeout_sec=60.0): + self.get_logger().error(f'Service not available: /robot_state_publisher/get_parameters') + rclpy.shutdown(); return + req = GetParameters.Request() + req.names = ['robot_description'] + future = self.cli.call_async(req) + future.add_done_callback(self._on_robot_description) + + # Initial torque limits + torque_limit = Float64MultiArray() + torque_limit.data = [0.01]*21 + self.pub_limit.publish(torque_limit) + + # enable torque controllers + torque_on = Float64MultiArray() + torque_on.data = [1.0]*11 + self.pub_on.publish(torque_on) + + + + def _on_robot_description(self, future): + try: + response = future.result() + if response: + urdf_xml = response.values[0].string_value + print("URDF loaded from parameter server") + + # Robot model from parameter + self.robot = self.load_robot_from_param(urdf_xml) + self.q = pin.neutral(self.robot.model) + + print("Robot model is ready - starting gravity compensation") + + except Exception as e: + self.get_logger().error(f"Error getting parameters: {e}") + + + # callback to update object masses carried by the arms + def object_mass_cb(self, msg: Float64MultiArray): + + try: + if len(msg.data) >= 2: + self.l_arm_object_mass = msg.data[0] + self.r_arm_object_mass = msg.data[1] + self.get_logger().info(f"Updated object masses: Left Arm = {self.l_arm_object_mass}, Right Arm = {self.r_arm_object_mass}") + except Exception as e: + self.get_logger().error(f"Error in object_mass_cb: {e}") + + + # load robot model from URDF string and reduce it by locking unneeded joints + def load_robot_from_param(self, urdf_xml: str) -> RobotWrapper: + + print("Loading robot model from String URDF") + model = pin.buildModelFromXML(urdf_xml) + robot = pin.RobotWrapper(model) + + print("reducing model") + # Optional: reduce model + tolock = [ + 'tripod_joint', + #'l_shoulder_pitch', + #'l_shoulder_roll', + #'l_elbow_yaw', + #'l_elbow_pitch', + + #'l_wrist_roll', + #'l_wrist_pitch', + #'l_wrist_yaw', + + 'l_hand_finger', + 'l_hand_finger_proximal', + 'l_hand_finger_distal', + 'l_hand_finger_proximal_mimic', + 'l_hand_finger_distal_mimic', + 'left_bar_joint_mimic', + 'left_bar_prism_joint_mimic', + 'neck_roll', + 'neck_pitch', + 'neck_yaw', + 'antenna_left', + 'antenna_right', + + # 'r_shoulder_pitch', + # 'r_shoulder_roll', + # 'r_elbow_yaw', + # 'r_elbow_pitch', + #'r_wrist_roll', + #'r_wrist_pitch', + #'r_wrist_yaw', + + 'r_hand_finger', + 'r_hand_finger_proximal', + 'r_hand_finger_distal', + 'r_hand_finger_proximal_mimic', + 'r_hand_finger_distal_mimic', + 'right_bar_joint_mimic', + 'right_bar_prism_joint_mimic' + ] + + # Get the ID of all existing joints + lvals = np.zeros(robot.nq) + jointsToLockIDs = [] + for i,jn in enumerate(tolock): + if robot.model.existJointName(jn): + jointsToLockIDs.append(robot.model.getJointId(jn)) + model = pin.buildReducedModel(robot.model, jointsToLockIDs, lvals) + data = model.createData() + + robot.model = model + robot.data = data + print("Robot model loaded and reduced") + return robot + + def joint_state_cb(self, msg: JointState): + + if self.robot is None: + return + + try: + q = np.zeros(self.robot.nq) + names = [""]* self.robot.nq + for i, name in enumerate(msg.name): + if name in self.robot.model.names: + idx = self.robot.model.getJointId(name) + q[idx - 1] = msg.position[i] # -1 since 0 is universe + names[idx - 1] = name + + self.q = q + # Compute gravity torques + tau = pin.computeGeneralizedGravity(self.robot.model, self.robot.data, self.q) + + # extract left and right arm torques + tau_l = np.array(tau[:7].copy()) + tau_r = np.array(tau[7:].copy()) + + if self.l_arm_object_mass != 0 : + joint_id = self.robot.model.getFrameId("l_arm_tip") + J = pin.computeFrameJacobian(self.robot.model, + self.robot.data, + self.q, + joint_id, + reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] + + tau_l = tau_l + J[:,:7].T @ np.array([0,0, self.l_arm_object_mass*9.81]) + + if self.r_arm_object_mass != 0: + joint_id = self.robot.model.getFrameId("r_arm_tip") + J = pin.computeFrameJacobian(self.robot.model, + self.robot.data, + self.q, + joint_id, + reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] + + tau_r = tau_r + J[:,7:].T @ np.array([0,0, self.r_arm_object_mass*9.81]) + + + effort_msg = Float64MultiArray() + effort_msg.data = tau_l.tolist() + self.publ.publish(effort_msg) + effort_msg.data = tau_r.tolist() + self.pubr.publish(effort_msg) + + + except Exception as e: + self.get_logger().error(f"Error in joint_state_cb: {e}") + +def main(args=None): + rclpy.init(args=args) + node = GravityCompensator() + rclpy.spin(node) + +if __name__ == '__main__': + main() diff --git a/reachy2_gravity_compensation/resource/reachy2_gravity_compensation b/reachy2_gravity_compensation/resource/reachy2_gravity_compensation new file mode 100644 index 0000000..e69de29 diff --git a/reachy2_gravity_compensation/setup.cfg b/reachy2_gravity_compensation/setup.cfg new file mode 100644 index 0000000..93b675b --- /dev/null +++ b/reachy2_gravity_compensation/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/reachy2_gravity_compensation +[install] +install_scripts=$base/lib/reachy2_gravity_compensation diff --git a/reachy2_gravity_compensation/setup.py b/reachy2_gravity_compensation/setup.py new file mode 100644 index 0000000..4b363c1 --- /dev/null +++ b/reachy2_gravity_compensation/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'reachy2_gravity_compensation' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools', 'pin==2.7.0'], + zip_safe=True, + maintainer='reachy', + maintainer_email='reachy@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + "console_scripts": [ + "gravity_compensator = reachy2_gravity_compensation.gravity_compensator:main", + ], + }, +) diff --git a/reachy2_gravity_compensation/test/test_copyright.py b/reachy2_gravity_compensation/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/reachy2_gravity_compensation/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/reachy2_gravity_compensation/test/test_flake8.py b/reachy2_gravity_compensation/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/reachy2_gravity_compensation/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/reachy2_gravity_compensation/test/test_pep257.py b/reachy2_gravity_compensation/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/reachy2_gravity_compensation/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 067c6d23958b8e76bbd12c9cde6f1aec944b78f9 Mon Sep 17 00:00:00 2001 From: reachy2-pvt02 Date: Wed, 22 Oct 2025 16:37:37 +0200 Subject: [PATCH 2/9] add scripts + mass estimation --- .../gravity_compensator.py | 46 ++++++++++++------- .../scripts/make_complient.sh | 2 + .../scripts/set_payload.sh | 16 +++++++ 3 files changed, 47 insertions(+), 17 deletions(-) create mode 100644 reachy2_gravity_compensation/scripts/make_complient.sh create mode 100644 reachy2_gravity_compensation/scripts/set_payload.sh diff --git a/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py index b3249a9..3ea161d 100755 --- a/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py +++ b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py @@ -55,6 +55,7 @@ def __init__(self): self.pubr = self.create_publisher(Float64MultiArray, '/r_arm_forward_effort_controller/commands', 10) self.pub_on = self.create_publisher(Float64MultiArray, '/forward_torque_controller/commands', 10) self.pub_limit = self.create_publisher(Float64MultiArray, '/forward_torque_limit_controller/commands', 10) + self.pub_object_mass = self.create_publisher(Float64MultiArray, '/current_payload_mass', 10) # Get URDF from parameter server @@ -179,12 +180,15 @@ def joint_state_cb(self, msg: JointState): try: q = np.zeros(self.robot.nq) + tau_k = np.zeros(self.robot.nv) names = [""]* self.robot.nq for i, name in enumerate(msg.name): if name in self.robot.model.names: idx = self.robot.model.getJointId(name) q[idx - 1] = msg.position[i] # -1 since 0 is universe names[idx - 1] = name + tau_k[idx - 1] = msg.effort[i] + self.q = q # Compute gravity torques @@ -194,33 +198,41 @@ def joint_state_cb(self, msg: JointState): tau_l = np.array(tau[:7].copy()) tau_r = np.array(tau[7:].copy()) + joint_id = self.robot.model.getFrameId("l_arm_tip") + J = pin.computeFrameJacobian(self.robot.model, + self.robot.data, + self.q, + joint_id, + reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] + + l_m = self.l_arm_object_mass if self.l_arm_object_mass != 0 : - joint_id = self.robot.model.getFrameId("l_arm_tip") - J = pin.computeFrameJacobian(self.robot.model, - self.robot.data, - self.q, - joint_id, - reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] - tau_l = tau_l + J[:,:7].T @ np.array([0,0, self.l_arm_object_mass*9.81]) + else: + l_m = (np.linalg.pinv(J[:,:7].T)@(tau_k[:7] - tau_l))[2]/9.81 - if self.r_arm_object_mass != 0: - joint_id = self.robot.model.getFrameId("r_arm_tip") - J = pin.computeFrameJacobian(self.robot.model, - self.robot.data, - self.q, - joint_id, - reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] + joint_id = self.robot.model.getFrameId("r_arm_tip") + J = pin.computeFrameJacobian(self.robot.model, + self.robot.data, + self.q, + joint_id, + reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] + r_m = self.r_arm_object_mass + if self.r_arm_object_mass != 0: tau_r = tau_r + J[:,7:].T @ np.array([0,0, self.r_arm_object_mass*9.81]) - - + else: + r_m = (np.linalg.pinv(J[:,7:].T)@(tau_k[7:] - tau_r))[2]/9.81 + effort_msg = Float64MultiArray() effort_msg.data = tau_l.tolist() self.publ.publish(effort_msg) effort_msg.data = tau_r.tolist() self.pubr.publish(effort_msg) - + + object_mass_msg = Float64MultiArray() + object_mass_msg.data = [l_m, r_m] + self.pub_object_mass.publish(object_mass_msg) except Exception as e: self.get_logger().error(f"Error in joint_state_cb: {e}") diff --git a/reachy2_gravity_compensation/scripts/make_complient.sh b/reachy2_gravity_compensation/scripts/make_complient.sh new file mode 100644 index 0000000..7a948c5 --- /dev/null +++ b/reachy2_gravity_compensation/scripts/make_complient.sh @@ -0,0 +1,2 @@ +ros2 topic pub --once /forward_torque_controller/commands std_msgs/Float64MultiArray "{ data:[1,1,1,1,1,1,1,1,1,1,1]}" +ros2 topic pub --once /forward_torque_limit_controller/commands std_msgs/Float64MultiArray "{data:[0.001, 0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001]}" diff --git a/reachy2_gravity_compensation/scripts/set_payload.sh b/reachy2_gravity_compensation/scripts/set_payload.sh new file mode 100644 index 0000000..9613fad --- /dev/null +++ b/reachy2_gravity_compensation/scripts/set_payload.sh @@ -0,0 +1,16 @@ +# receives payload mass (kg) for two arms and publish it to the appropriate topic +# Usage: ./set_payload.sh +# if no argument is given, both payloads are set to 0 kg + +if [ -z "$1" ]; then + LEFT_PAYLOAD=0.0 + else + LEFT_PAYLOAD=$1 + fi +if [ -z "$2" ]; then + RIGHT_PAYLOAD=0.0 + else + RIGHT_PAYLOAD=$2 + fi + +ros2 topic pub --once /set_payload_mass std_msgs/Float64MultiArray "{ data:[$LEFT_PAYLOAD, $RIGHT_PAYLOAD]}" \ No newline at end of file From c65f9a49ecbd8f79079d9533ab4152b7f6d31de7 Mon Sep 17 00:00:00 2001 From: reachy2-pvt02 Date: Wed, 22 Oct 2025 17:42:51 +0200 Subject: [PATCH 3/9] remove pubs for limts and torque on --- .../gravity_compensator.py | 108 +++++++++++------- 1 file changed, 66 insertions(+), 42 deletions(-) diff --git a/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py index 3ea161d..37bfb1b 100755 --- a/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py +++ b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py @@ -49,12 +49,11 @@ def __init__(self): # Subs and pubs self.sub = self.create_subscription(JointState, '/joint_states', self.joint_state_cb, 10) + self.sub_torque_on = self.create_subscription(Float64MultiArray, '/forward_torque_controller/commands', self.torque_on_cb, 10) self.sub_object_mass = self.create_subscription(Float64MultiArray, '/set_payload_mass', self.object_mass_cb, 10) self.publ = self.create_publisher(Float64MultiArray, '/l_arm_forward_effort_controller/commands', 10) self.pubr = self.create_publisher(Float64MultiArray, '/r_arm_forward_effort_controller/commands', 10) - self.pub_on = self.create_publisher(Float64MultiArray, '/forward_torque_controller/commands', 10) - self.pub_limit = self.create_publisher(Float64MultiArray, '/forward_torque_limit_controller/commands', 10) self.pub_object_mass = self.create_publisher(Float64MultiArray, '/current_payload_mass', 10) @@ -67,16 +66,25 @@ def __init__(self): req.names = ['robot_description'] future = self.cli.call_async(req) future.add_done_callback(self._on_robot_description) - - # Initial torque limits - torque_limit = Float64MultiArray() - torque_limit.data = [0.01]*21 - self.pub_limit.publish(torque_limit) - # enable torque controllers - torque_on = Float64MultiArray() - torque_on.data = [1.0]*11 - self.pub_on.publish(torque_on) + self.create_timer(0.005, self.update) # 200Hz update rate + + self.r_arm_on = False + self.l_arm_on = False + + + def torque_on_cb(self, msg: Float64MultiArray): + # if torque is off, reset object masses to zero + try: + self.l_arm_on = True + self.r_arm_on = True + for i, val in enumerate(msg.data): + if i > 1 and i < 4: + self.l_arm_on = self.l_arm_on and bool(val) + elif i > 5 and i < 8: + self.r_arm_on = self.r_arm_on and bool(val) + except Exception as e: + self.get_logger().error(f"Error in torque_on_cb: {e}") @@ -189,28 +197,44 @@ def joint_state_cb(self, msg: JointState): names[idx - 1] = name tau_k[idx - 1] = msg.effort[i] - self.q = q - # Compute gravity torques - tau = pin.computeGeneralizedGravity(self.robot.model, self.robot.data, self.q) + self.tau_k = tau_k + + except Exception as e: + self.get_logger().error(f"Error in joint_state_cb: {e}") - # extract left and right arm torques + # function that will run continously + def update(self): + + # Compute gravity torques + tau = pin.computeGeneralizedGravity(self.robot.model, self.robot.data, self.q) + + tau_l = np.zeros(7) + l_m = 0 + tau_r = np.zeros(7) + r_m = 0 + + # extract left and right arm torques + if self.l_arm_on: tau_l = np.array(tau[:7].copy()) - tau_r = np.array(tau[7:].copy()) - + # get the jacobian at the left arm tip joint_id = self.robot.model.getFrameId("l_arm_tip") J = pin.computeFrameJacobian(self.robot.model, - self.robot.data, - self.q, - joint_id, - reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] - - l_m = self.l_arm_object_mass + self.robot.data, + self.q, + joint_id, + reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] + + # left payload mass estimation + l_m = (np.linalg.pinv(J[:,:7].T)@(self.tau_k[:7] - tau_l))[2]/9.81 + # add payload gravity compensation - if payload mass is not zero if self.l_arm_object_mass != 0 : tau_l = tau_l + J[:,:7].T @ np.array([0,0, self.l_arm_object_mass*9.81]) - else: - l_m = (np.linalg.pinv(J[:,:7].T)@(tau_k[:7] - tau_l))[2]/9.81 - + + if self.r_arm_on: + tau_r = np.array(tau[7:14].copy()) + + # get the jacobian at the right arm tip joint_id = self.robot.model.getFrameId("r_arm_tip") J = pin.computeFrameJacobian(self.robot.model, self.robot.data, @@ -218,25 +242,25 @@ def joint_state_cb(self, msg: JointState): joint_id, reference_frame=pin.LOCAL_WORLD_ALIGNED)[:3,:] - r_m = self.r_arm_object_mass + # right payload mass estimation + r_m = (np.linalg.pinv(J[:,7:].T)@(self.tau_k[7:] - tau_r))[2]/9.81 + # add payload gravity compensation - if payload mass is not zero if self.r_arm_object_mass != 0: tau_r = tau_r + J[:,7:].T @ np.array([0,0, self.r_arm_object_mass*9.81]) - else: - r_m = (np.linalg.pinv(J[:,7:].T)@(tau_k[7:] - tau_r))[2]/9.81 - - effort_msg = Float64MultiArray() - effort_msg.data = tau_l.tolist() - self.publ.publish(effort_msg) - effort_msg.data = tau_r.tolist() - self.pubr.publish(effort_msg) + - object_mass_msg = Float64MultiArray() - object_mass_msg.data = [l_m, r_m] - self.pub_object_mass.publish(object_mass_msg) - - except Exception as e: - self.get_logger().error(f"Error in joint_state_cb: {e}") - + effort_msg = Float64MultiArray() + effort_msg.data = tau_l.tolist() + self.publ.publish(effort_msg) + effort_msg.data = tau_r.tolist() + self.pubr.publish(effort_msg) + + object_mass_msg = Float64MultiArray() + object_mass_msg.data = [l_m, r_m] + self.pub_object_mass.publish(object_mass_msg) + + + def main(args=None): rclpy.init(args=args) node = GravityCompensator() From d84d501f32a6a39b167c9f7e0cf98804993e0586 Mon Sep 17 00:00:00 2001 From: reachy2-pvt02 Date: Thu, 23 Oct 2025 14:21:44 +0200 Subject: [PATCH 4/9] added the readme --- README.md | 1 + reachy2_gravity_compensation/README.md | 30 ++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) create mode 100644 reachy2_gravity_compensation/README.md diff --git a/README.md b/README.md index cc568ad..1981321 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ * [dynamic_state_router](./dynamic_state_router/) - ROS2 node to simplify the use of forward controllers * [pollen_kdl_kinematics](./pollen_kdl_kinematics/) - ROS2 bindings of KDL kinematics * [pollen_goto](./pollen_goto/) - ROS2 Action server and client to perform gotos (joint interpolations) +* [reachy2_gravity_compensation](./reachy2_gravity_compensation/) - Gravity compensation controller for Reachy2 robot See the readme of each package for more information. diff --git a/reachy2_gravity_compensation/README.md b/reachy2_gravity_compensation/README.md new file mode 100644 index 0000000..4fad0ee --- /dev/null +++ b/reachy2_gravity_compensation/README.md @@ -0,0 +1,30 @@ +# This node provides gravity compensation for the Reachy2 robot using ROS2 control framework. + +It subscribes to the robot's joint states and computes the necessary torques to counteract gravity, allowing for easier manipulation and interaction with the robot. +By deafult, it is launched in the background by the ros launch and is always on. + + +The node will compensate for the gravity of both arms and allows setting an additional weight at the end-effector of each arm using the topic `/set_payload_mass`. +This topic receives two float64 values representing the mass (in kg) to be added to the left and right arms, respectively. + +For example to set a payload of 0.5 kg on the left arm and 1.0 kg on the right arm, you can use the following command: + +```bash +ros2 topic pub /set_payload_mass std_msgs/msg/Float64MultiArray "{data: [0.5, 1.0]}" +``` + +or you can also use a script provided in the `scripts` folder: + +```bash +sh set_payload 0.5 1.0 +``` + +The node also allows to estimate the current payload mass of each arm which is published continuously on the topic `/current_payload_mass` as a Float64MultiArray message containing two values: the estimated mass (in kg) for the left and right arms, respectively. + +To run the node manually, you can use the following command: + +```bash +ros2 topic echo /current_payload_mass +``` + + From 36e57f4894b27a4da2b6461b6141e3b056beba4b Mon Sep 17 00:00:00 2001 From: reachy2-pvt02 Date: Thu, 23 Oct 2025 14:52:15 +0200 Subject: [PATCH 5/9] script rename + readme --- reachy2_gravity_compensation/README.md | 20 ++++++++++++++++++- .../{make_complient.sh => make_compliant.sh} | 0 2 files changed, 19 insertions(+), 1 deletion(-) rename reachy2_gravity_compensation/scripts/{make_complient.sh => make_compliant.sh} (100%) diff --git a/reachy2_gravity_compensation/README.md b/reachy2_gravity_compensation/README.md index 4fad0ee..4f8eecf 100644 --- a/reachy2_gravity_compensation/README.md +++ b/reachy2_gravity_compensation/README.md @@ -3,10 +3,28 @@ It subscribes to the robot's joint states and computes the necessary torques to counteract gravity, allowing for easier manipulation and interaction with the robot. By deafult, it is launched in the background by the ros launch and is always on. +To make the robot go to the gravity compensation mode, you will have to: +- enable the actuators: torque_on +- set the torque limits to a very small number (e.g. 0.001 - 0.1%) + +For example, you can use the following commands: +```bash +ros2 topic pub --once /forward_torque_controller/commands std_msgs/Float64MultiArray "{ data:[1,1,1,1,1,1,1,1,1,1,1]}" +ros2 topic pub --once /forward_torque_limit_controller/commands std_msgs/Float64MultiArray "{data:[0.001, 0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001]}" +``` + + +Or you cane use the script provided in the `scripts` folder: + +```bash +sh make_compliant.sh +``` The node will compensate for the gravity of both arms and allows setting an additional weight at the end-effector of each arm using the topic `/set_payload_mass`. This topic receives two float64 values representing the mass (in kg) to be added to the left and right arms, respectively. + + For example to set a payload of 0.5 kg on the left arm and 1.0 kg on the right arm, you can use the following command: ```bash @@ -16,7 +34,7 @@ ros2 topic pub /set_payload_mass std_msgs/msg/Float64MultiArray "{data: [0.5, 1. or you can also use a script provided in the `scripts` folder: ```bash -sh set_payload 0.5 1.0 +sh set_payload.sh 0.5 1.0 ``` The node also allows to estimate the current payload mass of each arm which is published continuously on the topic `/current_payload_mass` as a Float64MultiArray message containing two values: the estimated mass (in kg) for the left and right arms, respectively. diff --git a/reachy2_gravity_compensation/scripts/make_complient.sh b/reachy2_gravity_compensation/scripts/make_compliant.sh similarity index 100% rename from reachy2_gravity_compensation/scripts/make_complient.sh rename to reachy2_gravity_compensation/scripts/make_compliant.sh From 281ca62d3869038fd005d5102b70f7c35d6694fb Mon Sep 17 00:00:00 2001 From: reachy2-pvt02 Date: Thu, 23 Oct 2025 15:06:27 +0200 Subject: [PATCH 6/9] zero instead of 0.001 --- reachy2_gravity_compensation/README.md | 5 ++--- reachy2_gravity_compensation/scripts/make_compliant.sh | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/reachy2_gravity_compensation/README.md b/reachy2_gravity_compensation/README.md index 4f8eecf..d9c112e 100644 --- a/reachy2_gravity_compensation/README.md +++ b/reachy2_gravity_compensation/README.md @@ -5,15 +5,14 @@ By deafult, it is launched in the background by the ros launch and is always on. To make the robot go to the gravity compensation mode, you will have to: - enable the actuators: torque_on -- set the torque limits to a very small number (e.g. 0.001 - 0.1%) +- set the torque limits to zero or a very small number (e.g.<0.1% - 0.001) For example, you can use the following commands: ```bash ros2 topic pub --once /forward_torque_controller/commands std_msgs/Float64MultiArray "{ data:[1,1,1,1,1,1,1,1,1,1,1]}" -ros2 topic pub --once /forward_torque_limit_controller/commands std_msgs/Float64MultiArray "{data:[0.001, 0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001]}" +ros2 topic pub --once /forward_torque_limit_controller/commands std_msgs/Float64MultiArray "{data:[0.0, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]}" ``` - Or you cane use the script provided in the `scripts` folder: ```bash diff --git a/reachy2_gravity_compensation/scripts/make_compliant.sh b/reachy2_gravity_compensation/scripts/make_compliant.sh index 7a948c5..604e459 100644 --- a/reachy2_gravity_compensation/scripts/make_compliant.sh +++ b/reachy2_gravity_compensation/scripts/make_compliant.sh @@ -1,2 +1,2 @@ ros2 topic pub --once /forward_torque_controller/commands std_msgs/Float64MultiArray "{ data:[1,1,1,1,1,1,1,1,1,1,1]}" -ros2 topic pub --once /forward_torque_limit_controller/commands std_msgs/Float64MultiArray "{data:[0.001, 0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001]}" +ros2 topic pub --once /forward_torque_limit_controller/commands std_msgs/Float64MultiArray "{data:[0.000, 0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000,0.000]}" From 1a06c7dbf2c9eafaba9b708c27e7d0d49bbc43d9 Mon Sep 17 00:00:00 2001 From: RemiFabre Date: Wed, 29 Oct 2025 08:57:05 +0100 Subject: [PATCH 7/9] Fixed initial start condition to avoid crash if URDF was not read on time. --- .../reachy2_gravity_compensation/gravity_compensator.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py index 37bfb1b..f24cda1 100755 --- a/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py +++ b/reachy2_gravity_compensation/reachy2_gravity_compensation/gravity_compensator.py @@ -93,13 +93,13 @@ def _on_robot_description(self, future): response = future.result() if response: urdf_xml = response.values[0].string_value - print("URDF loaded from parameter server") + self.get_logger().info("URDF loaded from parameter server") # Robot model from parameter self.robot = self.load_robot_from_param(urdf_xml) self.q = pin.neutral(self.robot.model) - print("Robot model is ready - starting gravity compensation") + self.get_logger().info("Robot model is ready - starting gravity compensation") except Exception as e: self.get_logger().error(f"Error getting parameters: {e}") @@ -205,6 +205,8 @@ def joint_state_cb(self, msg: JointState): # function that will run continously def update(self): + if self.robot is None: + return # Compute gravity torques tau = pin.computeGeneralizedGravity(self.robot.model, self.robot.data, self.q) From b41f115135198938cc78fe0590e930ff0dc343c6 Mon Sep 17 00:00:00 2001 From: reachy2-pvt04 Date: Wed, 29 Oct 2025 11:31:47 +0100 Subject: [PATCH 8/9] Different torque limits for shoulder, elbow, wrist --- useful_commands/infinite_square_test.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/useful_commands/infinite_square_test.py b/useful_commands/infinite_square_test.py index 5731be5..aeb14fb 100644 --- a/useful_commands/infinite_square_test.py +++ b/useful_commands/infinite_square_test.py @@ -11,7 +11,9 @@ from reachy2_sdk.parts.joints_based_part import JointsBasedPart # These are integer values between 0 and 100 -TORQUE_LIMIT=80 +TORQUE_LIMIT_SHOULDER=20.0 +TORQUE_LIMIT_ELBOW=10.0 +TORQUE_LIMIT_WRIST=20.0 SPEED_LIMIT=25 @@ -36,16 +38,24 @@ def build_pose_matrix(x: float, y: float, z: float) -> npt.NDArray[np.float64]: ] ) -def set_speed_and_torque_limits(reachy, torque_limit=100, speed_limit=25) -> None: +def set_speed_and_torque_limits(reachy, torque_limit_shoulder=100, torque_limit_elbow=100, torque_limit_wrist=100, speed_limit=25) -> None: """Set back speed and torque limits of all parts to given value.""" if not reachy.info: reachy._logger.warning("Reachy is not connected!") return + reachy.r_arm.shoulder.set_torque_limits(torque_limit_shoulder) + reachy.r_arm.elbow.set_torque_limits(torque_limit_elbow) + reachy.r_arm.wrist.set_torque_limits(torque_limit_wrist) + reachy.l_arm.shoulder.set_torque_limits(torque_limit_shoulder) + reachy.l_arm.elbow.set_torque_limits(torque_limit_elbow) + reachy.l_arm.wrist.set_torque_limits(torque_limit_wrist) + for part in reachy.info._enabled_parts.values(): if issubclass(type(part), JointsBasedPart): part.set_speed_limits(speed_limit) - part.set_torque_limits(torque_limit) + #part.set_torque_limits(torque_limit) + time.sleep(0.5) def draw_square(reachy: ReachySDK) -> None: @@ -148,7 +158,7 @@ def goto_to_point_A(reachy: ReachySDK) -> None: print("Turning on Reachy") reachy.turn_on() - set_speed_and_torque_limits(reachy, torque_limit=TORQUE_LIMIT, speed_limit=SPEED_LIMIT) + set_speed_and_torque_limits(reachy, torque_limit_shoulder=TORQUE_LIMIT_SHOULDER, torque_limit_elbow=TORQUE_LIMIT_ELBOW, torque_limit_wrist=TORQUE_LIMIT_WRIST, speed_limit=SPEED_LIMIT) time.sleep(0.2) try : From b85c21ed26abfa2e838e4a4e47c98bc748f7202d Mon Sep 17 00:00:00 2001 From: reachy2-pvt04 Date: Wed, 29 Oct 2025 12:00:37 +0100 Subject: [PATCH 9/9] Helper scripts for grippers --- useful_commands/gripper_close.py | 15 +++++++++++++++ useful_commands/gripper_open.py | 15 +++++++++++++++ 2 files changed, 30 insertions(+) create mode 100644 useful_commands/gripper_close.py create mode 100644 useful_commands/gripper_open.py diff --git a/useful_commands/gripper_close.py b/useful_commands/gripper_close.py new file mode 100644 index 0000000..89e680e --- /dev/null +++ b/useful_commands/gripper_close.py @@ -0,0 +1,15 @@ +import time + +import numpy as np +from reachy2_sdk import ReachySDK + + +def gripper_test(): + print("Trying to connect on localhost Reachy...") + reachy = ReachySDK(host="localhost") + reachy.r_arm.gripper.close() + reachy.l_arm.gripper.close() + +if __name__ == "__main__": + # main_test() + gripper_test() diff --git a/useful_commands/gripper_open.py b/useful_commands/gripper_open.py new file mode 100644 index 0000000..90a33e2 --- /dev/null +++ b/useful_commands/gripper_open.py @@ -0,0 +1,15 @@ +import time + +import numpy as np +from reachy2_sdk import ReachySDK + + +def gripper_test(): + print("Trying to connect on localhost Reachy...") + reachy = ReachySDK(host="localhost") + reachy.r_arm.gripper.open() + reachy.l_arm.gripper.open() + +if __name__ == "__main__": + # main_test() + gripper_test()