diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a9bafbb --- /dev/null +++ b/.gitignore @@ -0,0 +1,10 @@ +build/ +install/ +log/ + +**/__pycache__/ +*.pyc +*.pyo + +.DS_Store +.repo-backups/ diff --git a/README.md b/README.md index c630125..fec27bb 100644 --- a/README.md +++ b/README.md @@ -1,54 +1,218 @@ # rx1 ## Introduction -RX1 Humanoid is an opensource robot project hosted on [https://red-rabbit-robotics.ghost.io/](https://red-rabbit-robotics.ghost.io/). This repo is a ROS1 meta package containing basic functioning packages of the RX1 humanoid including the following: +RX1 Humanoid is an open-source robot project from Red Rabbit Robotics. -* rx1_description: URDF file -* rx1_motor: Feetech servo motor control -* rx1_ik: Inverse Kinematics demo -* rx1_bringup: Launch everything -* rx1_gazebo: Gazebo simulation +This repository is a ROS 2 conversion of the original ROS 1 RX1 project, with a MoveIt 2 setup added for planning and RViz-based manipulation workflows. + +References: + +* Original project page: [https://red-rabbit-robotics.ghost.io/](https://red-rabbit-robotics.ghost.io/) +* Original ROS 1 repository: [https://github.com/Red-Rabbit-Robotics/rx1](https://github.com/Red-Rabbit-Robotics/rx1) + +## Current Status + +This repo has been ported toward ROS 2 and currently supports: + +* ROS 2 package metadata and `colcon` builds +* RViz robot visualization through `rx1_description` +* MoveIt 2 planning setup through `rx1_moveit_config` +* ROS 2 motor package build through `rx1_motor` +* ROS 2-compatible `feetech_lib` for real servo communication + +Current direction: + +* MoveIt 2 is the preferred manipulation path +* the old `rx1_ik` package is still present, but MoveIt 2 is the main ROS 2 planning interface + +Known gaps: + +* Gazebo and MoveIt execution now work on the maintained Jazzy path, but simulator startup still depends on the ROS 2 / Gazebo Harmonic stack being available locally +* `ik_solver_lib` is still needed only if you want to keep the old custom IK workflow +* hardware use still requires the proper serial device, servo IDs, and safe testing on the real robot + +## Packages + +Main packages in this repository: + +* `rx1_description`: URDF/Xacro model, meshes, RViz config, and robot state publishing launches +* `rx1_motor`: ROS 2 motor node for Feetech-based hardware control +* `rx1_ik`: legacy custom IK path kept from the original project +* `rx1_bringup`: top-level launch integration +* `rx1_gazebo`: Gazebo simulation package +* `rx1_moveit_config`: MoveIt 2 configuration, SRDF, RViz config, and demo launch + +External dependency used for real servos: + +* `feetech_lib`: ROS 2-converted Feetech communication library + +## ROS 1 to ROS 2 Notes + +Main command and workflow changes: + +* `catkin_make` or `catkin build` -> `colcon build` +* `roslaunch ` -> `ros2 launch ` +* `rosrun ` -> `ros2 run ` +* ROS 1 parameter server usage -> ROS 2 declared parameters and YAML parameter files +* `tf` static publishers -> `tf2_ros/static_transform_publisher` +* ROS 1 launch XML -> ROS 2 Python launch files ## Installation -Assuming you have already had ROS 1 Noetic or Melodic installed in your Ubuntu computer. - -Dependencies: - -* [feetech_lib](https://github.com/Red-Rabbit-Robotics/feetech_lib) -* [ik\_solver\_lib](https://github.com/Red-Rabbit-Robotics/ik_solver_lib) -* other packages listed in the package.xml files you can install through: -`rosdep install --from-paths src --ignore-src -r -y` - -## Demo -1. Test the URDF -`roslaunch rx1_description urdf_test.launch` -You should be able control the joint angles through sliders. -![image](https://github.com/Red-Rabbit-Robotics/rx1/blob/master/media/urdf_test.gif) - -2. Test the inverse kinematics -`roslaunch rx1_description urdf.launch` -`roslaunch rx1_ik rx1_ik_marker.launch` -You should be able click and drag the end effector poses to play with the ik. -![image](https://github.com/Red-Rabbit-Robotics/rx1/blob/master/media/ik.gif) - -3. Test with the actual robots -Go to rx1_motor.launch file and modify the USB port value to the corresponding one of your Feetech servo controller board. -Then, after doing either 1 or 2 above, type: -`roslaunch rx1_motor rx1_motor.launch` -You should be able to see the actual robot move based on your commands. - -4. Gazebo simulation -Install dependencies: -`pip install tk` -Run simulation: -`roslaunch rx1_gazebo rx1_gazebo.launch` -`roscd rx1_gazebo/scripts/` -`python3 controller_gui_example.py` -You should be able click and drag the sliders and command joints angles through the GUI. -![image](https://github.com/Red-Rabbit-Robotics/rx1/blob/master/media/gazebo.gif) - -## Extra notes - -1. Note that some of the mesh files don't match exactly the latest CAD file of the RX1 Humanoid robot due to development overtime. The overall shape stays the same though. -2. The inertia values in the `rx1.urdf.xacro` file are placeholders, while the `rx1.urdf` has the proper inertia values and it's created by using the `rx1_description/scripts/calc_inertia.py` script. (**acknowledgement**: this script is modified from [https://github.com/gstavrinos/calc-inertia/blob/master/calc_inertia_for_urdf.py](https://github.com/gstavrinos/calc-inertia/blob/master/calc_inertia_for_urdf.py)) -3. To launch everything by `roslaunch rx1_bringup bringup.launch` , you need to have the depth camera's ROS package [OrbbecSDK_ROS1](https://github.com/orbbec/OrbbecSDK_ROS1) installed as well. + +Example workspace setup: + +```bash +mkdir -p ~/rx1_ws/src +cd ~/rx1_ws/src +git clone rx1 +git clone feetech_lib +cd .. +source /opt/ros/jazzy/setup.bash +rosdep install --from-paths src --ignore-src -r -y +colcon build +source install/setup.bash +``` + +If you do not need real servo hardware support, `feetech_lib` is not required for MoveIt 2 and basic RViz workflows. + +Recommended ROS 2 distro: + +* Jazzy on Ubuntu + +## Current Launch Defaults + +The maintained simulation path now uses optimized collision-model copies by default: + +* MoveIt model: `rx1_optimized.urdf.xacro` +* Gazebo model: `rx1_optimized.harmonic.urdf.xacro` + +The original model files are still kept in the repository and can be selected manually through launch arguments if needed. + +## MoveIt 2 Demo + +MoveIt 2 is the recommended way to work with RX1 in ROS 2. + +Launch the planning demo: + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +export ROS_LOG_DIR=/tmp/ros_logs +ros2 launch rx1_moveit_config demo.launch.py +``` + +What this gives you: + +* RViz with `base_link` as the fixed frame +* `move_group` +* planning groups for `right_arm`, `left_arm`, `dual_arms`, and `head` +* KDL-based kinematics configuration for the supported groups + +Notes: + +* the orange overlay in RViz is typically the MoveIt goal state visualization, not a collision by itself +* octomap sensor warnings can appear even when planning in RViz is otherwise working +* the optimized model reduces collision-check cost while keeping the original visual meshes + +## URDF / RViz Demo + +To inspect the robot model without MoveIt: + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +ros2 launch rx1_description urdf_test.launch.py +``` + +This starts: + +* `robot_state_publisher` +* `joint_state_publisher_gui` +* `rviz2` + +## Hardware Servo Control + +For real Feetech servo control: + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +ros2 launch rx1_motor rx1_motor.launch.py +``` + +Important: + +* `feetech_lib` is required for real hardware control +* it is not required for MoveIt 2 planning or basic RViz model visualization +* verify the correct serial port such as `/dev/ttyUSB0` before enabling hardware + +## Gazebo + +Gazebo support has been ported toward ROS 2 and the maintained Jazzy path now includes: + +* default optimized simulation models +* a Gazebo-specific RViz config +* delayed controller startup to avoid `controller_manager` races +* a `gazebo_demo.launch.py` flow that waits for the simulated trajectory action server before starting the MoveIt side + +Launch Gazebo only: + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +unset ROS_LOCALHOST_ONLY +ros2 launch rx1_gazebo rx1_gazebo.launch.py +``` + +Launch Gazebo with the simulation RViz view: + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +unset ROS_LOCALHOST_ONLY +ros2 launch rx1_gazebo rx1_gazebo_rviz.launch.py +``` + +Launch Gazebo with MoveIt planning in RViz: + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +unset ROS_LOCALHOST_ONLY +ros2 launch rx1_moveit_config gazebo_demo.launch.py +``` + +What this gives you: + +* Gazebo simulation +* controller startup through `gz_ros2_control` +* RViz with MoveIt +* execution through `rx1_joint_trajectory_controller` + +Useful launch arguments: + +* `robot_x:=... robot_y:=... robot_z:=...` to place the robot in the world +* `moveit_model:=...` and `sim_model:=...` if you want to override the optimized defaults in `gazebo_demo.launch.py` +* `model:=...` if you want to override the optimized default in `rx1_gazebo_rviz.launch.py` + +Simulation notes: + +* `gazebo_demo.launch.py` now starts the MoveIt side only after the simulated `follow_joint_trajectory` action server is available +* MoveIt uses a stamped `/joint_states_moveit` stream internally to avoid zero-timestamp execution failures from the raw sim state stream +* if RViz camera rendering is unstable on your GPU driver, keep camera displays disabled in MoveIt RViz and use `ros2 run rqt_image_view rqt_image_view` to inspect `/rx1/rgbd_camera/image` or `/rx1/rgbd_camera/depth_image` +* the repeated `base_link` inertia warning from KDL is non-fatal and does not block planning or execution + +## Legacy IK Path + +The original custom IK path is still present in `rx1_ik`, but MoveIt 2 is the preferred ROS 2 workflow. + +Use the legacy IK path only if you specifically want to preserve the original IK behavior and also have a ROS 2-compatible `ik_solver_lib`. + +## Extra Notes + +1. Some mesh files may not exactly match the latest RX1 CAD state, but the overall robot shape is preserved. +2. `rx1.urdf.xacro` contains placeholder inertia structure in places, while generated URDF content is used for runtime visualization and planning. +3. For ROS 2 use, the practical order is: + * validate `rx1_description` + * use `rx1_moveit_config` + * bring up `rx1_motor` only when moving to real hardware diff --git a/TODO.md b/TODO.md new file mode 100644 index 0000000..aaea696 --- /dev/null +++ b/TODO.md @@ -0,0 +1,13 @@ +# RX1 ROS 2 TODO + +- Confirm the RViz orange goal-state overlay behaves correctly after selecting each MoveIt planning group. +- Re-test all MoveIt groups in fresh sessions: `right_arm`, `left_arm`, `head`, `torso`, `right_arm_with_torso`, `left_arm_with_torso`, and `dual_arms`. +- Polish the fake hardware path so repeated `Plan and Execute` runs stay reliable across all supported groups. +- Decide the next execution target: keep RViz fake hardware only, add Gazebo execution, or wire real hardware. +- If simulation execution is needed, connect MoveIt to a proper ROS 2 controller stack for RX1. +- Simplify collision geometry in the URDF so MoveIt stops warning about high-vertex collision meshes. +- Add a dummy root link above `base_link` to remove the KDL root-inertia warning. +- Either configure an octomap sensor source or disable the unused occupancy-map path in the MoveIt demo. +- Do a systematic self-collision review and tighten the SRDF collision matrix where needed. +- Update the README with the current ROS 2 launch commands, fake-hardware workflow, and known limitations. +- After the sim path is stable, decide whether to connect MoveIt trajectory execution to `rx1_motor` and `feetech_lib` for the real robot. diff --git a/rx1/CMakeLists.txt b/rx1/CMakeLists.txt deleted file mode 100644 index 081cd7c..0000000 --- a/rx1/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(rx1) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/rx1_bringup/CMakeLists.txt b/rx1_bringup/CMakeLists.txt deleted file mode 100644 index cd5ae4a..0000000 --- a/rx1_bringup/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(rx1_bringup) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - sensor_msgs - diagnostic_msgs -) - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - CATKIN_DEPENDS roscpp std_msgs sensor_msgs diagnostic_msgs -) - -################################################################################ -# Build -################################################################################ -include_directories( - ${catkin_INCLUDE_DIRS} -) diff --git a/rx1_bringup/package.xml b/rx1_bringup/package.xml deleted file mode 100644 index de2eeb4..0000000 --- a/rx1_bringup/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - rx1_bringup - 1.2.5 - - roslaunch scripts for starting the rx1 robot - - MIT - lethic - lethic - catkin - roscpp - std_msgs - diagnostic_msgs - sensor_msgs - rx1_description - joint_state_publisher - robot_state_publisher - diff --git a/rx1_bringup/scripts/play_bags.py b/rx1_bringup/scripts/play_bags.py deleted file mode 100644 index 4828359..0000000 --- a/rx1_bringup/scripts/play_bags.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python - -import subprocess -import time - -# List of ROS bag files to play in sequence -rosbags = [ - "/home/lingkang/rosbag/dual_arm_hand_bag.bag", - "/home/lingkang/rosbag/command_head.bag", - "/home/lingkang/rosbag/right_arm_2.bag", - "/home/lingkang/rosbag/command_head.bag", - "/home/lingkang/rosbag/dual_arm_hand_bag.bag", - "/home/lingkang/rosbag/command_head.bag", - "/home/lingkang/rosbag/dual_arm_hand_bag.bag", - "/home/lingkang/rosbag/command_head.bag", - "/home/lingkang/rosbag/dual_arm_hand_bag.bag", - "/home/lingkang/rosbag/command_head.bag", -] - -# Function to play a single ROS bag file -def play_rosbag(bag_file): - print(f"Playing {bag_file}") - process = subprocess.Popen(["rosbag", "play", "--clock", bag_file]) - process.wait() # Wait for the current bag file to finish playing - -if __name__ == '__main__': - # Set use_sim_time to true - subprocess.call(["rosparam", "set", "/use_sim_time", "true"]) - - # Ensure roscore is running - try: - #roscore_process = subprocess.Popen(["roscore"]) - #time.sleep(5) # Give roscore some time to start - - # Play each ROS bag file in sequence - for bag in rosbags: - play_rosbag(bag) - for bag in rosbags: - play_rosbag(bag) - for bag in rosbags: - play_rosbag(bag) - for bag in rosbags: - play_rosbag(bag) - for bag in rosbags: - play_rosbag(bag) - - finally: - # Terminate roscore - roscore_process.terminate() - diff --git a/rx1_description/CMakeLists.txt b/rx1_description/CMakeLists.txt deleted file mode 100644 index afe3604..0000000 --- a/rx1_description/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(rx1_description) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - urdf - xacro -) - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - CATKIN_DEPENDS urdf xacro -) - -################################################################################ -# Build -################################################################################ -include_directories( - ${catkin_INCLUDE_DIRS} -) - -################################################################################ -# Install -################################################################################ -install(DIRECTORY meshes rviz urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/rx1_description/package.xml b/rx1_description/package.xml deleted file mode 100644 index 55b8438..0000000 --- a/rx1_description/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - rx1_description - 1.2.5 - - 3D models of the rx1 for simulation and visualization - - MIT - lethic - lethic - catkin - urdf - xacro - - diff --git a/rx1_gazebo/CMakeLists.txt b/rx1_gazebo/CMakeLists.txt deleted file mode 100644 index d59c03b..0000000 --- a/rx1_gazebo/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(rx1_gazebo) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - urdf - xacro -) - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - CATKIN_DEPENDS urdf xacro -) - -################################################################################ -# Build -################################################################################ -include_directories( - ${catkin_INCLUDE_DIRS} -) - -################################################################################ -# Install -################################################################################ -install(DIRECTORY meshes rviz urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/rx1_gazebo/package.xml b/rx1_gazebo/package.xml deleted file mode 100644 index 0836c62..0000000 --- a/rx1_gazebo/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - rx1_gazebo - 1.2.5 - - rx1 humanoid gazebo simulation - - MIT - lethic - lethic - catkin - urdf - xacro - - diff --git a/rx1_gazebo/scripts/controller_example.py b/rx1_gazebo/scripts/controller_example.py deleted file mode 100755 index 3d1c889..0000000 --- a/rx1_gazebo/scripts/controller_example.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python - -import rospy -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint - -class ArmTorsoHeadController: - def __init__(self): - # Initialize the ROS node - rospy.init_node('move_arms_torso_head_to_zero', anonymous=True) - - # Publishers for the right arm, left arm, torso, and head controllers - self.right_arm_pub = rospy.Publisher('/right_arm_position_controller/command', JointTrajectory, queue_size=10) - self.left_arm_pub = rospy.Publisher('/left_arm_position_controller/command', JointTrajectory, queue_size=10) - self.torso_pub = rospy.Publisher('/torso_position_controller/command', JointTrajectory, queue_size=10) - self.head_pub = rospy.Publisher('/head_position_controller/command', JointTrajectory, queue_size=10) - - # Define joint names for the right arm - self.right_arm_joints = [ - 'right_shoul_base2shoul_joint', - 'right_shoul2shoul_rot_joint', - 'right_arm2armrot_joint', - 'right_armrot2elbow_joint', - 'right_forearm2forearmrot_joint', - 'right_forearmrot2forearm_pitch_joint', - 'right_forearm_pitch2forearm_roll_joint' - ] - - # Define joint names for the left arm - self.left_arm_joints = [ - 'left_shoul_base2shoul_joint', - 'left_shoul2shoul_rot_joint', - 'left_arm2armrot_joint', - 'left_armrot2elbow_joint', - 'left_forearm2forearmrot_joint', - 'left_forearmrot2forearm_pitch_joint', - 'left_forearm_pitch2forearm_roll_joint' - ] - - # Define joint names for the torso - self.torso_joints = [ - 'base2torso_yaw_joint', - 'torso_yaw2pitch_joint', - 'torso_pitch2roll_joint' - ] - - # Define joint names for the head - self.head_joints = [ - 'head_base2neck_yaw_joint', - 'neck_yaw2pitch_joint', - 'neck_pitch2head_depth_cam_mount_joint' - ] - - # Set the rate for the loop - self.rate = rospy.Rate(0.2) # 0.1 Hz - - def create_trajectory(self, joint_names, pos): - """Creates a JointTrajectory message with all joints set to 0 position.""" - traj = JointTrajectory() - traj.joint_names = joint_names - - # Create a trajectory point that sets all joints to position 0 - point = JointTrajectoryPoint() - point.positions = [pos] * len(joint_names) # Set all positions to 0 - point.time_from_start = rospy.Duration(2.0) # Move to zero position in 2 seconds - - traj.points.append(point) - return traj - - def move_right_arm_to_zero(self): - """Publishes a trajectory to move the right arm joints to position 0.""" - right_arm_traj = self.create_trajectory(self.right_arm_joints, 0) - rospy.loginfo("Sending trajectory to move right arm joints to position 0.") - self.right_arm_pub.publish(right_arm_traj) - - def move_left_arm_to_zero(self): - """Publishes a trajectory to move the left arm joints to position 0.""" - left_arm_traj = self.create_trajectory(self.left_arm_joints, 0) - rospy.loginfo("Sending trajectory to move left arm joints to position 0.") - self.left_arm_pub.publish(left_arm_traj) - - def move_torso_to_zero(self): - """Publishes a trajectory to move the torso joints to position 0.""" - torso_traj = self.create_trajectory(self.torso_joints, 0) - rospy.loginfo("Sending trajectory to move torso joints to position 0.") - self.torso_pub.publish(torso_traj) - - def move_head_to_zero(self): - """Publishes a trajectory to move the head joints to position 0.""" - head_traj = self.create_trajectory(self.head_joints, 0) - rospy.loginfo("Sending trajectory to move head joints to position 0.") - self.head_pub.publish(head_traj) - - def move_all_to_zero(self): - """Moves both arms, the torso, and the head to zero position.""" - while not rospy.is_shutdown(): - self.move_right_arm_to_zero() - self.move_left_arm_to_zero() - self.move_torso_to_zero() - self.move_head_to_zero() - self.rate.sleep() # Sleep to maintain the loop at 10 Hz - -if __name__ == '__main__': - try: - # Initialize the ArmTorsoHeadController class - controller = ArmTorsoHeadController() - - # Move all joints (arms, torso, and head) to zero position - controller.move_all_to_zero() - except rospy.ROSInterruptException: - pass - diff --git a/rx1_gazebo/scripts/controller_gui_example.py b/rx1_gazebo/scripts/controller_gui_example.py deleted file mode 100755 index a5be288..0000000 --- a/rx1_gazebo/scripts/controller_gui_example.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python - -import rospy -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -import tkinter as tk - -class ArmTorsoHeadController: - def __init__(self): - # Initialize the ROS node - rospy.init_node('move_arms_torso_head_with_gui', anonymous=True) - - # Publishers for the right arm, left arm, torso, and head controllers - self.right_arm_pub = rospy.Publisher('/right_arm_position_controller/command', JointTrajectory, queue_size=10) - self.left_arm_pub = rospy.Publisher('/left_arm_position_controller/command', JointTrajectory, queue_size=10) - self.torso_pub = rospy.Publisher('/torso_position_controller/command', JointTrajectory, queue_size=10) - self.head_pub = rospy.Publisher('/head_position_controller/command', JointTrajectory, queue_size=10) - - # Joint names for the right arm, left arm, torso, and head - self.right_arm_joints = [ - 'right_shoul_base2shoul_joint', - 'right_shoul2shoul_rot_joint', - 'right_arm2armrot_joint', - 'right_armrot2elbow_joint', - 'right_forearm2forearmrot_joint', - 'right_forearmrot2forearm_pitch_joint', - 'right_forearm_pitch2forearm_roll_joint' - ] - - self.left_arm_joints = [ - 'left_shoul_base2shoul_joint', - 'left_shoul2shoul_rot_joint', - 'left_arm2armrot_joint', - 'left_armrot2elbow_joint', - 'left_forearm2forearmrot_joint', - 'left_forearmrot2forearm_pitch_joint', - 'left_forearm_pitch2forearm_roll_joint' - ] - - self.torso_joints = [ - 'base2torso_yaw_joint', - 'torso_yaw2pitch_joint', - 'torso_pitch2roll_joint' - ] - - self.head_joints = [ - 'head_base2neck_yaw_joint', - 'neck_yaw2pitch_joint', - 'neck_pitch2head_depth_cam_mount_joint' - ] - - # Variables to hold joint positions from sliders - self.joint_positions = { - joint: 0.0 for joint in self.right_arm_joints + self.left_arm_joints + self.torso_joints + self.head_joints - } - - # Initialize the Tkinter GUI - self.init_gui() - - def init_gui(self): - """Initialize the Tkinter GUI with sliders for each joint.""" - self.root = tk.Tk() - self.root.title("Joint Angle Controller") - - row = 0 - # Create sliders for all joints - for joint_name in self.joint_positions: - label = tk.Label(self.root, text=joint_name) - label.grid(row=row, column=0) - slider = tk.Scale(self.root, from_=-3.14, to=3.14, resolution=0.01, orient=tk.HORIZONTAL, length=300, - command=lambda val, j=joint_name: self.update_joint_position(j, val)) - slider.grid(row=row, column=1) - row += 1 - - # Create a button to send the commands - send_button = tk.Button(self.root, text="Send Joint Commands", command=self.send_joint_commands) - send_button.grid(row=row, column=0, columnspan=2) - - # Start the Tkinter main loop - self.root.mainloop() - - def update_joint_position(self, joint_name, value): - """Update the joint position when the slider is moved.""" - self.joint_positions[joint_name] = float(value) - - def create_trajectory(self, joint_names): - """Creates a JointTrajectory message based on the slider values.""" - traj = JointTrajectory() - traj.joint_names = joint_names - - point = JointTrajectoryPoint() - point.positions = [self.joint_positions[joint] for joint in joint_names] - point.time_from_start = rospy.Duration(1.0) # Set the movement duration - - traj.points.append(point) - return traj - - def send_joint_commands(self): - """Send the joint commands based on the slider positions.""" - # Publish the trajectory for the right arm - right_arm_traj = self.create_trajectory(self.right_arm_joints) - self.right_arm_pub.publish(right_arm_traj) - - # Publish the trajectory for the left arm - left_arm_traj = self.create_trajectory(self.left_arm_joints) - self.left_arm_pub.publish(left_arm_traj) - - # Publish the trajectory for the torso - torso_traj = self.create_trajectory(self.torso_joints) - self.torso_pub.publish(torso_traj) - - # Publish the trajectory for the head - head_traj = self.create_trajectory(self.head_joints) - self.head_pub.publish(head_traj) - - rospy.loginfo("Joint commands sent based on GUI sliders.") - -if __name__ == '__main__': - try: - # Initialize the controller - controller = ArmTorsoHeadController() - except rospy.ROSInterruptException: - pass - diff --git a/rx1_ik/CMakeLists.txt b/rx1_ik/CMakeLists.txt deleted file mode 100644 index cc2f366..0000000 --- a/rx1_ik/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(rx1_ik) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - interactive_markers - std_msgs - tf2 - tf2_geometry_msgs - tf2_msgs - tf2_ros - tf2_kdl - sensor_msgs - pluginlib - ik_solver_lib - kdl_parser - geometry_msgs -) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - #INCLUDE_DIRS include - #LIBRARIES ${PROJECT_NAME} - #CATKIN_DEPENDS roscpp geometry_msgs interactive_markers tf2_msgs tf2_ros sensor_msgs pluginlib ik_solver_lib kdl_parser -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} - src/${PROJECT_NAME}/rx1_ik.cpp -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -add_executable(rx1_ik_node src/rx1_ik_node.cpp) -add_dependencies(rx1_ik_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(rx1_ik_node - ${catkin_LIBRARIES} - ${PROJECT_NAME} -) diff --git a/rx1_ik/include/rx1_ik/rx1_ik.h b/rx1_ik/include/rx1_ik/rx1_ik.h deleted file mode 100644 index 8489f5d..0000000 --- a/rx1_ik/include/rx1_ik/rx1_ik.h +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef RX1_IK_H -#define RX1_IK_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ik_solver_lib/base/ik_solver_base.h" - -class Rx1Ik -{ -public: - Rx1Ik(ros::NodeHandle& nh, ros::NodeHandle& priv_nh); - void initializeInteractiveMarker(); - void markerRightCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); - void markerLeftCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); - void rightGripperPoseCallback(const geometry_msgs::Pose& msg); - void leftGripperPoseCallback(const geometry_msgs::Pose& msg); - void spinOnce(); - void spin(); - void update(); - -private: - void make6DofMarker(visualization_msgs::InteractiveMarker &int_marker); - ros::NodeHandle nh_; - ros::NodeHandle priv_nh_; - - interactive_markers::InteractiveMarkerServer marker_server_; - visualization_msgs::InteractiveMarker int_marker_r_; - visualization_msgs::InteractiveMarker int_marker_l_; - - ros::Subscriber right_gripper_pose_sub_; - ros::Subscriber left_gripper_pose_sub_; - - ros::Publisher right_joint_state_pub_; - ros::Publisher left_joint_state_pub_; - - std::unique_ptr> ik_loader_r_ptr_; - boost::shared_ptr ik_solver_r_ptr_; - - std::unique_ptr> ik_loader_l_ptr_; - boost::shared_ptr ik_solver_l_ptr_; - - sensor_msgs::JointState right_prev_joint_state_msg_; - sensor_msgs::JointState left_prev_joint_state_msg_; - - sensor_msgs::JointState right_cur_joint_state_msg_; - sensor_msgs::JointState left_cur_joint_state_msg_; - - geometry_msgs::TransformStamped world_to_base_tf_stamped_; - - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - tf2_ros::TransformBroadcaster tf_br_; - - // Params - std::string chain_start_, chain_r_end_, chain_l_end_, urdf_param_; - double max_angle_change_; - double tracking_timeout_; - - // Ik variables - double right_last_ik_time_; - double left_last_ik_time_; - - // util functions - - // Get link's pose in frame - bool getLinkPose(const std::string frame, const std::string link, geometry_msgs::PoseStamped& pose); - // Turn the pose in frame a to frame b - bool getPoseInNewFrame(const geometry_msgs::PoseStamped old_pose, const std::string new_frame, geometry_msgs::PoseStamped& new_pose); - // Turn pose to transform - geometry_msgs::TransformStamped poseToTransformStamped(const geometry_msgs::Pose& pose, const std::string& frame_id, const std::string& child_frame_id); -}; - -#endif // RX1_IK_H diff --git a/rx1_ik/package.xml b/rx1_ik/package.xml deleted file mode 100644 index 43e491e..0000000 --- a/rx1_ik/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - rx1_ik - 1.2.5 - - Inverse Kinematics of the RX1 robot - - MIT - lethic - lethic - catkin - roscpp - std_msgs - interactive_markers - tf2 - tf2_geometry_msgs - tf2_msgs - tf2_ros - sensor_msgs - pluginlib - ik_solver_lib - kdl_parser - geometry_msgs - - roscpp - std_msgs - interactive_markers - tf2_geometry_msgs - tf2_msgs - tf2_ros - tf2 - sensor_msgs - pluginlib - ik_solver_lib - kdl_parser - - diff --git a/rx1_ik/src/rx1_ik/rx1_ik.cpp b/rx1_ik/src/rx1_ik/rx1_ik.cpp deleted file mode 100644 index bc7c13f..0000000 --- a/rx1_ik/src/rx1_ik/rx1_ik.cpp +++ /dev/null @@ -1,497 +0,0 @@ -#include - -#include - -#include "rx1_ik/rx1_ik.h" -#include -#include -#include -#include -#include -#include -#include - -Rx1Ik::Rx1Ik(ros::NodeHandle& nh, ros::NodeHandle& priv_nh) - : nh_(nh), - priv_nh_(priv_nh), - marker_server_("end_effector_marker"), - tf_listener_(tf_buffer_) -{ - // Load the IK solver plugin - try - { - // Retrieve parameters from the parameter server - double ik_timeout; - double eps; - - priv_nh_.param("chain_start", chain_start_, std::string("head_base_link")); - priv_nh_.param("chain_r_end", chain_r_end_, std::string("right_hand_link")); - priv_nh_.param("chain_l_end", chain_l_end_, std::string("left_hand_link")); - priv_nh_.param("urdf_param", urdf_param_, std::string("/robot_description")); - priv_nh_.param("ik_timeout", ik_timeout, 0.005); // Default timeout - priv_nh_.param("eps", eps, 1e-3); // Default error - priv_nh_.param("max_angle_change", max_angle_change_, 0.3); - priv_nh_.param("tracking_timeout", tracking_timeout_, 1.0); - - ROS_INFO("eps: %f", eps); - - // Create IK solver instance - ik_loader_r_ptr_ = std::make_unique>("ik_solver_lib", "ik_solver_plugin::IKSolverBase"); - ik_solver_r_ptr_ = ik_loader_r_ptr_->createInstance("ik_solver_plugin::TracIKSolver"); - ik_solver_r_ptr_->initialize(chain_start_, chain_r_end_, urdf_param_, ik_timeout, eps); - - right_last_ik_time_ = ros::Time::now().toSec() - tracking_timeout_; - - ik_loader_l_ptr_ = std::make_unique>("ik_solver_lib", "ik_solver_plugin::IKSolverBase"); - ik_solver_l_ptr_ = ik_loader_l_ptr_->createInstance("ik_solver_plugin::TracIKSolver"); - ik_solver_l_ptr_->initialize(chain_start_, chain_l_end_, urdf_param_, ik_timeout, eps); - - left_last_ik_time_ = ros::Time::now().toSec() - tracking_timeout_; - - ROS_INFO("TracIKSolver plugin loaded and initialized successfully."); - } - catch (const pluginlib::PluginlibException& ex) - { - ROS_ERROR("Failed to create the TracIKSolver plugin. Error: %s", ex.what()); - return; - } - - // Publisher for joint states - left_joint_state_pub_ = nh_.advertise("left_arm_joint_states", 10); - right_joint_state_pub_ = nh_.advertise("right_arm_joint_states", 10); - - // Initialize subscribers - right_gripper_pose_sub_ = nh_.subscribe("right_gripper_pose", 10, &Rx1Ik::rightGripperPoseCallback, this); - left_gripper_pose_sub_ = nh_.subscribe("left_gripper_pose", 10, &Rx1Ik::leftGripperPoseCallback, this); - - // Initialize joint states - sensor_msgs::JointState right_joint_state_msg; - right_joint_state_msg.header.stamp = ros::Time::now(); - right_joint_state_msg.name.resize(7); - right_joint_state_msg.position.resize(7); - std::vector right_joint_name = {"right_shoul_base2shoul_joint", - "right_shoul2shoul_rot_joint", - "right_arm2armrot_joint", - "right_armrot2elbow_joint", - "right_forearm2forearmrot_joint", - "right_forearmrot2forearm_pitch_joint", - "right_forearm_pitch2forearm_roll_joint"}; - for (int i = 0; i < 7; ++i) - { - right_joint_state_msg.position[i] = 0; - right_joint_state_msg.name[i] = right_joint_name[i]; - } - right_joint_state_msg.position[3] = -1.57; - right_joint_state_pub_.publish(right_joint_state_msg); - right_prev_joint_state_msg_ = right_joint_state_msg; - right_cur_joint_state_msg_ = right_joint_state_msg; - - - sensor_msgs::JointState left_joint_state_msg; - left_joint_state_msg.header.stamp = ros::Time::now(); - left_joint_state_msg.name.resize(7); - left_joint_state_msg.position.resize(7); - std::vector left_joint_name = {"left_shoul_base2shoul_joint", - "left_shoul2shoul_rot_joint", - "left_arm2armrot_joint", - "left_armrot2elbow_joint", - "left_forearm2forearmrot_joint", - "left_forearmrot2forearm_pitch_joint", - "left_forearm_pitch2forearm_roll_joint"}; - for (int i = 0; i < 7; ++i) - { - left_joint_state_msg.position[i] = 0; - left_joint_state_msg.name[i] = left_joint_name[i]; - } - left_joint_state_msg.position[3] = -1.57; - left_joint_state_pub_.publish(left_joint_state_msg); - left_prev_joint_state_msg_ = left_joint_state_msg; - left_cur_joint_state_msg_ = left_joint_state_msg; - - ROS_INFO("Joints initialized"); - - // Initialize floating joint - world_to_base_tf_stamped_.header.stamp = ros::Time::now(); - world_to_base_tf_stamped_.header.frame_id = "map"; - world_to_base_tf_stamped_.child_frame_id = "base_link"; - world_to_base_tf_stamped_.transform.translation.x = 0.0; - world_to_base_tf_stamped_.transform.translation.y = 0.0; - world_to_base_tf_stamped_.transform.translation.z = 0.0; - world_to_base_tf_stamped_.transform.rotation.x = 0.0; - world_to_base_tf_stamped_.transform.rotation.y = 0.0; - world_to_base_tf_stamped_.transform.rotation.z = 0.0; - world_to_base_tf_stamped_.transform.rotation.w = 1.0; - tf_br_.sendTransform(world_to_base_tf_stamped_); - - // Initialize the interactive marker - initializeInteractiveMarker(); -} - -void Rx1Ik::make6DofMarker(visualization_msgs::InteractiveMarker &int_marker) -{ - int_marker.scale = 0.15; - // Create a control for each degree of freedom - - // X-axis control (red) - visualization_msgs::InteractiveMarkerControl control; - control.orientation.w = 1; - control.orientation.x = 1; - control.orientation.y = 0; - control.orientation.z = 0; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - // Y-axis control (green) - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 1; - control.orientation.z = 0; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - // Z-axis control (blue) - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 0; - control.orientation.z = 1; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; - int_marker.controls.push_back(control); - - // Rotation around X-axis - control.orientation.w = 1; - control.orientation.x = 1; - control.orientation.y = 0; - control.orientation.z = 0; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - - // Rotation around Y-axis - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 1; - control.orientation.z = 0; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); - - // Rotation around Z-axis - control.orientation.w = 1; - control.orientation.x = 0; - control.orientation.y = 0; - control.orientation.z = 1; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; - int_marker.controls.push_back(control); -} - -void Rx1Ik::initializeInteractiveMarker() -{ - // Create an interactive marker for the end effector - //visualization_msgs::InteractiveMarker int_marker; - int_marker_r_.header.frame_id = chain_start_; //"torso_link"; - int_marker_r_.name = "right_end_effector"; - int_marker_r_.description = "Right End Effector Control"; - - - int_marker_l_.header.frame_id = chain_start_; - int_marker_l_.name = "left_end_effector"; - int_marker_l_.description = "Left End Effector Control"; - - // Create a 6-DOF control which allows moving and rotating along all axes - make6DofMarker(int_marker_r_); - make6DofMarker(int_marker_l_); - //make6DofMarker(int_marker_b_); - - // Add the interactive marker to our collection & - // tell the server to call processFeedback() when feedback arrives for it - marker_server_.insert(int_marker_r_, boost::bind(&Rx1Ik::markerRightCallback, this, _1)); - marker_server_.insert(int_marker_l_, boost::bind(&Rx1Ik::markerLeftCallback, this, _1)); - - // 'commit' changes and send to all clients - marker_server_.applyChanges(); -} - -void Rx1Ik::markerRightCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) -{ - if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) - { - // Convert the marker pose to a KDL::Frame - KDL::Frame desired_pose; - tf2::fromMsg(feedback->pose, desired_pose); - - // Solve IK - KDL::JntArray result_joint_positions; - if (ik_solver_r_ptr_->solveIK(desired_pose, result_joint_positions)) - { - bool success = true; - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - if (abs(right_prev_joint_state_msg_.position[i]-result_joint_positions(i)) > max_angle_change_) - success = false; - } - - if (success) - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - // simple smoothing - right_prev_joint_state_msg_.position[i] = right_prev_joint_state_msg_.position[i]*0.9 + result_joint_positions(i)*0.1; - } - ROS_INFO("Succeed finding right arm IK solution"); - } - else - { - ROS_INFO("Succeed finding right arm IK solution but ditch the result to reduce shake"); - } - } - else - { - ROS_WARN("Failed to find right arm IK solution"); - } - } -} - -void Rx1Ik::markerLeftCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) -{ - if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) - { - // Convert the marker pose to a KDL::Frame - KDL::Frame desired_pose; - tf2::fromMsg(feedback->pose, desired_pose); - - // Solve IK - KDL::JntArray result_joint_positions; - if (ik_solver_l_ptr_->solveIK(desired_pose, result_joint_positions)) - { - bool success = true; - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - if (abs(left_prev_joint_state_msg_.position[i]-result_joint_positions(i)) > max_angle_change_) - success = false; - } - - if (success) - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - // simple smoothing - left_prev_joint_state_msg_.position[i] = left_prev_joint_state_msg_.position[i]*0.9 + result_joint_positions(i)*0.1; - } - ROS_INFO("Succeed finding left arm IK solution"); - } - else - { - ROS_INFO("Succeed finding left arm IK solution but ditch the result to reduce shake"); - } - } - else - { - ROS_WARN("Failed to find left arm IK solution"); - } - } - -} - -void Rx1Ik::rightGripperPoseCallback(const geometry_msgs::Pose& msg) -{ - // Convert the msg pose to a KDL::Frame - KDL::Frame desired_pose; - tf2::fromMsg(msg, desired_pose); - - // Solve IK - KDL::JntArray result_joint_positions; - //if (ik_solver_r_ptr_->solveIK(desired_pose, result_joint_positions)) - - double before_ik_time = ros::Time::now().toSec(); - auto ik_solved = ik_solver_r_ptr_->solveIK(desired_pose, result_joint_positions); - ROS_INFO("right ik took %f sec", ros::Time::now().toSec() - before_ik_time); - - if (ik_solved) - { - bool success = true; - if ((ros::Time::now().toSec() - right_last_ik_time_) < tracking_timeout_) // if it's within tracking_timeout, then the angle change should be smaller than max_angle_change - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - if (abs(right_prev_joint_state_msg_.position[i]-result_joint_positions(i)) > max_angle_change_) - success = false; - } - } - - if (success) - { - if ((ros::Time::now().toSec() - right_last_ik_time_) < tracking_timeout_) - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - right_prev_joint_state_msg_.position[i] = result_joint_positions(i); - } - } - else - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - right_prev_joint_state_msg_.position[i] = result_joint_positions(i); - } - } - - right_last_ik_time_ = ros::Time::now().toSec(); - ROS_INFO("Succeed finding right IK solution"); - } - else - { - ROS_INFO("Succeed finding right IK solution but ditch the result to reduce shake"); - } - } - else - { - ROS_WARN("Failed to find right IK solution"); - } -} - -void Rx1Ik::leftGripperPoseCallback(const geometry_msgs::Pose& msg) -{ - // Convert the msg pose to a KDL::Frame - KDL::Frame desired_pose; - tf2::fromMsg(msg, desired_pose); - - // Solve IK - KDL::JntArray result_joint_positions; - if (ik_solver_l_ptr_->solveIK(desired_pose, result_joint_positions)) - { - bool success = true; - if ((ros::Time::now().toSec() - left_last_ik_time_) < tracking_timeout_) // if it's within tracking_timeout, then the angle change should be smaller than max_angle_change - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - if (abs(left_prev_joint_state_msg_.position[i]-result_joint_positions(i)) > max_angle_change_) - success = false; - } - } - - if (success) - { - if ((ros::Time::now().toSec() - left_last_ik_time_) < tracking_timeout_) - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - left_prev_joint_state_msg_.position[i] = result_joint_positions(i); - } - } - else - { - for (int i = 0; i < result_joint_positions.rows(); ++i) - { - left_prev_joint_state_msg_.position[i] = result_joint_positions(i); - } - } - - left_last_ik_time_ = ros::Time::now().toSec(); - ROS_INFO("Succeed finding left IK solution"); - } - else - { - ROS_INFO("Succeed finding left IK solution but ditch the result to reduce shake"); - } - } - else - { - ROS_WARN("Failed to find left IK solution"); - } -} -void Rx1Ik::spinOnce() -{ - ros::spinOnce(); - update(); -} - -void Rx1Ik::update() -{ - for (int i = 0; i < right_cur_joint_state_msg_.position.size(); i ++) - { - // simple smoothing - right_cur_joint_state_msg_.position[i] = right_cur_joint_state_msg_.position[i]*0.9 + right_prev_joint_state_msg_.position[i]*0.1; - } - right_cur_joint_state_msg_.header.stamp = ros::Time::now(); - right_joint_state_pub_.publish(right_cur_joint_state_msg_); - - for (int i = 0; i < left_cur_joint_state_msg_.position.size(); i ++) - { - // simple smoothing - left_cur_joint_state_msg_.position[i] = left_cur_joint_state_msg_.position[i]*0.9 + left_prev_joint_state_msg_.position[i]*0.1; - } - left_cur_joint_state_msg_.header.stamp = ros::Time::now(); - left_joint_state_pub_.publish(left_cur_joint_state_msg_); - - world_to_base_tf_stamped_.header.stamp = ros::Time::now(); - tf_br_.sendTransform(world_to_base_tf_stamped_); - geometry_msgs::PoseStamped pose; - if(getLinkPose(chain_start_, chain_r_end_, pose)) - { - int_marker_r_.pose = pose.pose; - marker_server_.insert(int_marker_r_); - marker_server_.applyChanges(); - //ROS_INFO("Marker right pose updated"); - } - if(getLinkPose(chain_start_, chain_l_end_, pose)) - { - int_marker_l_.pose = pose.pose; - marker_server_.insert(int_marker_l_); - marker_server_.applyChanges(); - //ROS_INFO("Marker left pose updated"); - } -} - -void Rx1Ik::spin() -{ - ros::Rate rate(20); - while(ros::ok()) - { - spinOnce(); - rate.sleep(); - } -} - -bool Rx1Ik::getLinkPose(const std::string frame, const std::string link, geometry_msgs::PoseStamped& pose) -{ - geometry_msgs::TransformStamped transformStamped; - try{ - transformStamped = tf_buffer_.lookupTransform(frame, link, ros::Time(0), ros::Duration(3.0)); - pose.header.stamp = ros::Time::now(); - pose.header.frame_id = frame; - pose.pose.position.x = transformStamped.transform.translation.x; - pose.pose.position.y = transformStamped.transform.translation.y; - pose.pose.position.z = transformStamped.transform.translation.z; - pose.pose.orientation = transformStamped.transform.rotation; - } - catch (tf2::TransformException &ex) { - ROS_WARN("%s",ex.what()); - return false; - } - return true; -} - -bool Rx1Ik::getPoseInNewFrame(const geometry_msgs::PoseStamped old_pose, const std::string new_frame, geometry_msgs::PoseStamped& new_pose) -{ - try { - new_pose = tf_buffer_.transform(old_pose, new_frame, ros::Duration(1.0)); - } - catch (tf2::TransformException &ex) { - ROS_WARN("getPoseInNewFrame: %s", ex.what()); - return false; - } - return true; -} - -geometry_msgs::TransformStamped Rx1Ik::poseToTransformStamped(const geometry_msgs::Pose& pose, const std::string& frame_id, const std::string& child_frame_id) { - geometry_msgs::TransformStamped transformStamped; - - transformStamped.header.stamp = ros::Time::now(); - transformStamped.header.frame_id = frame_id; - transformStamped.child_frame_id = child_frame_id; - - transformStamped.transform.translation.x = pose.position.x; - transformStamped.transform.translation.y = pose.position.y; - transformStamped.transform.translation.z = pose.position.z; - transformStamped.transform.rotation = pose.orientation; - - return transformStamped; -} diff --git a/rx1_ik/src/rx1_ik_node.cpp b/rx1_ik/src/rx1_ik_node.cpp deleted file mode 100644 index 02b9e07..0000000 --- a/rx1_ik/src/rx1_ik_node.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "rx1_ik/rx1_ik.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "rx1_ik"); - ros::NodeHandle nh; - ros::NodeHandle priv_nh("~"); - Rx1Ik rx1_ik_node(nh, priv_nh); - - try - { - rx1_ik_node.spin(); - } - catch (std::runtime_error& ex) - { - ROS_FATAL_STREAM("[RX1_IK] Runtime error: " << ex.what()); - return 1; - } - - return 0; -} - diff --git a/rx1_motor/CMakeLists.txt b/rx1_motor/CMakeLists.txt deleted file mode 100644 index 2040b14..0000000 --- a/rx1_motor/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(rx1_motor) - -add_compile_options(-std=c++17) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - feetech_lib - roscpp - sensor_msgs -) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS feetech_lib roscpp sensor_msgs -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} - src/${PROJECT_NAME}/rx1_motor.cpp -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -add_executable(rx1_motor_node src/rx1_motor_node.cpp) -add_dependencies(rx1_motor_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(rx1_motor_node - ${catkin_LIBRARIES} - ${PROJECT_NAME} -) diff --git a/rx1_motor/include/rx1_motor/rx1_motor.hpp b/rx1_motor/include/rx1_motor/rx1_motor.hpp deleted file mode 100644 index 350083d..0000000 --- a/rx1_motor/include/rx1_motor/rx1_motor.hpp +++ /dev/null @@ -1,117 +0,0 @@ -#ifndef RX1_MOTOR_H -#define RX1_MOTOR_H - -#include "feetech_lib/SMS_STS.h" -#include "feetech_lib/SCSCL.h" - -#include -#include -#include - -#include -#include - -namespace rx1_motor -{ - -class Rx1Motor -{ -public: - Rx1Motor(ros::NodeHandle& nh, ros::NodeHandle& priv_nh); - ~Rx1Motor(); - - void spinOnce(); - void spin(); - void update(); - -protected: - ros::NodeHandle nh_; - ros::NodeHandle priv_nh_; - -private: - std::string servo_port_; - SMS_STS sts_servo_; - SCSCL scs_servo_; - - static constexpr std::array right_arm_servo_ids_ = {11, 12, 13, 14, 15, 16, 17}; - static constexpr std::array right_arm_servo_dirs_ = {-1, -1, 1, 1, 1, 1, -1}; - static constexpr std::array right_arm_servo_gears_ = {3, 3, 3, 3, 1, 1, 1}; - - static constexpr std::array left_arm_servo_ids_ = {21, 22, 23, 24, 25, 26, 27}; - static constexpr std::array left_arm_servo_dirs_ = {-1, -1, 1, -1, 1, -1, -1}; - static constexpr std::array left_arm_servo_gears_ = {3, 3, 3, 3, 1, 1, 1}; - - static constexpr std::array head_servo_ids_ = {4, 5, 6, 7, 8}; - static constexpr std::array head_servo_dirs_ = {-1, -1, -1, 1, -1}; - static constexpr std::array head_servo_gears_ = {1, 1, 1, 1, 1}; - - static constexpr std::array torso_servo_ids_ = {1, 2, 3}; - static constexpr std::array torso_servo_dirs_ = {-1, 1, -1}; - static constexpr std::array torso_servo_gears_ = {3, 3, 3}; - - static constexpr std::array right_hand_servo_ids_ = {31, 32, 33, 34, 35, 36}; - static constexpr std::array right_hand_servo_default_ = {200, 2350, 2300, 420, 600, 440}; - static constexpr std::array right_hand_servo_range_ = {0, -320, -420, 220, -220, 220}; - - static constexpr std::array left_hand_servo_ids_ = {41, 42, 43, 44, 45, 46}; - static constexpr std::array left_hand_servo_default_ = {512, 1700, 1700, 650, 420, 630}; - static constexpr std::array left_hand_servo_range_ = {0, 350, 550, -260, 260, -300}; - - ros::Subscriber joint_state_sub_ = nh_.subscribe("/command_joint_states", 10, &Rx1Motor::jointStateCallback, this); - ros::Subscriber right_arm_joint_state_sub_ = nh_.subscribe("/right_arm_joint_states", 10, &Rx1Motor::rightArmJointStateCallback, this); - ros::Subscriber left_arm_joint_state_sub_ = nh_.subscribe("/left_arm_joint_states", 10, &Rx1Motor::leftArmJointStateCallback, this); - ros::Subscriber torso_joint_state_sub_ = nh_.subscribe("/torso_joint_states", 10, &Rx1Motor::torsoJointStateCallback, this); - ros::Subscriber head_joint_state_sub_ = nh_.subscribe("/head_joint_states", 10, &Rx1Motor::headJointStateCallback, this); - ros::Subscriber right_gripper_sub_ = nh_.subscribe("/right_gripper", 10, &Rx1Motor::rightGripperCallback, this); - ros::Subscriber left_gripper_sub_ = nh_.subscribe("/left_gripper", 10, &Rx1Motor::leftGripperCallback, this); - - void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); - void rightArmJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); - void leftArmJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); - void torsoJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); - void headJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); - void leftGripperCallback(const std_msgs::Float32::ConstPtr& msg); - void rightGripperCallback(const std_msgs::Float32::ConstPtr& msg); - - std::array torsoIk(double d, double L1, double h1, double h2, double roll, double pitch); - - void headMotorCommand(const std::vector& joint_positinos, - const std::vector& joint_speeds, - const std::vector& joint_accs); - - template - void motorCommand(const std::array& joint_ids, - const std::array& joint_dirs, - const std::array& joint_gears, - const std::vector& joint_angles, - const std::vector& joint_speeds, - const std::vector& joint_accs); - - static constexpr double TORSO_D_ = 0.0865; - static constexpr double TORSO_L1_ = 0.05; - static constexpr double TORSO_H1_ = 0.11; - static constexpr double TORSO_H2_ = 0.11; - - static constexpr double TORSO_SPEED_ = 1.0;// 500; - static constexpr double TORSO_ACC_ = 1.5; //10; - static constexpr double ARM_SPEED_ = 1.0; //700; - static constexpr double ARM_ACC_ = 3.0; //20; - static constexpr double HEAD_SPEED_ = 1.0; //500; - static constexpr double HEAD_ACC_ = 7.0; //50; - static constexpr double HAND_SPEED_ = 0; // maximum - static constexpr double HAND_ACC_ = 15; //100; - - // Based on Feetech manual, 50step/s = 0.732 RPM - // then 1 step /s = 0.00153232 rad /s - // 1/ 0.00153232 = 652.6051999582332, multiple this value to turn rad/s to Feetech speed - const double SPEED_CONST_ = 652.6051999582332; - // Based on Feetech manual, 1 unit of acc is 100 step/s^2 - // thus we can multiple this value to turn rad/s^2 to Feetech acc - const double ACC_CONST_ = 6.526051999582332; - - ros::Time last_spin_time_; -}; - -} // namespace rx1_motor - -#endif // RX1_MOTOR_H diff --git a/rx1_motor/src/rx1_motor/rx1_motor.cpp b/rx1_motor/src/rx1_motor/rx1_motor.cpp deleted file mode 100644 index 2837c89..0000000 --- a/rx1_motor/src/rx1_motor/rx1_motor.cpp +++ /dev/null @@ -1,417 +0,0 @@ -#include "rx1_motor/rx1_motor.hpp" - -#include -#include - -namespace rx1_motor -{ -/* -constexpr std::array Rx1Motor::right_arm_servo_ids_; -constexpr std::array Rx1Motor::right_arm_servo_dirs_; -constexpr std::array Rx1Motor::right_arm_servo_gears_; -constexpr std::array Rx1Motor::left_arm_servo_ids_; -constexpr std::array Rx1Motor::left_arm_servo_dirs_; -constexpr std::array Rx1Motor::left_arm_servo_gears_; -constexpr std::array Rx1Motor::head_servo_ids_; -constexpr std::array Rx1Motor::head_servo_dirs_; -constexpr std::array Rx1Motor::head_servo_gears_; -constexpr std::array Rx1Motor::torso_servo_ids_; -constexpr std::array Rx1Motor::torso_servo_dirs_; -constexpr std::array Rx1Motor::torso_servo_gears_; -constexpr std::array Rx1Motor::right_hand_servo_ids_; -constexpr std::array Rx1Motor::right_hand_servo_default_; -constexpr std::array Rx1Motor::right_hand_servo_range_; -constexpr std::array Rx1Motor::left_hand_servo_ids_; -constexpr std::array Rx1Motor::left_hand_servo_default_; -constexpr std::array Rx1Motor::left_hand_servo_range_; -*/ - -Rx1Motor::Rx1Motor(ros::NodeHandle& nh, ros::NodeHandle& priv_nh) - : nh_(nh), - priv_nh_(priv_nh) -{ - nh_.param("rx1_motor_node/servo_port", servo_port_, "/dev/ttyUSB-arduino4.3"); - - if (!sts_servo_.begin(1000000, servo_port_.c_str())) - { - ROS_ERROR("[RX1_MOTOR] Failed initialize sts servo port %s!", servo_port_.c_str()); - } - - if (!scs_servo_.begin(1000000, servo_port_.c_str())) - { - ROS_ERROR("[RX1_MOTOR] Failed initialize scs servo port %s!", servo_port_.c_str()); - } - - for(int i = 0; i < right_arm_servo_ids_.size(); i ++) - { - u8 id = right_arm_servo_ids_[i]; - sts_servo_.WritePosEx(id, 2048, 200, 20); - } - for(int i = 0; i < left_arm_servo_ids_.size(); i ++) - { - u8 id = left_arm_servo_ids_[i]; - sts_servo_.WritePosEx(id, 2048, 200, 20); - } - for(int i = 0; i < torso_servo_ids_.size(); i ++) - { - u8 id = torso_servo_ids_[i]; - sts_servo_.WritePosEx(id, 2048, 200, 20); - } - for(int i = 0; i < head_servo_ids_.size(); i ++) - { - u8 id = head_servo_ids_[i]; - - if (i == 1) - { - //sts_servo_.WritePosEx(id, 1600, 200, 20); - sts_servo_.WritePosEx(id, 2048, 200, 20); - } - else if (i == 0) - { - sts_servo_.WritePosEx(id, 2048, 200, 20); - } - else if (i == 2) - { - sts_servo_.WritePosEx(id, 2000, 200, 20); - } - else - { - scs_servo_.WritePos(id, 512, 0, 100); - } - } - - last_spin_time_ = ros::Time::now(); -} - -Rx1Motor::~Rx1Motor() -{ - sts_servo_.end(); -} - -void Rx1Motor::spinOnce() -{ - ros::spinOnce(); - update(); -} - -void Rx1Motor::spin() -{ - ros::Rate rate(100); - while(ros::ok()) - { - spinOnce(); - rate.sleep(); - double actual_rate = 1.0 / (ros::Time::now() - last_spin_time_).toSec(); - last_spin_time_ = ros::Time::now(); - ROS_INFO("[RX1_MOTOR] actual rate is %f", actual_rate); - } -} - -void Rx1Motor::update() -{ -} - - -void Rx1Motor::jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) -{ - std::vector joint_positions = msg->position; - - std::vector torso_joint_positions(3); - std::vector head_joint_positions(5); - std::vector right_arm_joint_positions(7); - std::vector left_arm_joint_positions(7); - - for (int i = 0; i < joint_positions.size(); i ++) - { - if (i < 3) - { - torso_joint_positions[i] = joint_positions[i]; - } - else if (i < 8) - { - head_joint_positions[i-3] = joint_positions[i]; - } - else if (i < 15) - { - right_arm_joint_positions[i-8] = joint_positions[i]; - } - else - { - left_arm_joint_positions[i-15] = joint_positions[i]; - } - } - - // Process the joint state information - auto angles = torsoIk(TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, torso_joint_positions[1], torso_joint_positions[2]); - torso_joint_positions[1] = angles[0]; - torso_joint_positions[2] = angles[1]; - std::vector torso_speeds_(torso_servo_ids_.size(), TORSO_SPEED_); - std::vector torso_accs_(torso_servo_ids_.size(), TORSO_ACC_); - motorCommand(torso_servo_ids_, torso_servo_dirs_, torso_servo_gears_, torso_joint_positions, torso_speeds_, torso_accs_); - - std::vector head_speeds(head_servo_ids_.size(), HEAD_SPEED_); - std::vector head_accs(head_servo_ids_.size(), HEAD_ACC_); - headMotorCommand(head_joint_positions, head_speeds, head_accs); - - - std::vector arm_speeds(right_arm_servo_ids_.size(), ARM_SPEED_); - std::vector arm_accs(right_arm_servo_ids_.size(), ARM_ACC_); - motorCommand(right_arm_servo_ids_, right_arm_servo_dirs_, right_arm_servo_gears_, right_arm_joint_positions, arm_speeds, arm_accs); - motorCommand(left_arm_servo_ids_, left_arm_servo_dirs_, left_arm_servo_gears_, left_arm_joint_positions, arm_speeds, arm_accs); - -} - - -void Rx1Motor::rightArmJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) -{ - // Access the joint state information - std::vector joint_positions = msg->position; - - //ros::Time command_start_time = ros::Time::now(); - // Process the joint state information - std::vector arm_speeds(right_arm_servo_ids_.size(), ARM_SPEED_); - std::vector arm_accs(right_arm_servo_ids_.size(), ARM_ACC_); - motorCommand(right_arm_servo_ids_, right_arm_servo_dirs_, right_arm_servo_gears_, joint_positions, arm_speeds, arm_accs); - //double time_spend = (ros::Time::now() - command_start_time).toSec(); - //ROS_INFO("[RX1_MOTOR] right arm command time is %f sec", time_spend); -} - -void Rx1Motor::leftArmJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) -{ - // Access the joint state information - std::vector joint_positions = msg->position; - - // Process the joint state information - std::vector arm_speeds(left_arm_servo_ids_.size(), ARM_SPEED_); - std::vector arm_accs(left_arm_servo_ids_.size(), ARM_ACC_); - motorCommand(left_arm_servo_ids_, left_arm_servo_dirs_, left_arm_servo_gears_, joint_positions, arm_speeds, arm_accs); -} - -void Rx1Motor::torsoJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) -{ - // Access the joint state information - std::vector joint_positions = msg->position; - - auto angles = torsoIk(TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, joint_positions[2], joint_positions[1]); - - joint_positions[1] = angles[0]; - joint_positions[2] = angles[1]; - - std::vector torso_speeds(torso_servo_ids_.size(), TORSO_SPEED_); - std::vector torso_accs(torso_servo_ids_.size(), TORSO_ACC_); - - motorCommand(torso_servo_ids_, torso_servo_dirs_, torso_servo_gears_, joint_positions, torso_speeds, torso_accs); -} - -std::array Rx1Motor::torsoIk(double d, double L1, double h1, double h2, double roll, double pitch) -{ - // obtained from https://github.com/rocketman123456/ros2_ws/blob/master/src/humanoid_motor_control_cpp/src/humanoid_control/kinematic/ankle_ik.cpp - double cx = cos(pitch); - double sx = sin(pitch); - double cy = cos(roll); - double sy = sin(roll); - - double AL = - L1 * L1 * cy + L1 * d * sx * sy; - double BL = - L1 * L1 * sy + L1 * h1 - L1 * d * sx * cy; - double CL = -(L1 * L1 + d * d - d * d *cx - L1 * h1 * sy - d * h1 * sx * cy); - - double LenL = sqrt(AL * AL + BL * BL); - - double AR = - L1 * L1 * cy - L1 * d * sx * sy; - double BR = - L1 * L1 * sy + L1 * h2 + L1 * d * sx * cy; - double CR = -(L1 * L1 + d * d - d * d *cx - L1 * h2 * sy + d * h2 * sx * cy); - - double LenR = sqrt(AR * AR + BR * BR); - - if (LenL <= abs(CL) || LenR <= abs(CR)) - { - return {0.0, 0.0}; - } - else - { - double tL_1 = asin(CL / LenL) - asin(AL / LenL); - double tL_2 = asin(CL / LenL) + acos(BL / LenL); - - double tR_1 = asin(CR / LenR) - asin(AR / LenR); - double tR_2 = asin(CR / LenR) + acos(BR / LenR); - - assert(fabs(tL_1 - tL_2) < 1e-3 && "tL_1 - tL_2 > 1e-3"); - assert(fabs(tR_1 - tR_2) < 1e-3 && "tR_1 - tR_2 > 1e-3"); - - return {tL_1, tR_1}; - } -} - - // Torso IK test - /* - auto angles = torsoIk(TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, 0, 0); - ROS_INFO("torso ik result for pitch 0, roll 0 is: %f %f", angles[0], angles[1]); - angles = torsoIk(TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, 0.2, 0); - ROS_INFO("torso ik result for pitch 0.2, roll 0 is: %f %f", angles[0], angles[1]); - angles = torsoIk(TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, 0, 0.2); - ROS_INFO("torso ik result for pitch 0, roll 0.2 is: %f %f", angles[0], angles[1]); - angles = torsoIk(TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, 0.2, 0.2); - ROS_INFO("torso ik result for pitch 0.2, roll 0.2 is: %f %f", angles[0], angles[1]); - */ - -void Rx1Motor::headJointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) -{ - // Access the joint state information - std::vector joint_positions = msg->position; - - std::vector joint_speeds(head_servo_ids_.size(), HEAD_SPEED_); - std::vector joint_accs(head_servo_ids_.size(), HEAD_ACC_); - - // Process the joint state information - headMotorCommand(joint_positions, joint_speeds, joint_accs); -} - -void Rx1Motor::rightGripperCallback(const std_msgs::Float32ConstPtr& msg) -{ - //ros::Time command_start_time = ros::Time::now(); - - double grip_ratio = msg->data; - - int length = right_hand_servo_ids_.size(); - u8 id; - s16 pos; - u16 speed = static_cast(HAND_SPEED_ * SPEED_CONST_); - u8 acc = static_cast(HAND_ACC_ * ACC_CONST_); - - for (int i = 0; i < length; i ++) - { - id = static_cast(right_hand_servo_ids_[i]); - pos = static_cast(right_hand_servo_default_[i] + grip_ratio * right_hand_servo_range_[i]); - - if (i == 1 || i == 2) // thumb and index fingers are sts servo, others are scs servos - { - sts_servo_.WritePosEx(id, pos, speed, acc); - } - else if (i == 3) - { - scs_servo_.WritePos(id, pos, 0, 400); // id, pos, time, speed - } - else if (i == 4) - { - scs_servo_.WritePos(id, pos, 0, 300); // id, pos, time, speed - } - else if (i == 5) - { - scs_servo_.WritePos(id, pos, 0, 200); // id, pos, time, speed - } - } - - // Thumb yaw - scs_servo_.WritePos(right_hand_servo_ids_[0], 200, 0, 400); // id, pos, time, speed - - //double time_spend = (ros::Time::now() - command_start_time).toSec(); - //ROS_INFO("[RX1_MOTOR] right hand command time is %f sec", time_spend); -} - -void Rx1Motor::leftGripperCallback(const std_msgs::Float32::ConstPtr& msg) -{ - //ros::Time command_start_time = ros::Time::now(); - double grip_ratio = msg->data; - - int length = left_hand_servo_ids_.size(); - u8 id; - s16 pos; - u16 speed = static_cast(HAND_SPEED_ * SPEED_CONST_); - u8 acc = static_cast(HAND_ACC_ * ACC_CONST_); - - for (int i = 0; i < length; i ++) - { - id = static_cast(left_hand_servo_ids_[i]); - pos = static_cast(left_hand_servo_default_[i] + grip_ratio * left_hand_servo_range_[i]); - if (i == 1 || i == 2) // thumb and index fingers are sts servo, others are scs servos - { - sts_servo_.WritePosEx(id, pos, speed, acc); - } - else if (i == 3) - { - scs_servo_.WritePos(id, pos, 0, 400); // id, pos, time, speed - } - else if (i == 4) - { - scs_servo_.WritePos(id, pos, 0, 300); // id, pos, time, speed - } - else if (i == 5) - { - scs_servo_.WritePos(id, pos, 0, 200); // id, pos, time, speed - } - } - - // Thumb yaw - scs_servo_.WritePos(left_hand_servo_ids_[0], 512, 0, 400); // id, pos, time, speed - //double time_spend = (ros::Time::now() - command_start_time).toSec(); - //ROS_INFO("[RX1_MOTOR] left hand command time is %f sec", time_spend); - -} - -void Rx1Motor::headMotorCommand(const std::vector& joint_positions, const std::vector& joint_speeds, const std::vector& joint_accs) -{ - u8 id; - s16 pos; - u16 speed; - u8 acc; - - for (int i = 0; i < joint_positions.size(); i ++) - { - id = static_cast(head_servo_ids_[i]); - speed = static_cast(joint_speeds[i] * SPEED_CONST_); - acc = static_cast(joint_accs[i] * ACC_CONST_); - if (i == 0 || i == 1 || i == 2) //neck - { - pos = static_cast(joint_positions[i]/3.14*2048*head_servo_dirs_[i]*head_servo_gears_[i] + 2048); - sts_servo_.WritePosEx(id, pos, speed, acc); - } - else // ear - { - pos = static_cast(joint_positions[i]/3.14*512*head_servo_dirs_[i]*head_servo_gears_[i] + 512); - scs_servo_.WritePos(id, pos, 0, 0); // id, pos, time, speed - } - } -} - -template -void Rx1Motor::motorCommand(const std::array& joint_ids, - const std::array& joint_dirs, - const std::array& joint_gears, - const std::vector& joint_angles, - const std::vector& joint_speeds, - const std::vector& joint_accs) -{ - int length = joint_ids.size(); - u8 *ids = (u8 *)malloc(length * sizeof(u8)); - s16 *pos = (s16 *)malloc(length * sizeof(s16)); - u16 *speeds = (u16* )malloc(length * sizeof(u16)); - u8 *accs = (u8 *)malloc(length * sizeof(u8)); - - if (ids == NULL || pos == NULL || speeds == NULL || accs == NULL) - { - ROS_WARN("[RX1_MOTOR] Motor command memory allocation failed!"); - } - - for (int i = 0; i < length; i ++) - { - ids[i] = static_cast(joint_ids[i]); - speeds[i] = static_cast(joint_speeds[i]*joint_gears[i]*SPEED_CONST_); - accs[i] = static_cast(joint_accs[i]*joint_gears[i]*ACC_CONST_); - pos[i] = static_cast(joint_angles[i]/3.14*2048*joint_dirs[i]*joint_gears[i] + 2048); - - // temporary change: make forearm faster - if (i >= 3) - { - speeds[i] = 0; - accs[i] = accs[i] * 10; - } - } - sts_servo_.SyncWritePosEx(ids, length, pos, speeds, accs); - - free(ids); - free(pos); - free(speeds); - free(accs); -} - -} // namespace rx1_motor diff --git a/rx1_motor/src/rx1_motor_node.cpp b/rx1_motor/src/rx1_motor_node.cpp deleted file mode 100644 index eb41908..0000000 --- a/rx1_motor/src/rx1_motor_node.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "rx1_motor/rx1_motor.hpp" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "rx1_motor_node"); - ros::NodeHandle nh; - ros::NodeHandle priv_nh("~"); - - rx1_motor::Rx1Motor rx1_motor_node(nh, priv_nh); - - try - { - rx1_motor_node.spin(); - } - catch (std::runtime_error& ex) - { - ROS_FATAL_STREAM("[RX1_MOTOR] Runtime error: " << ex.what()); - return 1; - } - return 0; -} - diff --git a/rx1_motor_lib/CMakeLists.txt b/rx1_motor_lib/CMakeLists.txt deleted file mode 100644 index 57d7132..0000000 --- a/rx1_motor_lib/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_Minimum_required(VERSION 2.8.3) -project(rx1_motor_lib) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - feetech_lib - roscpp - rospy - std_msgs -) - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES rx1_motor_lib - CATKIN_DEPENDS feetech_lib roscpp rospy std_msgs -) - -################################################################################ -# Build -################################################################################ -include_directories( - ${PROJECT_SOURCE_DIR}/include/rx1_motor_lib - ${catkin_INCLUDE_DIRS} -) - -file(GLOB hdrs include/rx1_motor_lib/*.hpp) -file(GLOB srs src/*.cpp) - -add_library(rx1_motor_lib ${hdrs} ${srs}) -add_dependencies(rx1_motor_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(rx1_motor_lib - ${catkin_LIBRARIES} -) diff --git a/rx1_motor_lib/package.xml b/rx1_motor_lib/package.xml deleted file mode 100644 index 89a4c54..0000000 --- a/rx1_motor_lib/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - rx1_motor_lib - 0.0.1 - RX1 motor driver package - - lethic - - MIT - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - feetech_lib - - diff --git a/src/feetech_lib/CMakeLists.txt b/src/feetech_lib/CMakeLists.txt new file mode 100644 index 0000000..178941f --- /dev/null +++ b/src/feetech_lib/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(feetech_lib) + +find_package(ament_cmake REQUIRED) + +file(GLOB hdrs include/feetech_lib/*.h) +file(GLOB srs src/*.cpp) + +add_library(feetech_lib ${hdrs} ${srs}) +target_compile_features(feetech_lib PUBLIC cxx_std_17) +target_include_directories(feetech_lib PUBLIC + $ + $ + $ +) + +install(TARGETS feetech_lib + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/LICENSE b/src/feetech_lib/LICENSE similarity index 100% rename from LICENSE rename to src/feetech_lib/LICENSE diff --git a/src/feetech_lib/README.md b/src/feetech_lib/README.md new file mode 100644 index 0000000..d739c6a --- /dev/null +++ b/src/feetech_lib/README.md @@ -0,0 +1,5 @@ +# feetech_lib +ROS 1 driver for Feetech servos. + +## Acknowledgement +This ROS package is created based on the [Feetech servo official SDK (specifically SCServo_Linux_220329.7z)](https://gitee.com/ftservo/SCServoSDK) and used Github repo feetech_lib as a reference. diff --git a/src/feetech_lib/include/feetech_lib/INST.h b/src/feetech_lib/include/feetech_lib/INST.h new file mode 100644 index 0000000..cc39027 --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/INST.h @@ -0,0 +1,26 @@ +/* + * INST.h + * 飞特串行舵机å议指令定义 + * 日期: 2021.12.8 + * 作者: + */ + +#ifndef _INST_H +#define _INST_H + +typedef char s8; +typedef unsigned char u8; +typedef unsigned short u16; +typedef short s16; +typedef unsigned long u32; +typedef long s32; + +#define INST_PING 0x01 +#define INST_READ 0x02 +#define INST_WRITE 0x03 +#define INST_REG_WRITE 0x04 +#define INST_REG_ACTION 0x05 +#define INST_SYNC_READ 0x82 +#define INST_SYNC_WRITE 0x83 + +#endif \ No newline at end of file diff --git a/src/feetech_lib/include/feetech_lib/SCS.h b/src/feetech_lib/include/feetech_lib/SCS.h new file mode 100644 index 0000000..cde81f2 --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/SCS.h @@ -0,0 +1,56 @@ +/* + * SCS.h + * 飞特串行舵机通信层åè®®ç¨‹åº + * 日期: 2022.3.29 + * 作者: + */ + +#ifndef _SCS_H +#define _SCS_H + +#include "INST.h" + +class SCS{ +public: + SCS(); + SCS(u8 End); + SCS(u8 End, u8 Level); + int genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//普通写指令 + int regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//异步写指令 + int RegWriteAction(u8 ID = 0xfe);//异步写执行指令 + void snycWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen);//åŒæ­¥å†™æŒ‡ä»¤ + int writeByte(u8 ID, u8 MemAddr, u8 bDat);//写1个字节 + int writeWord(u8 ID, u8 MemAddr, u16 wDat);//写2个字节 + int Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen);//读指令 + int readByte(u8 ID, u8 MemAddr);//读1个字节 + int readWord(u8 ID, u8 MemAddr);//读2个字节 + int Ping(u8 ID);//Ping指令 + int syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen);//åŒæ­¥è¯»æŒ‡ä»¤åŒ…å‘é€ + int syncReadPacketRx(u8 ID, u8 *nDat);//åŒæ­¥è¯»è¿”回包解ç ï¼ŒæˆåŠŸè¿”回内存字节数，失败返回0 + int syncReadRxPacketToByte();//解ç ä¸€ä¸ªå­—节 + int syncReadRxPacketToWrod(u8 negBit=0);//解ç ä¸¤ä¸ªå­—节，negBit为方å‘为，negBit=0è¡¨ç¤ºæ— æ–¹å‘ + void syncReadBegin(u8 IDN, u8 rxLen);//åŒæ­¥è¯»å¼€å§‹ + void syncReadEnd();//åŒæ­¥è¯»ç»“æŸ +public: + u8 Level;//舵机返回等级 + u8 End;//处ç†å™¨å¤§å°ç«¯ç»“æž„ + u8 Error;//èˆµæœºçŠ¶æ€ + u8 syncReadRxPacketIndex; + u8 syncReadRxPacketLen; + u8 *syncReadRxPacket; + u8 *syncReadRxBuff; + u16 syncReadRxBuffLen; + u16 syncReadRxBuffMax; +protected: + virtual int writeSCS(unsigned char *nDat, int nLen) = 0; + virtual int readSCS(unsigned char *nDat, int nLen) = 0; + virtual int writeSCS(unsigned char bDat) = 0; + virtual void rFlushSCS() = 0; + virtual void wFlushSCS() = 0; +protected: + void writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun); + void Host2SCS(u8 *DataL, u8* DataH, u16 Data);//1个16ä½æ•°æ‹†åˆ†ä¸º2个8ä½æ•° + u16 SCS2Host(u8 DataL, u8 DataH);//2个8ä½æ•°ç»„åˆä¸º1个16ä½æ•° + int Ack(u8 ID);//返回应答 +}; +#endif \ No newline at end of file diff --git a/src/feetech_lib/include/feetech_lib/SCSCL.h b/src/feetech_lib/include/feetech_lib/SCSCL.h new file mode 100644 index 0000000..ca328c5 --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/SCSCL.h @@ -0,0 +1,87 @@ +/* + * SCSCL.h + * 飞特SCSCLç³»åˆ—ä¸²è¡Œèˆµæœºåº”ç”¨å±‚ç¨‹åº + * 日期: 2020.6.17 + * 作者: + */ + +#ifndef _SCSCL_H +#define _SCSCL_H + + +#define SCSCL_1M 0 +#define SCSCL_0_5M 1 +#define SCSCL_250K 2 +#define SCSCL_128K 3 +#define SCSCL_115200 4 +#define SCSCL_76800 5 +#define SCSCL_57600 6 +#define SCSCL_38400 7 + +//内存表定义 +//-------EPROM(åªè¯»)-------- +#define SCSCL_VERSION_L 3 +#define SCSCL_VERSION_H 4 + +//-------EPROM(读写)-------- +#define SCSCL_ID 5 +#define SCSCL_BAUD_RATE 6 +#define SCSCL_MIN_ANGLE_LIMIT_L 9 +#define SCSCL_MIN_ANGLE_LIMIT_H 10 +#define SCSCL_MAX_ANGLE_LIMIT_L 11 +#define SCSCL_MAX_ANGLE_LIMIT_H 12 +#define SCSCL_CW_DEAD 26 +#define SCSCL_CCW_DEAD 27 + +//-------SRAM(读写)-------- +#define SCSCL_TORQUE_ENABLE 40 +#define SCSCL_GOAL_POSITION_L 42 +#define SCSCL_GOAL_POSITION_H 43 +#define SCSCL_GOAL_TIME_L 44 +#define SCSCL_GOAL_TIME_H 45 +#define SCSCL_GOAL_SPEED_L 46 +#define SCSCL_GOAL_SPEED_H 47 +#define SCSCL_LOCK 48 + +//-------SRAM(åªè¯»)-------- +#define SCSCL_PRESENT_POSITION_L 56 +#define SCSCL_PRESENT_POSITION_H 57 +#define SCSCL_PRESENT_SPEED_L 58 +#define SCSCL_PRESENT_SPEED_H 59 +#define SCSCL_PRESENT_LOAD_L 60 +#define SCSCL_PRESENT_LOAD_H 61 +#define SCSCL_PRESENT_VOLTAGE 62 +#define SCSCL_PRESENT_TEMPERATURE 63 +#define SCSCL_MOVING 66 +#define SCSCL_PRESENT_CURRENT_L 69 +#define SCSCL_PRESENT_CURRENT_H 70 + +#include "SCSerial.h" + +class SCSCL : public SCSerial +{ +public: + SCSCL(); + SCSCL(u8 End); + SCSCL(u8 End, u8 Level); + virtual int WritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//普通写å•ä¸ªèˆµæœºä½ç½®æŒ‡ä»¤ + virtual int RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//异步写å•ä¸ªèˆµæœºä½ç½®æŒ‡ä»¤(RegWriteAction生效) + virtual void SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[]);//åŒæ­¥å†™å¤šä¸ªèˆµæœºä½ç½®æŒ‡ä»¤ + virtual int PWMMode(u8 ID);//PWMè¾“å‡ºæ¨¡å¼ + virtual int WritePWM(u8 ID, s16 pwmOut);//PWM输出模å¼æŒ‡ä»¤ + virtual int EnableTorque(u8 ID, u8 Enable);//扭矩控制指令 + virtual int unLockEprom(u8 ID);//epromè§£é” + virtual int LockEprom(u8 ID);//epromåŠ é” + virtual int FeedBack(int ID);//åé¦ˆèˆµæœºä¿¡æ¯ + virtual int ReadPos(int ID);//读ä½ç½® + virtual int ReadSpeed(int ID);//读速度 + virtual int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000) + virtual int ReadVoltage(int ID);//读电压 + virtual int ReadTemper(int ID);//读温度 + virtual int ReadMove(int ID);//è¯»ç§»åŠ¨çŠ¶æ€ + virtual int ReadCurrent(int ID);//è¯»ç”µæµ +private: + u8 Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L+1]; +}; + +#endif \ No newline at end of file diff --git a/src/feetech_lib/include/feetech_lib/SCSerial.h b/src/feetech_lib/include/feetech_lib/SCSerial.h new file mode 100644 index 0000000..d6fe400 --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/SCSerial.h @@ -0,0 +1,48 @@ +/* + * SCSerial.h + * 飞特串行舵机硬件接å£å±‚ç¨‹åº + * 日期: 2022.3.29 + * 作者: + */ + +#ifndef _SCSERIAL_H +#define _SCSERIAL_H + +#include "SCS.h" +#include +#include +#include +#include +#include +#include + +class SCSerial : public SCS +{ +public: + SCSerial(); + SCSerial(u8 End); + SCSerial(u8 End, u8 Level); + +protected: + int writeSCS(unsigned char *nDat, int nLen);//输出nLen字节 + int readSCS(unsigned char *nDat, int nLen);//输入nLen字节 + int writeSCS(unsigned char bDat);//输出1字节 + void rFlushSCS();// + void wFlushSCS();// +public: + unsigned long int IOTimeOut;//输入输出超时 + int Err; +public: + virtual int getErr(){ return Err; } + virtual int setBaudRate(int baudRate); + virtual bool begin(int baudRate, const char* serialPort); + virtual void end(); +protected: + int fd;//serial port handle + struct termios orgopt;//fd ort opt + struct termios curopt;//fd cur opt + unsigned char txBuf[255]; + int txBufLen; +}; + +#endif \ No newline at end of file diff --git a/src/feetech_lib/include/feetech_lib/SCServo.h b/src/feetech_lib/include/feetech_lib/SCServo.h new file mode 100644 index 0000000..e50a63e --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/SCServo.h @@ -0,0 +1,15 @@ +/* + * SCServo.h + * é£žç‰¹ä¸²è¡ŒèˆµæœºæŽ¥å£ + * 日期: 2021.12.8 + * 作者: + */ + +#ifndef _SCSERVO_H +#define _SCSERVO_H + +#include "SMSBL.h" +#include "SCSCL.h" +#include "SMSCL.h" +#include "SMS_STS.h" +#endif \ No newline at end of file diff --git a/src/feetech_lib/include/feetech_lib/SMSBL.h b/src/feetech_lib/include/feetech_lib/SMSBL.h new file mode 100644 index 0000000..f427cfb --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/SMSBL.h @@ -0,0 +1,92 @@ +/* + * SMSBL.h + * 飞特SMSBLç³»åˆ—ä¸²è¡Œèˆµæœºåº”ç”¨å±‚ç¨‹åº + * 日期: 2020.6.17 + * 作者: + */ + +#ifndef _SMSBL_H +#define _SMSBL_H + +//波特率定义 +#define SMSBL_1M 0 +#define SMSBL_0_5M 1 +#define SMSBL_250K 2 +#define SMSBL_128K 3 +#define SMSBL_115200 4 +#define SMSBL_76800 5 +#define SMSBL_57600 6 +#define SMSBL_38400 7 + +//内存表定义 +//-------EPROM(åªè¯»)-------- +#define SMSBL_MODEL_L 3 +#define SMSBL_MODEL_H 4 + +//-------EPROM(读写)-------- +#define SMSBL_ID 5 +#define SMSBL_BAUD_RATE 6 +#define SMSBL_MIN_ANGLE_LIMIT_L 9 +#define SMSBL_MIN_ANGLE_LIMIT_H 10 +#define SMSBL_MAX_ANGLE_LIMIT_L 11 +#define SMSBL_MAX_ANGLE_LIMIT_H 12 +#define SMSBL_CW_DEAD 26 +#define SMSBL_CCW_DEAD 27 +#define SMSBL_OFS_L 31 +#define SMSBL_OFS_H 32 +#define SMSBL_MODE 33 + +//-------SRAM(读写)-------- +#define SMSBL_TORQUE_ENABLE 40 +#define SMSBL_ACC 41 +#define SMSBL_GOAL_POSITION_L 42 +#define SMSBL_GOAL_POSITION_H 43 +#define SMSBL_GOAL_TIME_L 44 +#define SMSBL_GOAL_TIME_H 45 +#define SMSBL_GOAL_SPEED_L 46 +#define SMSBL_GOAL_SPEED_H 47 +#define SMSBL_LOCK 55 + +//-------SRAM(åªè¯»)-------- +#define SMSBL_PRESENT_POSITION_L 56 +#define SMSBL_PRESENT_POSITION_H 57 +#define SMSBL_PRESENT_SPEED_L 58 +#define SMSBL_PRESENT_SPEED_H 59 +#define SMSBL_PRESENT_LOAD_L 60 +#define SMSBL_PRESENT_LOAD_H 61 +#define SMSBL_PRESENT_VOLTAGE 62 +#define SMSBL_PRESENT_TEMPERATURE 63 +#define SMSBL_MOVING 66 +#define SMSBL_PRESENT_CURRENT_L 69 +#define SMSBL_PRESENT_CURRENT_H 70 + +#include "SCSerial.h" + +class SMSBL : public SCSerial +{ +public: + SMSBL(); + SMSBL(u8 End); + SMSBL(u8 End, u8 Level); + virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//普通写å•ä¸ªèˆµæœºä½ç½®æŒ‡ä»¤ + virtual int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//异步写å•ä¸ªèˆµæœºä½ç½®æŒ‡ä»¤(RegWriteAction生效) + virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//åŒæ­¥å†™å¤šä¸ªèˆµæœºä½ç½®æŒ‡ä»¤ + virtual int WheelMode(u8 ID);//æ’é€Ÿæ¨¡å¼ + virtual int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0);//æ’速模å¼æŽ§åˆ¶æŒ‡ä»¤ + virtual int EnableTorque(u8 ID, u8 Enable);//扭力控制指令 + virtual int unLockEprom(u8 ID);//epromè§£é” + virtual int LockEprom(u8 ID);//epromåŠ é” + virtual int CalibrationOfs(u8 ID);//中ä½æ ¡å‡† + virtual int FeedBack(int ID);//åé¦ˆèˆµæœºä¿¡æ¯ + virtual int ReadPos(int ID);//读ä½ç½® + virtual int ReadSpeed(int ID);//读速度 + virtual int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000) + virtual int ReadVoltage(int ID);//读电压 + virtual int ReadTemper(int ID);//读温度 + virtual int ReadMove(int ID);//è¯»ç§»åŠ¨çŠ¶æ€ + virtual int ReadCurrent(int ID);//è¯»ç”µæµ +private: + u8 Mem[SMSBL_PRESENT_CURRENT_H-SMSBL_PRESENT_POSITION_L+1]; +}; + +#endif \ No newline at end of file diff --git a/src/feetech_lib/include/feetech_lib/SMSCL.h b/src/feetech_lib/include/feetech_lib/SMSCL.h new file mode 100644 index 0000000..e5e79c4 --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/SMSCL.h @@ -0,0 +1,109 @@ +/* + * SMSCL.h + * ·ÉÌØSMSCLϵÁд®Ðжæ»ú½Ó¿Ú + * ÈÕÆÚ: 2020.6.17 + * ×÷Õß: + */ + +#ifndef _SMSCL_H +#define _SMSCL_H + + +#define SMSCL_1M 0 +#define SMSCL_0_5M 1 +#define SMSCL_250K 2 +#define SMSCL_128K 3 +#define SMSCL_115200 4 +#define SMSCL_76800 5 +#define SMSCL_57600 6 +#define SMSCL_38400 7 + +//ÄÚ´æ±í¶¨Òå +//-------EPROM(Ö»¶Á)-------- +#define SMSCL_VERSION_L 3 +#define SMSCL_VERSION_H 4 + +//-------EPROM(¶Áд)-------- +#define SMSCL_ID 5 +#define SMSCL_BAUD_RATE 6 +#define SMSCL_RETURN_DELAY_TIME 7 +#define SMSCL_RETURN_LEVEL 8 +#define SMSCL_MIN_ANGLE_LIMIT_L 9 +#define SMSCL_MIN_ANGLE_LIMIT_H 10 +#define SMSCL_MAX_ANGLE_LIMIT_L 11 +#define SMSCL_MAX_ANGLE_LIMIT_H 12 +#define SMSCL_LIMIT_TEMPERATURE 13 +#define SMSCL_MAX_LIMIT_VOLTAGE 14 +#define SMSCL_MIN_LIMIT_VOLTAGE 15 +#define SMSCL_MAX_TORQUE_L 16 +#define SMSCL_MAX_TORQUE_H 17 +#define SMSCL_ALARM_LED 19 +#define SMSCL_ALARM_SHUTDOWN 20 +#define SMSCL_COMPLIANCE_P 21 +#define SMSCL_COMPLIANCE_D 22 +#define SMSCL_COMPLIANCE_I 23 +#define SMSCL_PUNCH_L 24 +#define SMSCL_PUNCH_H 25 +#define SMSCL_CW_DEAD 26 +#define SMSCL_CCW_DEAD 27 +#define SMSCL_OFS_L 33 +#define SMSCL_OFS_H 34 +#define SMSCL_MODE 35 +#define SMSCL_MAX_CURRENT_L 36 +#define SMSCL_MAX_CURRENT_H 37 + +//-------SRAM(¶Áд)-------- +#define SMSCL_TORQUE_ENABLE 40 +#define SMSCL_ACC 41 +#define SMSCL_GOAL_POSITION_L 42 +#define SMSCL_GOAL_POSITION_H 43 +#define SMSCL_GOAL_TIME_L 44 +#define SMSCL_GOAL_TIME_H 45 +#define SMSCL_GOAL_SPEED_L 46 +#define SMSCL_GOAL_SPEED_H 47 +#define SMSCL_LOCK 48 + +//-------SRAM(Ö»¶Á)-------- +#define SMSCL_PRESENT_POSITION_L 56 +#define SMSCL_PRESENT_POSITION_H 57 +#define SMSCL_PRESENT_SPEED_L 58 +#define SMSCL_PRESENT_SPEED_H 59 +#define SMSCL_PRESENT_LOAD_L 60 +#define SMSCL_PRESENT_LOAD_H 61 +#define SMSCL_PRESENT_VOLTAGE 62 +#define SMSCL_PRESENT_TEMPERATURE 63 +#define SMSCL_REGISTERED_INSTRUCTION 64 +#define SMSCL_MOVING 66 +#define SMSCL_PRESENT_CURRENT_L 69 +#define SMSCL_PRESENT_CURRENT_H 70 + +#include "SCSerial.h" + +class SMSCL : public SCSerial +{ +public: + SMSCL(); + SMSCL(u8 End); + SMSCL(u8 End, u8 Level); + virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//ÆÕͨдµ¥¸ö¶æ»úλÖÃÖ¸Áî + virtual int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//Ò첽дµ¥¸ö¶æ»úλÖÃÖ¸Áî(RegWriteActionÉúЧ) + virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//ͬ²½Ð´¶à¸ö¶æ»úλÖÃÖ¸Áî + virtual int WheelMode(u8 ID);//ºãËÙģʽ + virtual int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0);//ºãËÙģʽ¿ØÖÆÖ¸Áî + virtual int EnableTorque(u8 ID, u8 Enable);//ŤÁ¦¿ØÖÆÖ¸Áî + virtual int unLockEprom(u8 ID);//eprom½âËø + virtual int LockEprom(u8 ID);//eprom¼ÓËø + virtual int CalibrationOfs(u8 ID);//ÖÐλУ׼ + virtual int FeedBack(int ID);//·´À¡¶æ»úÐÅÏ¢ + virtual int ReadPos(int ID);//¶ÁλÖà + virtual int ReadSpeed(int ID);//¶ÁËÙ¶È + virtual int ReadLoad(int ID);//¶ÁÊä³öÖÁµç»úµÄµçѹ°Ù·Ö±È(0~1000) + virtual int ReadVoltage(int ID);//¶Áµçѹ + virtual int ReadTemper(int ID);//¶ÁÎÂ¶È + virtual int ReadMove(int ID);////¶ÁÒƶ¯×´Ì¬ + virtual int ReadCurrent(int ID);//¶ÁµçÁ÷ +private: + u8 Mem[SMSCL_PRESENT_CURRENT_H-SMSCL_PRESENT_POSITION_L+1]; +}; + +#endif \ No newline at end of file diff --git a/src/feetech_lib/include/feetech_lib/SMS_STS.h b/src/feetech_lib/include/feetech_lib/SMS_STS.h new file mode 100644 index 0000000..cc79366 --- /dev/null +++ b/src/feetech_lib/include/feetech_lib/SMS_STS.h @@ -0,0 +1,99 @@ +/* + * SMS_STS.h + * 飞特SMS/STSç³»åˆ—ä¸²è¡Œèˆµæœºåº”ç”¨å±‚ç¨‹åº + * 日期: 2021.12.8 + * 作者: + */ + +#ifndef _SMS_STS_H +#define _SMS_STS_H + +//波特率定义 +#define SMS_STS_1M 0 +#define SMS_STS_0_5M 1 +#define SMS_STS_250K 2 +#define SMS_STS_128K 3 +#define SMS_STS_115200 4 +#define SMS_STS_76800 5 +#define SMS_STS_57600 6 +#define SMS_STS_38400 7 + +//内存表定义 +//-------EPROM(åªè¯»)-------- +#define SMS_STS_MODEL_L 3 +#define SMS_STS_MODEL_H 4 + +//-------EPROM(读写)-------- +#define SMS_STS_ID 5 +#define SMS_STS_BAUD_RATE 6 +#define SMS_STS_MIN_ANGLE_LIMIT_L 9 +#define SMS_STS_MIN_ANGLE_LIMIT_H 10 +#define SMS_STS_MAX_ANGLE_LIMIT_L 11 +#define SMS_STS_MAX_ANGLE_LIMIT_H 12 +#define SMS_STS_CW_DEAD 26 +#define SMS_STS_CCW_DEAD 27 +#define SMS_STS_OFS_L 31 +#define SMS_STS_OFS_H 32 +#define SMS_STS_MODE 33 + +//-------SRAM(读写)-------- +#define SMS_STS_TORQUE_ENABLE 40 +#define SMS_STS_ACC 41 +#define SMS_STS_GOAL_POSITION_L 42 +#define SMS_STS_GOAL_POSITION_H 43 +#define SMS_STS_GOAL_TIME_L 44 +#define SMS_STS_GOAL_TIME_H 45 +#define SMS_STS_GOAL_SPEED_L 46 +#define SMS_STS_GOAL_SPEED_H 47 +#define SMSBL_TORQUE_LIMIT_L 48 +#define SMSBL_TORQUE_LIMIT_H 49 +#define SMS_STS_LOCK 55 + +//-------SRAM(åªè¯»)-------- +#define SMS_STS_PRESENT_POSITION_L 56 +#define SMS_STS_PRESENT_POSITION_H 57 +#define SMS_STS_PRESENT_SPEED_L 58 +#define SMS_STS_PRESENT_SPEED_H 59 +#define SMS_STS_PRESENT_LOAD_L 60 +#define SMS_STS_PRESENT_LOAD_H 61 +#define SMS_STS_PRESENT_VOLTAGE 62 +#define SMS_STS_PRESENT_TEMPERATURE 63 +#define SMS_STS_MOVING 66 +#define SMS_STS_PRESENT_CURRENT_L 69 +#define SMS_STS_PRESENT_CURRENT_H 70 + +#include "SCSerial.h" + +class SMS_STS : public SCSerial +{ +public: + SMS_STS(); + SMS_STS(u8 End); + SMS_STS(u8 End, u8 Level); + virtual int WriteID(u8 ID, u8 NewID); + virtual int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//普通写å•ä¸ªèˆµæœºä½ç½®æŒ‡ä»¤ + virtual int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//异步写å•ä¸ªèˆµæœºä½ç½®æŒ‡ä»¤(RegWriteAction生效) + virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//åŒæ­¥å†™å¤šä¸ªèˆµæœºä½ç½®æŒ‡ä»¤ + virtual int WriteTorqueLimit(u8 ID, u16 TorqueLimit); + virtual int WheelMode(u8 ID);//æ’é€Ÿæ¨¡å¼ + virtual int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0);//æ’速模å¼æŽ§åˆ¶æŒ‡ä»¤ + virtual int PWMMode(u8 ID);//PWMæ¨¡å¼ + virtual int PositionMode(u8 ID);//Positionæ¨¡å¼ + virtual int WritePWM(u8 ID, s16 pwmOut);//PWM模å¼æŽ§åˆ¶æŒ‡ä»¤ + virtual int EnableTorque(u8 ID, u8 Enable);//扭力控制指令 + virtual int unLockEprom(u8 ID);//epromè§£é” + virtual int LockEprom(u8 ID);//epromåŠ é” + virtual int CalibrationOfs(u8 ID);//中ä½æ ¡å‡† + virtual int FeedBack(int ID);//åé¦ˆèˆµæœºä¿¡æ¯ + virtual int ReadPos(int ID);//读ä½ç½® + virtual int ReadSpeed(int ID);//读速度 + virtual int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000) + virtual int ReadVoltage(int ID);//读电压 + virtual int ReadTemper(int ID);//读温度 + virtual int ReadMove(int ID);//è¯»ç§»åŠ¨çŠ¶æ€ + virtual int ReadCurrent(int ID);//è¯»ç”µæµ +private: + u8 Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L+1]; +}; + +#endif diff --git a/src/feetech_lib/package.xml b/src/feetech_lib/package.xml new file mode 100644 index 0000000..727535a --- /dev/null +++ b/src/feetech_lib/package.xml @@ -0,0 +1,13 @@ + + + feetech_lib + 0.0.1 + Feetech servo driver package + lethic + MIT + ament_cmake + + + ament_cmake + + diff --git a/src/feetech_lib/src/SCS.cpp b/src/feetech_lib/src/SCS.cpp new file mode 100644 index 0000000..f4f0e07 --- /dev/null +++ b/src/feetech_lib/src/SCS.cpp @@ -0,0 +1,394 @@ +/* + * SCS.cpp + * 飞特串行舵机通信层åè®®ç¨‹åº + * 日期: 2022.3.29 + * 作者: + */ +#include +#include +#include +#include "SCS.h" + +SCS::SCS() +{ + Level = 1;//除广播指令所有指令返回应答 + Error = 0; +} + +SCS::SCS(u8 End) +{ + Level = 1; + this->End = End; + Error = 0; +} + +SCS::SCS(u8 End, u8 Level) +{ + this->Level = Level; + this->End = End; + Error = 0; +} + +//1个16ä½æ•°æ‹†åˆ†ä¸º2个8ä½æ•° +//DataL为低ä½ï¼ŒDataHä¸ºé«˜ä½ +void SCS::Host2SCS(u8 *DataL, u8* DataH, u16 Data) +{ + if(End){ + *DataL = (Data>>8); + *DataH = (Data&0xff); + }else{ + *DataH = (Data>>8); + *DataL = (Data&0xff); + } +} + +//2个8ä½æ•°ç»„åˆä¸º1个16ä½æ•° +//DataL为低ä½ï¼ŒDataHä¸ºé«˜ä½ +u16 SCS::SCS2Host(u8 DataL, u8 DataH) +{ + u16 Data; + if(End){ + Data = DataL; + Data<<=8; + Data |= DataH; + }else{ + Data = DataH; + Data<<=8; + Data |= DataL; + } + return Data; +} + +void SCS::writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun) +{ + u8 msgLen = 2; + u8 bBuf[6]; + u8 CheckSum = 0; + bBuf[0] = 0xff; + bBuf[1] = 0xff; + bBuf[2] = ID; + bBuf[4] = Fun; + if(nDat){ + msgLen += nLen + 1; + bBuf[3] = msgLen; + bBuf[5] = MemAddr; + writeSCS(bBuf, 6); + + }else{ + bBuf[3] = msgLen; + writeSCS(bBuf, 5); + } + CheckSum = ID + msgLen + Fun + MemAddr; + u8 i = 0; + if(nDat){ + for(i=0; i=syncReadRxPacketLen){ + return -1; + } + return syncReadRxPacket[syncReadRxPacketIndex++]; +} + +int SCS::syncReadRxPacketToWrod(u8 negBit) +{ + if((syncReadRxPacketIndex+1)>=syncReadRxPacketLen){ + return -1; + } + int Word = SCS2Host(syncReadRxPacket[syncReadRxPacketIndex], syncReadRxPacket[syncReadRxPacketIndex+1]); + syncReadRxPacketIndex += 2; + if(negBit){ + if(Word&(1< - + rx1 0.0.0 The rx1 robot package lethic - MIT - - catkin + ament_cmake rx1_description rx1_bringup rx1_gazebo rx1_motor rx1_motor_lib rx1_ik - - + rx1_moveit_config + + ament_cmake + + diff --git a/src/rx1/rx1_bringup/CMakeLists.txt b/src/rx1/rx1_bringup/CMakeLists.txt new file mode 100644 index 0000000..b602deb --- /dev/null +++ b/src/rx1/rx1_bringup/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.8) +project(rx1_bringup) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch scripts + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/rx1_bringup/launch/bringup.launch b/src/rx1/rx1_bringup/launch/bringup.launch similarity index 100% rename from rx1_bringup/launch/bringup.launch rename to src/rx1/rx1_bringup/launch/bringup.launch diff --git a/src/rx1/rx1_bringup/launch/bringup.launch.py b/src/rx1/rx1_bringup/launch/bringup.launch.py new file mode 100644 index 0000000..44d4aff --- /dev/null +++ b/src/rx1/rx1_bringup/launch/bringup.launch.py @@ -0,0 +1,39 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + launch_camera = LaunchConfiguration("launch_camera") + servo_port = LaunchConfiguration("servo_port") + + return LaunchDescription([ + DeclareLaunchArgument("launch_camera", default_value="false"), + DeclareLaunchArgument("servo_port", default_value="/dev/ttyUSB-arduino3.3"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("rx1_motor"), "launch", "rx1_motor.launch.py"] + ) + ), + launch_arguments={"servo_port": servo_port}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("rx1_description"), "launch", "urdf.launch.py"] + ) + ) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("rx1_bringup"), "launch", "includes", "gemini2.launch.py"] + ) + ), + condition=IfCondition(launch_camera), + ), + ]) diff --git a/src/rx1/rx1_bringup/launch/includes/description.launch.py b/src/rx1/rx1_bringup/launch/includes/description.launch.py new file mode 100644 index 0000000..9defc4f --- /dev/null +++ b/src/rx1/rx1_bringup/launch/includes/description.launch.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + ) + ]) diff --git a/rx1_bringup/launch/includes/description.launch.xml b/src/rx1/rx1_bringup/launch/includes/description.launch.xml similarity index 100% rename from rx1_bringup/launch/includes/description.launch.xml rename to src/rx1/rx1_bringup/launch/includes/description.launch.xml diff --git a/rx1_bringup/launch/includes/gemini2.launch b/src/rx1/rx1_bringup/launch/includes/gemini2.launch similarity index 100% rename from rx1_bringup/launch/includes/gemini2.launch rename to src/rx1/rx1_bringup/launch/includes/gemini2.launch diff --git a/src/rx1/rx1_bringup/launch/includes/gemini2.launch.py b/src/rx1/rx1_bringup/launch/includes/gemini2.launch.py new file mode 100644 index 0000000..7382cf9 --- /dev/null +++ b/src/rx1/rx1_bringup/launch/includes/gemini2.launch.py @@ -0,0 +1,95 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + arg_names = { + "camera_name": "camera", + "depth_registration": "true", + "serial_number": "", + "usb_port": "", + "device_num": "1", + "vendor_id": "0x2bc5", + "product_id": "", + "enable_point_cloud": "true", + "enable_colored_point_cloud": "true", + "connection_delay": "100", + "color_width": "640", + "color_height": "360", + "color_fps": "15", + "enable_color": "true", + "color_format": "MJPG", + "flip_color": "false", + "enable_color_auto_exposure": "true", + "depth_width": "640", + "depth_height": "400", + "depth_fps": "15", + "enable_depth": "true", + "depth_format": "Y14", + "flip_depth": "false", + "depth_precision": "1mm", + "ir_width": "640", + "ir_height": "400", + "ir_fps": "15", + "enable_ir": "true", + "ir_format": "Y8", + "flip_ir": "false", + "enable_ir_auto_exposure": "true", + "enable_accel": "false", + "accel_rate": "100hz", + "accel_range": "4g", + "enable_gyro": "false", + "gyro_rate": "100hz", + "gyro_range": "1000dps", + "liner_accel_cov": "0.01", + "angular_vel_cov": "0.01", + "publish_tf": "true", + "tf_publish_rate": "10.0", + "ir_info_uri": "", + "color_info_uri": "", + "log_level": "none", + "enable_d2c_viewer": "false", + "enable_pipeline": "true", + "enable_ldp": "true", + "enable_soft_filter": "true", + "soft_filter_max_diff": "-1", + "soft_filter_speckle_size": "-1", + "sync_mode": "free_run", + "depth_delay_us": "0", + "color_delay_us": "0", + "trigger2image_delay_us": "0", + "trigger_out_delay_us": "0", + "trigger_out_enabled": "false", + "depth_work_mode": "", + "enable_frame_sync": "false", + } + + declare_args = [ + DeclareLaunchArgument(name, default_value=value) + for name, value in arg_names.items() + ] + + parameters = {name: LaunchConfiguration(name) for name in arg_names} + camera_name = LaunchConfiguration("camera_name") + + return LaunchDescription( + declare_args + + [ + Node( + package="orbbec_camera", + executable="orbbec_camera_node", + namespace=camera_name, + name="camera", + output="screen", + parameters=[parameters], + remappings=[ + ( + ["/", camera_name, "/depth/color/points"], + ["/", camera_name, "/depth_registered/points"], + ) + ], + ) + ] + ) diff --git a/rx1_bringup/launch/includes/teleop.launch b/src/rx1/rx1_bringup/launch/includes/teleop.launch similarity index 100% rename from rx1_bringup/launch/includes/teleop.launch rename to src/rx1/rx1_bringup/launch/includes/teleop.launch diff --git a/src/rx1/rx1_bringup/package.xml b/src/rx1/rx1_bringup/package.xml new file mode 100644 index 0000000..f437860 --- /dev/null +++ b/src/rx1/rx1_bringup/package.xml @@ -0,0 +1,17 @@ + + + rx1_bringup + 1.2.5 + Launch files for starting the rx1 robot + MIT + lethic + lethic + ament_cmake + rx1_description + rx1_motor + xacro + + + ament_cmake + + diff --git a/src/rx1/rx1_bringup/scripts/play_bags.py b/src/rx1/rx1_bringup/scripts/play_bags.py new file mode 100644 index 0000000..d185477 --- /dev/null +++ b/src/rx1/rx1_bringup/scripts/play_bags.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 + +import subprocess + + +ROSBAGS = [ + "/home/lingkang/rosbag/dual_arm_hand_bag.bag", + "/home/lingkang/rosbag/command_head.bag", + "/home/lingkang/rosbag/right_arm_2.bag", + "/home/lingkang/rosbag/command_head.bag", + "/home/lingkang/rosbag/dual_arm_hand_bag.bag", + "/home/lingkang/rosbag/command_head.bag", + "/home/lingkang/rosbag/dual_arm_hand_bag.bag", + "/home/lingkang/rosbag/command_head.bag", + "/home/lingkang/rosbag/dual_arm_hand_bag.bag", + "/home/lingkang/rosbag/command_head.bag", +] + + +def play_rosbag(bag_file: str) -> None: + print(f"Playing {bag_file}") + process = subprocess.Popen(["ros2", "bag", "play", "--clock", bag_file]) + process.wait() + + +if __name__ == "__main__": + for _ in range(5): + for bag in ROSBAGS: + play_rosbag(bag) diff --git a/src/rx1/rx1_description/CMakeLists.txt b/src/rx1/rx1_description/CMakeLists.txt new file mode 100644 index 0000000..6f62dfd --- /dev/null +++ b/src/rx1/rx1_description/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.8) +project(rx1_description) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch meshes rviz scripts urdf + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/rx1_description/launch/record_urdf_test.launch b/src/rx1/rx1_description/launch/record_urdf_test.launch similarity index 100% rename from rx1_description/launch/record_urdf_test.launch rename to src/rx1/rx1_description/launch/record_urdf_test.launch diff --git a/src/rx1/rx1_description/launch/record_urdf_test.launch.py b/src/rx1/rx1_description/launch/record_urdf_test.launch.py new file mode 100644 index 0000000..2f86545 --- /dev/null +++ b/src/rx1/rx1_description/launch/record_urdf_test.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import FindExecutable +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + model = LaunchConfiguration("model") + rvizconfig = LaunchConfiguration("rvizconfig") + + robot_description = { + "robot_description": Command([FindExecutable(name="xacro"), " ", model]) + } + + return LaunchDescription([ + DeclareLaunchArgument( + "model", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_description"), "urdf", "rx1.urdf"] + ), + ), + DeclareLaunchArgument( + "rvizconfig", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_description"), "rviz", "urdf.rviz"] + ), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[robot_description], + remappings=[("/joint_states", "/command_joint_states")], + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + output="screen", + remappings=[("/joint_states", "/command_joint_states")], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rvizconfig], + output="screen", + ), + ]) diff --git a/rx1_description/launch/urdf.launch b/src/rx1/rx1_description/launch/urdf.launch similarity index 100% rename from rx1_description/launch/urdf.launch rename to src/rx1/rx1_description/launch/urdf.launch diff --git a/src/rx1/rx1_description/launch/urdf.launch.py b/src/rx1/rx1_description/launch/urdf.launch.py new file mode 100644 index 0000000..a9c6072 --- /dev/null +++ b/src/rx1/rx1_description/launch/urdf.launch.py @@ -0,0 +1,64 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import FindExecutable + + +def generate_launch_description(): + model = LaunchConfiguration("model") + rvizconfig = LaunchConfiguration("rvizconfig") + + robot_description = { + "robot_description": Command([FindExecutable(name="xacro"), " ", model]) + } + + return LaunchDescription([ + DeclareLaunchArgument( + "model", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_description"), "urdf", "rx1.urdf"] + ), + ), + DeclareLaunchArgument( + "rvizconfig", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_description"), "rviz", "urdf.rviz"] + ), + ), + Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_aggregator", + output="screen", + parameters=[ + { + "source_list": [ + "/right_arm_joint_states", + "/left_arm_joint_states", + "/torso_joint_states", + "/head_joint_states", + "/command_joint_states", + "/visualize_joint_states", + ] + } + ], + remappings=[("/joint_states", "/aggregated_joint_states")], + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[robot_description, {"publish_frequency": 50.0}], + remappings=[("/joint_states", "/aggregated_joint_states")], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rvizconfig], + output="screen", + ), + ]) diff --git a/rx1_description/launch/urdf_test.launch b/src/rx1/rx1_description/launch/urdf_test.launch similarity index 100% rename from rx1_description/launch/urdf_test.launch rename to src/rx1/rx1_description/launch/urdf_test.launch diff --git a/src/rx1/rx1_description/launch/urdf_test.launch.py b/src/rx1/rx1_description/launch/urdf_test.launch.py new file mode 100644 index 0000000..2f86545 --- /dev/null +++ b/src/rx1/rx1_description/launch/urdf_test.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import FindExecutable +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + model = LaunchConfiguration("model") + rvizconfig = LaunchConfiguration("rvizconfig") + + robot_description = { + "robot_description": Command([FindExecutable(name="xacro"), " ", model]) + } + + return LaunchDescription([ + DeclareLaunchArgument( + "model", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_description"), "urdf", "rx1.urdf"] + ), + ), + DeclareLaunchArgument( + "rvizconfig", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_description"), "rviz", "urdf.rviz"] + ), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[robot_description], + remappings=[("/joint_states", "/command_joint_states")], + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + output="screen", + remappings=[("/joint_states", "/command_joint_states")], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rvizconfig], + output="screen", + ), + ]) diff --git a/rx1_description/meshes/archive/torso_base_link.stl b/src/rx1/rx1_description/meshes/archive/torso_base_link.stl similarity index 100% rename from rx1_description/meshes/archive/torso_base_link.stl rename to src/rx1/rx1_description/meshes/archive/torso_base_link.stl diff --git a/rx1_description/meshes/archive/torso_link.stl b/src/rx1/rx1_description/meshes/archive/torso_link.stl similarity index 100% rename from rx1_description/meshes/archive/torso_link.stl rename to src/rx1/rx1_description/meshes/archive/torso_link.stl diff --git a/rx1_description/meshes/archive/torso_link_v2.stl b/src/rx1/rx1_description/meshes/archive/torso_link_v2.stl similarity index 100% rename from rx1_description/meshes/archive/torso_link_v2.stl rename to src/rx1/rx1_description/meshes/archive/torso_link_v2.stl diff --git a/rx1_description/meshes/base_link.stl b/src/rx1/rx1_description/meshes/base_link.stl similarity index 100% rename from rx1_description/meshes/base_link.stl rename to src/rx1/rx1_description/meshes/base_link.stl diff --git a/rx1_description/meshes/head/head_base_link.stl b/src/rx1/rx1_description/meshes/head/head_base_link.stl similarity index 100% rename from rx1_description/meshes/head/head_base_link.stl rename to src/rx1/rx1_description/meshes/head/head_base_link.stl diff --git a/rx1_description/meshes/head/head_depth_cam_mount_link.stl b/src/rx1/rx1_description/meshes/head/head_depth_cam_mount_link.stl similarity index 100% rename from rx1_description/meshes/head/head_depth_cam_mount_link.stl rename to src/rx1/rx1_description/meshes/head/head_depth_cam_mount_link.stl diff --git a/rx1_description/meshes/head/left_ear_link.stl b/src/rx1/rx1_description/meshes/head/left_ear_link.stl similarity index 100% rename from rx1_description/meshes/head/left_ear_link.stl rename to src/rx1/rx1_description/meshes/head/left_ear_link.stl diff --git a/rx1_description/meshes/head/neck_pitch_link.stl b/src/rx1/rx1_description/meshes/head/neck_pitch_link.stl similarity index 100% rename from rx1_description/meshes/head/neck_pitch_link.stl rename to src/rx1/rx1_description/meshes/head/neck_pitch_link.stl diff --git a/rx1_description/meshes/head/neck_yaw_link.stl b/src/rx1/rx1_description/meshes/head/neck_yaw_link.stl similarity index 100% rename from rx1_description/meshes/head/neck_yaw_link.stl rename to src/rx1/rx1_description/meshes/head/neck_yaw_link.stl diff --git a/rx1_description/meshes/head/right_ear_link.stl b/src/rx1/rx1_description/meshes/head/right_ear_link.stl similarity index 100% rename from rx1_description/meshes/head/right_ear_link.stl rename to src/rx1/rx1_description/meshes/head/right_ear_link.stl diff --git a/rx1_description/meshes/left_arm/left_elbow_link.stl b/src/rx1/rx1_description/meshes/left_arm/left_elbow_link.stl similarity index 100% rename from rx1_description/meshes/left_arm/left_elbow_link.stl rename to src/rx1/rx1_description/meshes/left_arm/left_elbow_link.stl diff --git a/rx1_description/meshes/left_arm/left_forearm_link.stl b/src/rx1/rx1_description/meshes/left_arm/left_forearm_link.stl similarity index 100% rename from rx1_description/meshes/left_arm/left_forearm_link.stl rename to src/rx1/rx1_description/meshes/left_arm/left_forearm_link.stl diff --git a/rx1_description/meshes/left_arm/left_hand_link.stl b/src/rx1/rx1_description/meshes/left_arm/left_hand_link.stl similarity index 100% rename from rx1_description/meshes/left_arm/left_hand_link.stl rename to src/rx1/rx1_description/meshes/left_arm/left_hand_link.stl diff --git a/rx1_description/meshes/left_arm/left_upper_arm_link.stl b/src/rx1/rx1_description/meshes/left_arm/left_upper_arm_link.stl similarity index 100% rename from rx1_description/meshes/left_arm/left_upper_arm_link.stl rename to src/rx1/rx1_description/meshes/left_arm/left_upper_arm_link.stl diff --git a/rx1_description/meshes/right_arm/right_elbow_link.stl b/src/rx1/rx1_description/meshes/right_arm/right_elbow_link.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_elbow_link.stl rename to src/rx1/rx1_description/meshes/right_arm/right_elbow_link.stl diff --git a/rx1_description/meshes/right_arm/right_forearm_link.stl b/src/rx1/rx1_description/meshes/right_arm/right_forearm_link.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_forearm_link.stl rename to src/rx1/rx1_description/meshes/right_arm/right_forearm_link.stl diff --git a/rx1_description/meshes/right_arm/right_forearm_pitch_link v2.stl b/src/rx1/rx1_description/meshes/right_arm/right_forearm_pitch_link v2.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_forearm_pitch_link v2.stl rename to src/rx1/rx1_description/meshes/right_arm/right_forearm_pitch_link v2.stl diff --git a/rx1_description/meshes/right_arm/right_forearm_roll_link.stl b/src/rx1/rx1_description/meshes/right_arm/right_forearm_roll_link.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_forearm_roll_link.stl rename to src/rx1/rx1_description/meshes/right_arm/right_forearm_roll_link.stl diff --git a/rx1_description/meshes/right_arm/right_forearm_yaw_link v3.stl b/src/rx1/rx1_description/meshes/right_arm/right_forearm_yaw_link v3.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_forearm_yaw_link v3.stl rename to src/rx1/rx1_description/meshes/right_arm/right_forearm_yaw_link v3.stl diff --git a/rx1_description/meshes/right_arm/right_hand_link.stl b/src/rx1/rx1_description/meshes/right_arm/right_hand_link.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_hand_link.stl rename to src/rx1/rx1_description/meshes/right_arm/right_hand_link.stl diff --git a/rx1_description/meshes/right_arm/right_shoul_base_link v2.stl b/src/rx1/rx1_description/meshes/right_arm/right_shoul_base_link v2.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_shoul_base_link v2.stl rename to src/rx1/rx1_description/meshes/right_arm/right_shoul_base_link v2.stl diff --git a/rx1_description/meshes/right_arm/right_shoul_link v2.stl b/src/rx1/rx1_description/meshes/right_arm/right_shoul_link v2.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_shoul_link v2.stl rename to src/rx1/rx1_description/meshes/right_arm/right_shoul_link v2.stl diff --git a/rx1_description/meshes/right_arm/right_upper_arm_link v4.stl b/src/rx1/rx1_description/meshes/right_arm/right_upper_arm_link v4.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_upper_arm_link v4.stl rename to src/rx1/rx1_description/meshes/right_arm/right_upper_arm_link v4.stl diff --git a/rx1_description/meshes/right_arm/right_upper_arm_link.stl b/src/rx1/rx1_description/meshes/right_arm/right_upper_arm_link.stl similarity index 100% rename from rx1_description/meshes/right_arm/right_upper_arm_link.stl rename to src/rx1/rx1_description/meshes/right_arm/right_upper_arm_link.stl diff --git a/rx1_description/meshes/torso_link.stl b/src/rx1/rx1_description/meshes/torso_link.stl similarity index 100% rename from rx1_description/meshes/torso_link.stl rename to src/rx1/rx1_description/meshes/torso_link.stl diff --git a/rx1_description/meshes/torso_yaw_link.stl b/src/rx1/rx1_description/meshes/torso_yaw_link.stl similarity index 100% rename from rx1_description/meshes/torso_yaw_link.stl rename to src/rx1/rx1_description/meshes/torso_yaw_link.stl diff --git a/src/rx1/rx1_description/package.xml b/src/rx1/rx1_description/package.xml new file mode 100644 index 0000000..402c619 --- /dev/null +++ b/src/rx1/rx1_description/package.xml @@ -0,0 +1,19 @@ + + + rx1_description + 1.2.5 + 3D models of the rx1 for simulation and visualization + MIT + lethic + lethic + ament_cmake + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz2 + xacro + + + ament_cmake + + diff --git a/rx1_description/rviz/urdf.rviz b/src/rx1/rx1_description/rviz/urdf.rviz similarity index 93% rename from rx1_description/rviz/urdf.rviz rename to src/rx1/rx1_description/rviz/urdf.rviz index bf476f5..4b1e3ec 100644 --- a/rx1_description/rviz/urdf.rviz +++ b/src/rx1/rx1_description/rviz/urdf.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: @@ -11,21 +11,21 @@ Panels: - /PointCloud21 Splitter Ratio: 0.5008431673049927 Tree Height: 140 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Name: Time SyncMode: 0 SyncSource: PointCloud2 @@ -38,7 +38,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 238; 238; 236 Enabled: true Line Style: @@ -55,11 +55,11 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 0.699999988079071 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false Enabled: true Links: - All Links Enabled: false + All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false @@ -240,8 +240,8 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF - Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false Filter (blacklist): "" Filter (whitelist): "" Frame Timeout: 15 @@ -383,7 +383,7 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Class: rviz/InteractiveMarkers + - Class: rviz_default_plugins/InteractiveMarkers Enable Transparency: true Enabled: true Name: InteractiveMarkers @@ -392,7 +392,7 @@ Visualization Manager: Show Visual Aids: false Update Topic: /end_effector_marker/update Value: true - - Class: rviz/Image + - Class: rviz_default_plugins/Image Enabled: false Image Topic: /tag_detections_image Max Value: 1 @@ -404,16 +404,16 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: false - - Class: rviz/Camera - Enabled: true + - Class: rviz_default_plugins/Camera + Enabled: false Image Rendering: background and overlay - Image Topic: /camera/color/image_raw + Image Topic: /rx1/rgbd_camera/image Name: Camera Overlay Alpha: 0.8500000238418579 Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false Visibility: Grid: true Image: true @@ -431,11 +431,11 @@ Visualization Manager: Value: true Axis: X Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 136; 138; 133 Color Transformer: AxisColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -446,11 +446,11 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /camera/depth_registered/points + Topic: /rx1/rgbd_camera/points Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false Enabled: true Global Options: Background Color: 186; 189; 182 @@ -459,26 +459,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 3.9285216331481934 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/rx1_description/scripts/calc_inertia.py b/src/rx1/rx1_description/scripts/calc_inertia.py similarity index 100% rename from rx1_description/scripts/calc_inertia.py rename to src/rx1/rx1_description/scripts/calc_inertia.py diff --git a/rx1_description/urdf/rx1.gazebo.urdf b/src/rx1/rx1_description/urdf/rx1.gazebo.urdf similarity index 100% rename from rx1_description/urdf/rx1.gazebo.urdf rename to src/rx1/rx1_description/urdf/rx1.gazebo.urdf diff --git a/src/rx1/rx1_description/urdf/rx1.harmonic.urdf.xacro b/src/rx1/rx1_description/urdf/rx1.harmonic.urdf.xacro new file mode 100644 index 0000000..0c6fe94 --- /dev/null +++ b/src/rx1/rx1_description/urdf/rx1.harmonic.urdf.xacro @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + 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 + 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 + + + + 0.0 + 0.0 + + + + + + ${controllers_file} + true + 0.5 + + + + + + true + 15 + true + ${rgbd_topic_root} + + 1.0471975512 + + 640 + 480 + R8G8B8 + + + 0.1 + 5.0 + + + + + diff --git a/rx1_description/urdf/rx1.urdf b/src/rx1/rx1_description/urdf/rx1.urdf similarity index 100% rename from rx1_description/urdf/rx1.urdf rename to src/rx1/rx1_description/urdf/rx1.urdf diff --git a/rx1_description/urdf/rx1.urdf.xacro b/src/rx1/rx1_description/urdf/rx1.urdf.xacro similarity index 99% rename from rx1_description/urdf/rx1.urdf.xacro rename to src/rx1/rx1_description/urdf/rx1.urdf.xacro index aa16f5c..46605f1 100644 --- a/rx1_description/urdf/rx1.urdf.xacro +++ b/src/rx1/rx1_description/urdf/rx1.urdf.xacro @@ -29,7 +29,8 @@ - + + diff --git a/src/rx1/rx1_description/urdf/rx1_optimized.harmonic.urdf.xacro b/src/rx1/rx1_description/urdf/rx1_optimized.harmonic.urdf.xacro new file mode 100644 index 0000000..c4b281a --- /dev/null +++ b/src/rx1/rx1_description/urdf/rx1_optimized.harmonic.urdf.xacro @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + 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 + 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 + + + + 0.0 + 0.0 + + + + + + ${controllers_file} + true + 0.5 + + + + + + true + 15 + true + ${rgbd_topic_root} + + 1.0471975512 + + 640 + 480 + R8G8B8 + + + 0.1 + 5.0 + + + + + diff --git a/src/rx1/rx1_description/urdf/rx1_optimized.urdf.xacro b/src/rx1/rx1_description/urdf/rx1_optimized.urdf.xacro new file mode 100644 index 0000000..377a923 --- /dev/null +++ b/src/rx1/rx1_description/urdf/rx1_optimized.urdf.xacro @@ -0,0 +1,680 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rx1_description/urdf/transmission.urdf b/src/rx1/rx1_description/urdf/transmission.urdf similarity index 100% rename from rx1_description/urdf/transmission.urdf rename to src/rx1/rx1_description/urdf/transmission.urdf diff --git a/src/rx1/rx1_gazebo/CMakeLists.txt b/src/rx1/rx1_gazebo/CMakeLists.txt new file mode 100644 index 0000000..a9dd3e8 --- /dev/null +++ b/src/rx1/rx1_gazebo/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(rx1_gazebo) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY config launch rviz scripts worlds + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/controller_example.py + scripts/controller_gui_example.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/rx1_gazebo/config/rx1_control.yaml b/src/rx1/rx1_gazebo/config/rx1_control.yaml similarity index 100% rename from rx1_gazebo/config/rx1_control.yaml rename to src/rx1/rx1_gazebo/config/rx1_control.yaml diff --git a/src/rx1/rx1_gazebo/config/rx1_harmonic_controllers.yaml b/src/rx1/rx1_gazebo/config/rx1_harmonic_controllers.yaml new file mode 100644 index 0000000..9da1752 --- /dev/null +++ b/src/rx1/rx1_gazebo/config/rx1_harmonic_controllers.yaml @@ -0,0 +1,42 @@ +controller_manager: + ros__parameters: + update_rate: 200 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + rx1_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +rx1_joint_trajectory_controller: + ros__parameters: + joints: + - base2torso_yaw_joint + - torso_yaw2pitch_joint + - torso_pitch2roll_joint + - head_base2neck_yaw_joint + - neck_yaw2pitch_joint + - neck_pitch2head_depth_cam_mount_joint + - right_shoul_base2shoul_joint + - right_shoul2shoul_rot_joint + - right_arm2armrot_joint + - right_armrot2elbow_joint + - right_forearm2forearmrot_joint + - right_forearmrot2forearm_pitch_joint + - right_forearm_pitch2forearm_roll_joint + - left_shoul_base2shoul_joint + - left_shoul2shoul_rot_joint + - left_arm2armrot_joint + - left_armrot2elbow_joint + - left_forearm2forearmrot_joint + - left_forearmrot2forearm_pitch_joint + - left_forearm_pitch2forearm_roll_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + allow_partial_joints_goal: true + constraints: + goal_time: 1.0 + stopped_velocity_tolerance: 0.02 diff --git a/rx1_gazebo/launch/rx1_gazebo.launch b/src/rx1/rx1_gazebo/launch/rx1_gazebo.launch similarity index 100% rename from rx1_gazebo/launch/rx1_gazebo.launch rename to src/rx1/rx1_gazebo/launch/rx1_gazebo.launch diff --git a/src/rx1/rx1_gazebo/launch/rx1_gazebo.launch.py b/src/rx1/rx1_gazebo/launch/rx1_gazebo.launch.py new file mode 100644 index 0000000..79cbf96 --- /dev/null +++ b/src/rx1/rx1_gazebo/launch/rx1_gazebo.launch.py @@ -0,0 +1,199 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, SetEnvironmentVariable, TimerAction +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + model = LaunchConfiguration("model") + world = LaunchConfiguration("world") + robot_name = LaunchConfiguration("robot_name") + robot_x = LaunchConfiguration("robot_x") + robot_y = LaunchConfiguration("robot_y") + robot_z = LaunchConfiguration("robot_z") + use_rviz = LaunchConfiguration("use_rviz") + + rx1_gazebo_share = get_package_share_directory("rx1_gazebo") + rx1_description_share = get_package_share_directory("rx1_description") + ros_gz_sim_share = get_package_share_directory("ros_gz_sim") + description_resource_root = os.path.dirname(rx1_description_share) + gazebo_resource_root = os.path.dirname(rx1_gazebo_share) + existing_resource_path = os.environ.get("GZ_SIM_RESOURCE_PATH", "") + resource_path_entries = [ + entry + for entry in [ + existing_resource_path, + description_resource_root, + gazebo_resource_root, + ] + if entry + ] + + controllers_file = os.path.join( + rx1_gazebo_share, "config", "rx1_harmonic_controllers.yaml" + ) + robot_description = Command( + [ + FindExecutable(name="xacro"), + " ", + "--verbosity", + " 0 ", + model, + " ", + "controllers_file:=", + controllers_file, + " ", + "rgbd_topic_root:=/rx1/rgbd_camera", + ] + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_share, "launch", "gz_sim.launch.py") + ), + launch_arguments={"gz_args": ["-r ", world]}.items(), + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + { + "use_sim_time": True, + "robot_description": robot_description, + } + ], + ) + + spawn_robot = Node( + package="ros_gz_sim", + executable="create", + output="screen", + parameters=[ + { + "name": robot_name, + "topic": "robot_description", + "x": robot_x, + "y": robot_y, + "z": robot_z, + } + ], + ) + + bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + output="screen", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", + "/rx1/rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image", + "/rx1/rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", + "/rx1/rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image", + "/rx1/rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked", + ], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + "--controller-manager-timeout", + "120", + "--switch-timeout", + "30.0", + ], + output="screen", + ) + + trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "rx1_joint_trajectory_controller", + "--controller-manager", + "/controller_manager", + "--controller-manager-timeout", + "120", + "--switch-timeout", + "30.0", + ], + output="screen", + ) + + launch_joint_state_broadcaster = RegisterEventHandler( + OnProcessExit( + target_action=spawn_robot, + # Gazebo may report entity creation before gz_ros2_control has fully + # brought up controller_manager services. A short delay avoids a flaky + # race where the spawner exits before /controller_manager is ready. + on_exit=[TimerAction(period=5.0, actions=[joint_state_broadcaster_spawner])], + ) + ) + + launch_trajectory_controller = RegisterEventHandler( + OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[trajectory_controller_spawner], + ) + ) + + rviz = Node( + package="rviz2", + executable="rviz2", + arguments=[ + "-d", + os.path.join(rx1_gazebo_share, "rviz", "gazebo.rviz"), + ], + parameters=[ + { + "use_sim_time": True, + "robot_description": robot_description, + } + ], + condition=IfCondition(use_rviz), + output="screen", + ) + + actions = [ + DeclareLaunchArgument( + "model", + default_value=os.path.join( + rx1_description_share, "urdf", "rx1_optimized.harmonic.urdf.xacro" + ), + ), + DeclareLaunchArgument( + "world", + default_value=os.path.join( + rx1_gazebo_share, "worlds", "rx1_table_world.sdf" + ), + ), + DeclareLaunchArgument("robot_name", default_value="rx1"), + DeclareLaunchArgument("robot_x", default_value="0.0"), + DeclareLaunchArgument("robot_y", default_value="0.0"), + DeclareLaunchArgument("robot_z", default_value="0.0"), + DeclareLaunchArgument("use_rviz", default_value="false"), + SetEnvironmentVariable( + name="GZ_SIM_RESOURCE_PATH", + value=":".join(resource_path_entries), + ), + gazebo, + robot_state_publisher, + bridge, + spawn_robot, + launch_joint_state_broadcaster, + launch_trajectory_controller, + rviz, + ] + + return LaunchDescription(actions) diff --git a/src/rx1/rx1_gazebo/launch/rx1_gazebo_rviz.launch.py b/src/rx1/rx1_gazebo/launch/rx1_gazebo_rviz.launch.py new file mode 100644 index 0000000..7faedce --- /dev/null +++ b/src/rx1/rx1_gazebo/launch/rx1_gazebo_rviz.launch.py @@ -0,0 +1,56 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + rx1_gazebo_share = get_package_share_directory("rx1_gazebo") + rx1_description_share = get_package_share_directory("rx1_description") + + model = LaunchConfiguration("model") + world = LaunchConfiguration("world") + robot_name = LaunchConfiguration("robot_name") + robot_x = LaunchConfiguration("robot_x") + robot_y = LaunchConfiguration("robot_y") + robot_z = LaunchConfiguration("robot_z") + + gazebo_with_rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(rx1_gazebo_share, "launch", "rx1_gazebo.launch.py") + ), + launch_arguments={ + "model": model, + "world": world, + "robot_name": robot_name, + "robot_x": robot_x, + "robot_y": robot_y, + "robot_z": robot_z, + "use_rviz": "true", + }.items(), + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "model", + default_value=os.path.join( + rx1_description_share, "urdf", "rx1_optimized.harmonic.urdf.xacro" + ), + ), + DeclareLaunchArgument( + "world", + default_value=os.path.join( + rx1_gazebo_share, "worlds", "rx1_table_world.sdf" + ), + ), + DeclareLaunchArgument("robot_name", default_value="rx1"), + DeclareLaunchArgument("robot_x", default_value="0.0"), + DeclareLaunchArgument("robot_y", default_value="0.0"), + DeclareLaunchArgument("robot_z", default_value="0.0"), + gazebo_with_rviz, + ] + ) diff --git a/src/rx1/rx1_gazebo/package.xml b/src/rx1/rx1_gazebo/package.xml new file mode 100644 index 0000000..74f8443 --- /dev/null +++ b/src/rx1/rx1_gazebo/package.xml @@ -0,0 +1,32 @@ + + + rx1_gazebo + 1.2.5 + rx1 humanoid gazebo simulation + MIT + lethic + lethic + ament_cmake + ament_index_python + controller_manager + gz_ros2_control + joint_state_broadcaster + joint_trajectory_controller + launch + launch_ros + robot_state_publisher + ros_gz_bridge + ros_gz_sim + rviz2 + ros2controlcli + rx1_description + rclpy + sensor_msgs + rosgraph_msgs + trajectory_msgs + xacro + + + ament_cmake + + diff --git a/src/rx1/rx1_gazebo/rviz/gazebo.rviz b/src/rx1/rx1_gazebo/rviz/gazebo.rviz new file mode 100644 index 0000000..ddd8194 --- /dev/null +++ b/src/rx1/rx1_gazebo/rviz/gazebo.rviz @@ -0,0 +1,120 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 600 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 0.30000001192092896 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.5 + Focal Point: + X: 0 + Y: 0 + Z: 0.9 + Pitch: 0.45 + Yaw: 0.8 +Window Geometry: + Height: 1000 + Width: 1600 diff --git a/src/rx1/rx1_gazebo/scripts/controller_example.py b/src/rx1/rx1_gazebo/scripts/controller_example.py new file mode 100644 index 0000000..75011e5 --- /dev/null +++ b/src/rx1/rx1_gazebo/scripts/controller_example.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 + +from rclpy.duration import Duration +import rclpy +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + + +class ArmTorsoHeadController(Node): + def __init__(self) -> None: + super().__init__("move_arms_torso_head_to_zero") + + self.right_arm_pub = self.create_publisher( + JointTrajectory, "/right_arm_position_controller/joint_trajectory", 10 + ) + self.left_arm_pub = self.create_publisher( + JointTrajectory, "/left_arm_position_controller/joint_trajectory", 10 + ) + self.torso_pub = self.create_publisher( + JointTrajectory, "/torso_position_controller/joint_trajectory", 10 + ) + self.head_pub = self.create_publisher( + JointTrajectory, "/head_position_controller/joint_trajectory", 10 + ) + + self.right_arm_joints = [ + "right_shoul_base2shoul_joint", + "right_shoul2shoul_rot_joint", + "right_arm2armrot_joint", + "right_armrot2elbow_joint", + "right_forearm2forearmrot_joint", + "right_forearmrot2forearm_pitch_joint", + "right_forearm_pitch2forearm_roll_joint", + ] + self.left_arm_joints = [ + "left_shoul_base2shoul_joint", + "left_shoul2shoul_rot_joint", + "left_arm2armrot_joint", + "left_armrot2elbow_joint", + "left_forearm2forearmrot_joint", + "left_forearmrot2forearm_pitch_joint", + "left_forearm_pitch2forearm_roll_joint", + ] + self.torso_joints = [ + "base2torso_yaw_joint", + "torso_yaw2pitch_joint", + "torso_pitch2roll_joint", + ] + self.head_joints = [ + "head_base2neck_yaw_joint", + "neck_yaw2pitch_joint", + "neck_pitch2head_depth_cam_mount_joint", + ] + + self.timer = self.create_timer(5.0, self.move_all_to_zero) + + def create_trajectory(self, joint_names, pos): + traj = JointTrajectory() + traj.joint_names = joint_names + + point = JointTrajectoryPoint() + point.positions = [pos] * len(joint_names) + point.time_from_start = Duration(seconds=2.0).to_msg() + traj.points.append(point) + return traj + + def move_all_to_zero(self): + self.right_arm_pub.publish(self.create_trajectory(self.right_arm_joints, 0.0)) + self.left_arm_pub.publish(self.create_trajectory(self.left_arm_joints, 0.0)) + self.torso_pub.publish(self.create_trajectory(self.torso_joints, 0.0)) + self.head_pub.publish(self.create_trajectory(self.head_joints, 0.0)) + self.get_logger().info("Sent zero-position trajectories.") + + +def main(): + rclpy.init() + node = ArmTorsoHeadController() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/rx1/rx1_gazebo/scripts/controller_gui_example.py b/src/rx1/rx1_gazebo/scripts/controller_gui_example.py new file mode 100644 index 0000000..c95b91c --- /dev/null +++ b/src/rx1/rx1_gazebo/scripts/controller_gui_example.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +import tkinter as tk + +from rclpy.duration import Duration +import rclpy +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + + +class ArmTorsoHeadController(Node): + def __init__(self) -> None: + super().__init__("move_arms_torso_head_with_gui") + + self.right_arm_pub = self.create_publisher( + JointTrajectory, "/right_arm_position_controller/joint_trajectory", 10 + ) + self.left_arm_pub = self.create_publisher( + JointTrajectory, "/left_arm_position_controller/joint_trajectory", 10 + ) + self.torso_pub = self.create_publisher( + JointTrajectory, "/torso_position_controller/joint_trajectory", 10 + ) + self.head_pub = self.create_publisher( + JointTrajectory, "/head_position_controller/joint_trajectory", 10 + ) + + self.right_arm_joints = [ + "right_shoul_base2shoul_joint", + "right_shoul2shoul_rot_joint", + "right_arm2armrot_joint", + "right_armrot2elbow_joint", + "right_forearm2forearmrot_joint", + "right_forearmrot2forearm_pitch_joint", + "right_forearm_pitch2forearm_roll_joint", + ] + self.left_arm_joints = [ + "left_shoul_base2shoul_joint", + "left_shoul2shoul_rot_joint", + "left_arm2armrot_joint", + "left_armrot2elbow_joint", + "left_forearm2forearmrot_joint", + "left_forearmrot2forearm_pitch_joint", + "left_forearm_pitch2forearm_roll_joint", + ] + self.torso_joints = [ + "base2torso_yaw_joint", + "torso_yaw2pitch_joint", + "torso_pitch2roll_joint", + ] + self.head_joints = [ + "head_base2neck_yaw_joint", + "neck_yaw2pitch_joint", + "neck_pitch2head_depth_cam_mount_joint", + ] + + self.joint_positions = { + joint: 0.0 + for joint in self.right_arm_joints + + self.left_arm_joints + + self.torso_joints + + self.head_joints + } + + self.init_gui() + + def init_gui(self): + self.root = tk.Tk() + self.root.title("Joint Angle Controller") + + row = 0 + for joint_name in self.joint_positions: + label = tk.Label(self.root, text=joint_name) + label.grid(row=row, column=0) + slider = tk.Scale( + self.root, + from_=-3.14, + to=3.14, + resolution=0.01, + orient=tk.HORIZONTAL, + length=300, + command=lambda val, j=joint_name: self.update_joint_position(j, val), + ) + slider.grid(row=row, column=1) + row += 1 + + send_button = tk.Button( + self.root, text="Send Joint Commands", command=self.send_joint_commands + ) + send_button.grid(row=row, column=0, columnspan=2) + + self.root.after(100, self.spin_ros_once) + self.root.mainloop() + + def spin_ros_once(self): + rclpy.spin_once(self, timeout_sec=0.0) + self.root.after(100, self.spin_ros_once) + + def update_joint_position(self, joint_name, value): + self.joint_positions[joint_name] = float(value) + + def create_trajectory(self, joint_names): + traj = JointTrajectory() + traj.joint_names = joint_names + + point = JointTrajectoryPoint() + point.positions = [self.joint_positions[joint] for joint in joint_names] + point.time_from_start = Duration(seconds=1.0).to_msg() + traj.points.append(point) + return traj + + def send_joint_commands(self): + self.right_arm_pub.publish(self.create_trajectory(self.right_arm_joints)) + self.left_arm_pub.publish(self.create_trajectory(self.left_arm_joints)) + self.torso_pub.publish(self.create_trajectory(self.torso_joints)) + self.head_pub.publish(self.create_trajectory(self.head_joints)) + self.get_logger().info("Joint commands sent based on GUI sliders.") + + +def main(): + rclpy.init() + node = ArmTorsoHeadController() + try: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/rx1_gazebo/worlds/empty.world b/src/rx1/rx1_gazebo/worlds/empty.world similarity index 100% rename from rx1_gazebo/worlds/empty.world rename to src/rx1/rx1_gazebo/worlds/empty.world diff --git a/src/rx1/rx1_gazebo/worlds/rx1_table_world.sdf b/src/rx1/rx1_gazebo/worlds/rx1_table_world.sdf new file mode 100644 index 0000000..e35df28 --- /dev/null +++ b/src/rx1/rx1_gazebo/worlds/rx1_table_world.sdf @@ -0,0 +1,120 @@ + + + + 0 0 -9.8 + + + + + + ogre2 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.7 0.7 0.7 1 + 0.7 0.7 0.7 1 + + + + + + + true + 0.8 0.0 0.0 0 0 0 + + + 0 0 0.755 0 0 0 + + + 0.913 0.913 0.04 + + + + + 0 0 0.37 0 0 0 + + + 0.05 0.05 0.74 + + + + + 0 0 0.02 0 0 0 + + + 0.56 0.56 0.04 + + + + + 0 0 0.755 0 0 0 + + + 0.913 0.913 0.04 + + + + 0.45 0.30 0.20 1 + 0.45 0.30 0.20 1 + + + + 0 0 0.37 0 0 0 + + + 0.05 0.05 0.74 + + + + 0.15 0.15 0.15 1 + 0.15 0.15 0.15 1 + + + + 0 0 0.02 0 0 0 + + + 0.56 0.56 0.04 + + + + 0.15 0.15 0.15 1 + 0.15 0.15 0.15 1 + + + + + + diff --git a/rx1_gazebo/worlds/table_world.world b/src/rx1/rx1_gazebo/worlds/table_world.world similarity index 100% rename from rx1_gazebo/worlds/table_world.world rename to src/rx1/rx1_gazebo/worlds/table_world.world diff --git a/src/rx1/rx1_ik/CMakeLists.txt b/src/rx1/rx1_ik/CMakeLists.txt new file mode 100644 index 0000000..e489b32 --- /dev/null +++ b/src/rx1/rx1_ik/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.8) +project(rx1_ik) + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_kdl REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(ik_solver_lib REQUIRED) + +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}/rx1_ik.cpp +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + geometry_msgs + interactive_markers + ik_solver_lib + kdl_parser + pluginlib + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_kdl + tf2_msgs + tf2_ros + visualization_msgs +) + +add_executable(rx1_ik_node src/rx1_ik_node.cpp) +target_compile_features(rx1_ik_node PUBLIC cxx_std_17) +target_link_libraries(rx1_ik_node ${PROJECT_NAME}) +ament_target_dependencies(rx1_ik_node rclcpp) + +install(TARGETS ${PROJECT_NAME} rx1_ik_node + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY config include launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + geometry_msgs + interactive_markers + ik_solver_lib + kdl_parser + pluginlib + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_kdl + tf2_msgs + tf2_ros + visualization_msgs +) +ament_package() diff --git a/src/rx1/rx1_ik/config/ik.ros2.yaml b/src/rx1/rx1_ik/config/ik.ros2.yaml new file mode 100644 index 0000000..f6f92a7 --- /dev/null +++ b/src/rx1/rx1_ik/config/ik.ros2.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + chain_r_end: "right_forearm_roll_link" + chain_l_end: "left_forearm_roll_link" + eps: 0.02 + ik_timeout: 0.01 + max_angle_change: 0.6 + tracking_timeout: 1.0 diff --git a/rx1_ik/config/ik.yaml b/src/rx1/rx1_ik/config/ik.yaml similarity index 100% rename from rx1_ik/config/ik.yaml rename to src/rx1/rx1_ik/config/ik.yaml diff --git a/src/rx1/rx1_ik/config/ik_marker.ros2.yaml b/src/rx1/rx1_ik/config/ik_marker.ros2.yaml new file mode 100644 index 0000000..3ae0c07 --- /dev/null +++ b/src/rx1/rx1_ik/config/ik_marker.ros2.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + chain_r_end: "right_forearm_roll_link" + chain_l_end: "left_forearm_roll_link" + eps: 0.01 + ik_timeout: 0.05 + max_angle_change: 0.6 + tracking_timeout: 1.0 diff --git a/rx1_ik/config/ik_marker.yaml b/src/rx1/rx1_ik/config/ik_marker.yaml similarity index 100% rename from rx1_ik/config/ik_marker.yaml rename to src/rx1/rx1_ik/config/ik_marker.yaml diff --git a/src/rx1/rx1_ik/config/ik_traj.ros2.yaml b/src/rx1/rx1_ik/config/ik_traj.ros2.yaml new file mode 100644 index 0000000..963505a --- /dev/null +++ b/src/rx1/rx1_ik/config/ik_traj.ros2.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + chain_r_end: "right_forearm_roll_link" + chain_l_end: "left_forearm_roll_link" + eps: 0.01 + ik_timeout: 0.05 + max_angle_change: 2.0 + tracking_timeout: 1.0 diff --git a/rx1_ik/config/ik_traj.yaml b/src/rx1/rx1_ik/config/ik_traj.yaml similarity index 100% rename from rx1_ik/config/ik_traj.yaml rename to src/rx1/rx1_ik/config/ik_traj.yaml diff --git a/src/rx1/rx1_ik/include/rx1_ik/rx1_ik.h b/src/rx1/rx1_ik/include/rx1_ik/rx1_ik.h new file mode 100644 index 0000000..d7830ca --- /dev/null +++ b/src/rx1/rx1_ik/include/rx1_ik/rx1_ik.h @@ -0,0 +1,84 @@ +#ifndef RX1_IK_H +#define RX1_IK_H + +#include "ik_solver_lib/base/ik_solver_base.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +class Rx1Ik : public rclcpp::Node +{ +public: + Rx1Ik(); + + void initializeInteractiveMarker(); + void spin(); + void update(); + +private: + using InteractiveMarkerFeedback = visualization_msgs::msg::InteractiveMarkerFeedback; + using JointState = sensor_msgs::msg::JointState; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using TransformStamped = geometry_msgs::msg::TransformStamped; + + void markerRightCallback(const InteractiveMarkerFeedback::ConstSharedPtr feedback); + void markerLeftCallback(const InteractiveMarkerFeedback::ConstSharedPtr feedback); + void rightGripperPoseCallback(const Pose::SharedPtr msg); + void leftGripperPoseCallback(const Pose::SharedPtr msg); + void make6DofMarker(visualization_msgs::msg::InteractiveMarker & int_marker); + bool getLinkPose(const std::string & frame, const std::string & link, PoseStamped & pose); + bool getPoseInNewFrame( + const PoseStamped & old_pose, const std::string & new_frame, PoseStamped & new_pose); + TransformStamped poseToTransformStamped( + const Pose & pose, const std::string & frame_id, const std::string & child_frame_id); + + std::unique_ptr marker_server_; + visualization_msgs::msg::InteractiveMarker int_marker_r_; + visualization_msgs::msg::InteractiveMarker int_marker_l_; + + rclcpp::Subscription::SharedPtr right_gripper_pose_sub_; + rclcpp::Subscription::SharedPtr left_gripper_pose_sub_; + rclcpp::Publisher::SharedPtr right_joint_state_pub_; + rclcpp::Publisher::SharedPtr left_joint_state_pub_; + + std::unique_ptr> ik_loader_r_ptr_; + std::shared_ptr ik_solver_r_ptr_; + std::unique_ptr> ik_loader_l_ptr_; + std::shared_ptr ik_solver_l_ptr_; + + JointState right_prev_joint_state_msg_; + JointState left_prev_joint_state_msg_; + JointState right_cur_joint_state_msg_; + JointState left_cur_joint_state_msg_; + + TransformStamped world_to_base_tf_stamped_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + tf2_ros::TransformBroadcaster tf_br_; + + std::string chain_start_; + std::string chain_r_end_; + std::string chain_l_end_; + std::string urdf_param_; + double max_angle_change_; + double tracking_timeout_; + double right_last_ik_time_; + double left_last_ik_time_; +}; + +#endif // RX1_IK_H diff --git a/rx1_ik/launch/rx1_ik_apriltag.launch b/src/rx1/rx1_ik/launch/rx1_ik_apriltag.launch similarity index 100% rename from rx1_ik/launch/rx1_ik_apriltag.launch rename to src/rx1/rx1_ik/launch/rx1_ik_apriltag.launch diff --git a/src/rx1/rx1_ik/launch/rx1_ik_apriltag.launch.py b/src/rx1/rx1_ik/launch/rx1_ik_apriltag.launch.py new file mode 100644 index 0000000..3b4ff43 --- /dev/null +++ b/src/rx1/rx1_ik/launch/rx1_ik_apriltag.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + params_file = LaunchConfiguration("params_file") + + return LaunchDescription([ + DeclareLaunchArgument( + "params_file", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_ik"), "config", "ik.ros2.yaml"] + ), + ), + Node( + package="rx1_ik", + executable="rx1_ik_node", + name="rx1_ik", + output="screen", + parameters=[params_file], + ), + ]) diff --git a/rx1_ik/launch/rx1_ik_marker.launch b/src/rx1/rx1_ik/launch/rx1_ik_marker.launch similarity index 100% rename from rx1_ik/launch/rx1_ik_marker.launch rename to src/rx1/rx1_ik/launch/rx1_ik_marker.launch diff --git a/src/rx1/rx1_ik/launch/rx1_ik_marker.launch.py b/src/rx1/rx1_ik/launch/rx1_ik_marker.launch.py new file mode 100644 index 0000000..6de7069 --- /dev/null +++ b/src/rx1/rx1_ik/launch/rx1_ik_marker.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + params_file = LaunchConfiguration("params_file") + + return LaunchDescription([ + DeclareLaunchArgument( + "params_file", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_ik"), "config", "ik_marker.ros2.yaml"] + ), + ), + Node( + package="rx1_ik", + executable="rx1_ik_node", + name="rx1_ik", + output="screen", + parameters=[params_file], + ), + ]) diff --git a/rx1_ik/launch/rx1_ik_traj.launch b/src/rx1/rx1_ik/launch/rx1_ik_traj.launch similarity index 100% rename from rx1_ik/launch/rx1_ik_traj.launch rename to src/rx1/rx1_ik/launch/rx1_ik_traj.launch diff --git a/src/rx1/rx1_ik/launch/rx1_ik_traj.launch.py b/src/rx1/rx1_ik/launch/rx1_ik_traj.launch.py new file mode 100644 index 0000000..756c956 --- /dev/null +++ b/src/rx1/rx1_ik/launch/rx1_ik_traj.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + params_file = LaunchConfiguration("params_file") + + return LaunchDescription([ + DeclareLaunchArgument( + "params_file", + default_value=PathJoinSubstitution( + [FindPackageShare("rx1_ik"), "config", "ik_traj.ros2.yaml"] + ), + ), + Node( + package="rx1_ik", + executable="rx1_ik_node", + name="rx1_ik", + output="screen", + parameters=[params_file], + ), + ]) diff --git a/src/rx1/rx1_ik/package.xml b/src/rx1/rx1_ik/package.xml new file mode 100644 index 0000000..ffce8bf --- /dev/null +++ b/src/rx1/rx1_ik/package.xml @@ -0,0 +1,28 @@ + + + rx1_ik + 1.2.5 + Inverse Kinematics of the RX1 robot + MIT + lethic + lethic + ament_cmake + geometry_msgs + ik_solver_lib + interactive_markers + kdl_parser + pluginlib + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_kdl + tf2_msgs + tf2_ros + visualization_msgs + + + ament_cmake + + diff --git a/src/rx1/rx1_ik/src/rx1_ik/rx1_ik.cpp b/src/rx1/rx1_ik/src/rx1_ik/rx1_ik.cpp new file mode 100644 index 0000000..87dc7c7 --- /dev/null +++ b/src/rx1/rx1_ik/src/rx1_ik/rx1_ik.cpp @@ -0,0 +1,368 @@ +#include "rx1_ik/rx1_ik.h" + +#include +#include + +#include +#include +#include + +using std::placeholders::_1; + +Rx1Ik::Rx1Ik() +: rclcpp::Node("rx1_ik"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_, this, false), + tf_br_(this) +{ + marker_server_ = std::make_unique( + "end_effector_marker", this); + + const double ik_timeout = this->declare_parameter("ik_timeout", 0.005); + const double eps = this->declare_parameter("eps", 1e-3); + chain_start_ = this->declare_parameter("chain_start", "head_base_link"); + chain_r_end_ = this->declare_parameter("chain_r_end", "right_hand_link"); + chain_l_end_ = this->declare_parameter("chain_l_end", "left_hand_link"); + urdf_param_ = this->declare_parameter("urdf_param", "robot_description"); + max_angle_change_ = this->declare_parameter("max_angle_change", 0.3); + tracking_timeout_ = this->declare_parameter("tracking_timeout", 1.0); + + try { + ik_loader_r_ptr_ = std::make_unique>( + "ik_solver_lib", "ik_solver_plugin::IKSolverBase"); + ik_solver_r_ptr_ = ik_loader_r_ptr_->createSharedInstance("ik_solver_plugin::TracIKSolver"); + ik_solver_r_ptr_->initialize(chain_start_, chain_r_end_, urdf_param_, ik_timeout, eps); + right_last_ik_time_ = this->now().seconds() - tracking_timeout_; + + ik_loader_l_ptr_ = std::make_unique>( + "ik_solver_lib", "ik_solver_plugin::IKSolverBase"); + ik_solver_l_ptr_ = ik_loader_l_ptr_->createSharedInstance("ik_solver_plugin::TracIKSolver"); + ik_solver_l_ptr_->initialize(chain_start_, chain_l_end_, urdf_param_, ik_timeout, eps); + left_last_ik_time_ = this->now().seconds() - tracking_timeout_; + + RCLCPP_INFO(get_logger(), "TracIKSolver plugin loaded and initialized successfully."); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_ERROR(get_logger(), "Failed to create the TracIKSolver plugin. Error: %s", ex.what()); + throw; + } + + left_joint_state_pub_ = this->create_publisher("left_arm_joint_states", 10); + right_joint_state_pub_ = this->create_publisher("right_arm_joint_states", 10); + + right_gripper_pose_sub_ = this->create_subscription( + "right_gripper_pose", 10, std::bind(&Rx1Ik::rightGripperPoseCallback, this, _1)); + left_gripper_pose_sub_ = this->create_subscription( + "left_gripper_pose", 10, std::bind(&Rx1Ik::leftGripperPoseCallback, this, _1)); + + JointState right_joint_state_msg; + right_joint_state_msg.header.stamp = this->now(); + right_joint_state_msg.name = { + "right_shoul_base2shoul_joint", + "right_shoul2shoul_rot_joint", + "right_arm2armrot_joint", + "right_armrot2elbow_joint", + "right_forearm2forearmrot_joint", + "right_forearmrot2forearm_pitch_joint", + "right_forearm_pitch2forearm_roll_joint"}; + right_joint_state_msg.position = {0.0, 0.0, 0.0, -1.57, 0.0, 0.0, 0.0}; + right_joint_state_pub_->publish(right_joint_state_msg); + right_prev_joint_state_msg_ = right_joint_state_msg; + right_cur_joint_state_msg_ = right_joint_state_msg; + + JointState left_joint_state_msg; + left_joint_state_msg.header.stamp = this->now(); + left_joint_state_msg.name = { + "left_shoul_base2shoul_joint", + "left_shoul2shoul_rot_joint", + "left_arm2armrot_joint", + "left_armrot2elbow_joint", + "left_forearm2forearmrot_joint", + "left_forearmrot2forearm_pitch_joint", + "left_forearm_pitch2forearm_roll_joint"}; + left_joint_state_msg.position = {0.0, 0.0, 0.0, -1.57, 0.0, 0.0, 0.0}; + left_joint_state_pub_->publish(left_joint_state_msg); + left_prev_joint_state_msg_ = left_joint_state_msg; + left_cur_joint_state_msg_ = left_joint_state_msg; + + world_to_base_tf_stamped_.header.stamp = this->now(); + world_to_base_tf_stamped_.header.frame_id = "map"; + world_to_base_tf_stamped_.child_frame_id = "base_link"; + world_to_base_tf_stamped_.transform.rotation.w = 1.0; + tf_br_.sendTransform(world_to_base_tf_stamped_); + + initializeInteractiveMarker(); +} + +void Rx1Ik::make6DofMarker(visualization_msgs::msg::InteractiveMarker & int_marker) +{ + int_marker.scale = 0.15; + visualization_msgs::msg::InteractiveMarkerControl control; + + control.orientation.w = 1.0; + control.orientation.x = 1.0; + control.orientation.y = 0.0; + control.orientation.z = 0.0; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control.orientation.x = 0.0; + control.orientation.y = 1.0; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control.orientation.y = 0.0; + control.orientation.z = 1.0; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control.orientation.x = 1.0; + control.orientation.z = 0.0; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + + control.orientation.x = 0.0; + control.orientation.y = 1.0; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + + control.orientation.y = 0.0; + control.orientation.z = 1.0; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); +} + +void Rx1Ik::initializeInteractiveMarker() +{ + int_marker_r_.header.frame_id = chain_start_; + int_marker_r_.name = "right_end_effector"; + int_marker_r_.description = "Right End Effector Control"; + + int_marker_l_.header.frame_id = chain_start_; + int_marker_l_.name = "left_end_effector"; + int_marker_l_.description = "Left End Effector Control"; + + make6DofMarker(int_marker_r_); + make6DofMarker(int_marker_l_); + + marker_server_->insert(int_marker_r_, std::bind(&Rx1Ik::markerRightCallback, this, _1)); + marker_server_->insert(int_marker_l_, std::bind(&Rx1Ik::markerLeftCallback, this, _1)); + marker_server_->applyChanges(); +} + +void Rx1Ik::markerRightCallback(const InteractiveMarkerFeedback::ConstSharedPtr feedback) +{ + if (feedback->event_type != InteractiveMarkerFeedback::POSE_UPDATE) { + return; + } + + KDL::Frame desired_pose; + tf2::fromMsg(feedback->pose, desired_pose); + + KDL::JntArray result_joint_positions; + if (!ik_solver_r_ptr_->solveIK(desired_pose, result_joint_positions)) { + RCLCPP_WARN(get_logger(), "Failed to find right arm IK solution"); + return; + } + + bool success = true; + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + if (std::abs(right_prev_joint_state_msg_.position[i] - result_joint_positions(i)) > max_angle_change_) { + success = false; + } + } + + if (success) { + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + right_prev_joint_state_msg_.position[i] = + right_prev_joint_state_msg_.position[i] * 0.9 + result_joint_positions(i) * 0.1; + } + RCLCPP_INFO(get_logger(), "Succeed finding right arm IK solution"); + } else { + RCLCPP_INFO(get_logger(), "Succeed finding right arm IK solution but ditch the result to reduce shake"); + } +} + +void Rx1Ik::markerLeftCallback(const InteractiveMarkerFeedback::ConstSharedPtr feedback) +{ + if (feedback->event_type != InteractiveMarkerFeedback::POSE_UPDATE) { + return; + } + + KDL::Frame desired_pose; + tf2::fromMsg(feedback->pose, desired_pose); + + KDL::JntArray result_joint_positions; + if (!ik_solver_l_ptr_->solveIK(desired_pose, result_joint_positions)) { + RCLCPP_WARN(get_logger(), "Failed to find left arm IK solution"); + return; + } + + bool success = true; + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + if (std::abs(left_prev_joint_state_msg_.position[i] - result_joint_positions(i)) > max_angle_change_) { + success = false; + } + } + + if (success) { + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + left_prev_joint_state_msg_.position[i] = + left_prev_joint_state_msg_.position[i] * 0.9 + result_joint_positions(i) * 0.1; + } + RCLCPP_INFO(get_logger(), "Succeed finding left arm IK solution"); + } else { + RCLCPP_INFO(get_logger(), "Succeed finding left arm IK solution but ditch the result to reduce shake"); + } +} + +void Rx1Ik::rightGripperPoseCallback(const Pose::SharedPtr msg) +{ + KDL::Frame desired_pose; + tf2::fromMsg(*msg, desired_pose); + + KDL::JntArray result_joint_positions; + const double before_ik_time = this->now().seconds(); + const bool ik_solved = ik_solver_r_ptr_->solveIK(desired_pose, result_joint_positions); + RCLCPP_INFO(get_logger(), "right ik took %f sec", this->now().seconds() - before_ik_time); + + if (!ik_solved) { + RCLCPP_WARN(get_logger(), "Failed to find right IK solution"); + return; + } + + bool success = true; + if ((this->now().seconds() - right_last_ik_time_) < tracking_timeout_) { + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + if (std::abs(right_prev_joint_state_msg_.position[i] - result_joint_positions(i)) > max_angle_change_) { + success = false; + } + } + } + + if (success) { + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + right_prev_joint_state_msg_.position[i] = result_joint_positions(i); + } + right_last_ik_time_ = this->now().seconds(); + RCLCPP_INFO(get_logger(), "Succeed finding right IK solution"); + } else { + RCLCPP_INFO(get_logger(), "Succeed finding right IK solution but ditch the result to reduce shake"); + } +} + +void Rx1Ik::leftGripperPoseCallback(const Pose::SharedPtr msg) +{ + KDL::Frame desired_pose; + tf2::fromMsg(*msg, desired_pose); + + KDL::JntArray result_joint_positions; + if (!ik_solver_l_ptr_->solveIK(desired_pose, result_joint_positions)) { + RCLCPP_WARN(get_logger(), "Failed to find left IK solution"); + return; + } + + bool success = true; + if ((this->now().seconds() - left_last_ik_time_) < tracking_timeout_) { + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + if (std::abs(left_prev_joint_state_msg_.position[i] - result_joint_positions(i)) > max_angle_change_) { + success = false; + } + } + } + + if (success) { + for (unsigned int i = 0; i < result_joint_positions.rows(); ++i) { + left_prev_joint_state_msg_.position[i] = result_joint_positions(i); + } + left_last_ik_time_ = this->now().seconds(); + RCLCPP_INFO(get_logger(), "Succeed finding left IK solution"); + } else { + RCLCPP_INFO(get_logger(), "Succeed finding left IK solution but ditch the result to reduce shake"); + } +} + +void Rx1Ik::update() +{ + for (size_t i = 0; i < right_cur_joint_state_msg_.position.size(); ++i) { + right_cur_joint_state_msg_.position[i] = + right_cur_joint_state_msg_.position[i] * 0.9 + right_prev_joint_state_msg_.position[i] * 0.1; + } + right_cur_joint_state_msg_.header.stamp = this->now(); + right_joint_state_pub_->publish(right_cur_joint_state_msg_); + + for (size_t i = 0; i < left_cur_joint_state_msg_.position.size(); ++i) { + left_cur_joint_state_msg_.position[i] = + left_cur_joint_state_msg_.position[i] * 0.9 + left_prev_joint_state_msg_.position[i] * 0.1; + } + left_cur_joint_state_msg_.header.stamp = this->now(); + left_joint_state_pub_->publish(left_cur_joint_state_msg_); + + world_to_base_tf_stamped_.header.stamp = this->now(); + tf_br_.sendTransform(world_to_base_tf_stamped_); + + PoseStamped pose; + if (getLinkPose(chain_start_, chain_r_end_, pose)) { + int_marker_r_.pose = pose.pose; + marker_server_->insert(int_marker_r_); + marker_server_->applyChanges(); + } + if (getLinkPose(chain_start_, chain_l_end_, pose)) { + int_marker_l_.pose = pose.pose; + marker_server_->insert(int_marker_l_); + marker_server_->applyChanges(); + } +} + +void Rx1Ik::spin() +{ + rclcpp::WallRate rate(20.0); + while (rclcpp::ok()) { + rclcpp::spin_some(shared_from_this()); + update(); + rate.sleep(); + } +} + +bool Rx1Ik::getLinkPose(const std::string & frame, const std::string & link, PoseStamped & pose) +{ + try { + const auto transform_stamped = tf_buffer_.lookupTransform( + frame, link, rclcpp::Time(0, 0, get_clock()->get_clock_type()), rclcpp::Duration::from_seconds(3.0)); + pose.header.stamp = this->now(); + pose.header.frame_id = frame; + pose.pose.position.x = transform_stamped.transform.translation.x; + pose.pose.position.y = transform_stamped.transform.translation.y; + pose.pose.position.z = transform_stamped.transform.translation.z; + pose.pose.orientation = transform_stamped.transform.rotation; + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + return false; + } + return true; +} + +bool Rx1Ik::getPoseInNewFrame( + const PoseStamped & old_pose, const std::string & new_frame, PoseStamped & new_pose) +{ + try { + new_pose = tf_buffer_.transform(old_pose, new_frame, rclcpp::Duration::from_seconds(1.0)); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "getPoseInNewFrame: %s", ex.what()); + return false; + } + return true; +} + +Rx1Ik::TransformStamped Rx1Ik::poseToTransformStamped( + const Pose & pose, const std::string & frame_id, const std::string & child_frame_id) +{ + TransformStamped transform_stamped; + transform_stamped.header.stamp = this->now(); + transform_stamped.header.frame_id = frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform.translation.x = pose.position.x; + transform_stamped.transform.translation.y = pose.position.y; + transform_stamped.transform.translation.z = pose.position.z; + transform_stamped.transform.rotation = pose.orientation; + return transform_stamped; +} diff --git a/src/rx1/rx1_ik/src/rx1_ik_node.cpp b/src/rx1/rx1_ik/src/rx1_ik_node.cpp new file mode 100644 index 0000000..9fc9797 --- /dev/null +++ b/src/rx1/rx1_ik/src/rx1_ik_node.cpp @@ -0,0 +1,18 @@ +#include "rx1_ik/rx1_ik.h" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + try { + node->spin(); + } catch (const std::runtime_error & ex) { + RCLCPP_FATAL(node->get_logger(), "[RX1_IK] Runtime error: %s", ex.what()); + rclcpp::shutdown(); + return 1; + } + + rclcpp::shutdown(); + return 0; +} diff --git a/src/rx1/rx1_motor/CMakeLists.txt b/src/rx1/rx1_motor/CMakeLists.txt new file mode 100644 index 0000000..f883c54 --- /dev/null +++ b/src/rx1/rx1_motor/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.8) +project(rx1_motor) + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(feetech_lib REQUIRED) + +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}/rx1_motor.cpp +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} + feetech_lib + rclcpp + sensor_msgs + std_msgs +) + +add_executable(rx1_motor_node src/rx1_motor_node.cpp) +target_compile_features(rx1_motor_node PUBLIC cxx_std_17) +target_link_libraries(rx1_motor_node ${PROJECT_NAME}) +ament_target_dependencies(rx1_motor_node + rclcpp +) + +install(TARGETS ${PROJECT_NAME} rx1_motor_node + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + feetech_lib + rclcpp + sensor_msgs + std_msgs +) +ament_package() diff --git a/src/rx1/rx1_motor/include/rx1_motor/rx1_motor.hpp b/src/rx1/rx1_motor/include/rx1_motor/rx1_motor.hpp new file mode 100644 index 0000000..0b57d65 --- /dev/null +++ b/src/rx1/rx1_motor/include/rx1_motor/rx1_motor.hpp @@ -0,0 +1,114 @@ +#ifndef RX1_MOTOR_H +#define RX1_MOTOR_H + +#include "feetech_lib/SCSCL.h" +#include "feetech_lib/SMS_STS.h" + +#include +#include +#include + +#include +#include +#include + +namespace rx1_motor +{ + +class Rx1Motor : public rclcpp::Node +{ +public: + Rx1Motor(); + ~Rx1Motor() override; + + void spin(); + void update(); + +private: + using JointState = sensor_msgs::msg::JointState; + using Float32 = std_msgs::msg::Float32; + + void jointStateCallback(const JointState::SharedPtr msg); + void rightArmJointStateCallback(const JointState::SharedPtr msg); + void leftArmJointStateCallback(const JointState::SharedPtr msg); + void torsoJointStateCallback(const JointState::SharedPtr msg); + void headJointStateCallback(const JointState::SharedPtr msg); + void leftGripperCallback(const Float32::SharedPtr msg); + void rightGripperCallback(const Float32::SharedPtr msg); + + std::array torsoIk( + double d, double l1, double h1, double h2, double roll, double pitch); + + void headMotorCommand( + const std::vector & joint_positions, + const std::vector & joint_speeds, + const std::vector & joint_accs); + + template + void motorCommand( + const std::array & joint_ids, + const std::array & joint_dirs, + const std::array & joint_gears, + const std::vector & joint_angles, + const std::vector & joint_speeds, + const std::vector & joint_accs); + + std::string servo_port_; + SMS_STS sts_servo_; + SCSCL scs_servo_; + + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::Subscription::SharedPtr right_arm_joint_state_sub_; + rclcpp::Subscription::SharedPtr left_arm_joint_state_sub_; + rclcpp::Subscription::SharedPtr torso_joint_state_sub_; + rclcpp::Subscription::SharedPtr head_joint_state_sub_; + rclcpp::Subscription::SharedPtr right_gripper_sub_; + rclcpp::Subscription::SharedPtr left_gripper_sub_; + + static constexpr std::array right_arm_servo_ids_ = {11, 12, 13, 14, 15, 16, 17}; + static constexpr std::array right_arm_servo_dirs_ = {-1, -1, 1, 1, 1, 1, -1}; + static constexpr std::array right_arm_servo_gears_ = {3, 3, 3, 3, 1, 1, 1}; + + static constexpr std::array left_arm_servo_ids_ = {21, 22, 23, 24, 25, 26, 27}; + static constexpr std::array left_arm_servo_dirs_ = {-1, -1, 1, -1, 1, -1, -1}; + static constexpr std::array left_arm_servo_gears_ = {3, 3, 3, 3, 1, 1, 1}; + + static constexpr std::array head_servo_ids_ = {4, 5, 6, 7, 8}; + static constexpr std::array head_servo_dirs_ = {-1, -1, -1, 1, -1}; + static constexpr std::array head_servo_gears_ = {1, 1, 1, 1, 1}; + + static constexpr std::array torso_servo_ids_ = {1, 2, 3}; + static constexpr std::array torso_servo_dirs_ = {-1, 1, -1}; + static constexpr std::array torso_servo_gears_ = {3, 3, 3}; + + static constexpr std::array right_hand_servo_ids_ = {31, 32, 33, 34, 35, 36}; + static constexpr std::array right_hand_servo_default_ = {200, 2350, 2300, 420, 600, 440}; + static constexpr std::array right_hand_servo_range_ = {0, -320, -420, 220, -220, 220}; + + static constexpr std::array left_hand_servo_ids_ = {41, 42, 43, 44, 45, 46}; + static constexpr std::array left_hand_servo_default_ = {512, 1700, 1700, 650, 420, 630}; + static constexpr std::array left_hand_servo_range_ = {0, 350, 550, -260, 260, -300}; + + static constexpr double TORSO_D_ = 0.0865; + static constexpr double TORSO_L1_ = 0.05; + static constexpr double TORSO_H1_ = 0.11; + static constexpr double TORSO_H2_ = 0.11; + + static constexpr double TORSO_SPEED_ = 1.0; + static constexpr double TORSO_ACC_ = 1.5; + static constexpr double ARM_SPEED_ = 1.0; + static constexpr double ARM_ACC_ = 3.0; + static constexpr double HEAD_SPEED_ = 1.0; + static constexpr double HEAD_ACC_ = 7.0; + static constexpr double HAND_SPEED_ = 0.0; + static constexpr double HAND_ACC_ = 15.0; + + const double speed_const_ = 652.6051999582332; + const double acc_const_ = 6.526051999582332; + + rclcpp::Time last_spin_time_; +}; + +} // namespace rx1_motor + +#endif // RX1_MOTOR_H diff --git a/rx1_motor/launch/rx1_motor.launch b/src/rx1/rx1_motor/launch/rx1_motor.launch similarity index 100% rename from rx1_motor/launch/rx1_motor.launch rename to src/rx1/rx1_motor/launch/rx1_motor.launch diff --git a/src/rx1/rx1_motor/launch/rx1_motor.launch.py b/src/rx1/rx1_motor/launch/rx1_motor.launch.py new file mode 100644 index 0000000..708498e --- /dev/null +++ b/src/rx1/rx1_motor/launch/rx1_motor.launch.py @@ -0,0 +1,19 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + servo_port = LaunchConfiguration("servo_port") + + return LaunchDescription([ + DeclareLaunchArgument("servo_port", default_value="/dev/ttyUSB-arduino3.3"), + Node( + package="rx1_motor", + executable="rx1_motor_node", + name="rx1_motor_node", + output="screen", + parameters=[{"servo_port": servo_port}], + ), + ]) diff --git a/rx1_motor/package.xml b/src/rx1/rx1_motor/package.xml similarity index 57% rename from rx1_motor/package.xml rename to src/rx1/rx1_motor/package.xml index 3d6ef90..09bda1b 100644 --- a/rx1_motor/package.xml +++ b/src/rx1/rx1_motor/package.xml @@ -1,18 +1,20 @@ - + rx1_motor 1.2.5 - - rx1 motor hardware control - + rx1 motor hardware control MIT lethic lethic - catkin + ament_cmake feetech_lib - roscpp + rclcpp sensor_msgs + std_msgs urdf xacro - + + ament_cmake + + diff --git a/src/rx1/rx1_motor/src/rx1_motor/rx1_motor.cpp b/src/rx1/rx1_motor/src/rx1_motor/rx1_motor.cpp new file mode 100644 index 0000000..3981e50 --- /dev/null +++ b/src/rx1/rx1_motor/src/rx1_motor/rx1_motor.cpp @@ -0,0 +1,315 @@ +#include "rx1_motor/rx1_motor.hpp" + +#include +#include +#include + +namespace rx1_motor +{ + +using std::placeholders::_1; + +Rx1Motor::Rx1Motor() +: rclcpp::Node("rx1_motor_node") +{ + servo_port_ = this->declare_parameter("servo_port", "/dev/ttyUSB-arduino4.3"); + + joint_state_sub_ = this->create_subscription( + "/command_joint_states", 10, std::bind(&Rx1Motor::jointStateCallback, this, _1)); + right_arm_joint_state_sub_ = this->create_subscription( + "/right_arm_joint_states", 10, std::bind(&Rx1Motor::rightArmJointStateCallback, this, _1)); + left_arm_joint_state_sub_ = this->create_subscription( + "/left_arm_joint_states", 10, std::bind(&Rx1Motor::leftArmJointStateCallback, this, _1)); + torso_joint_state_sub_ = this->create_subscription( + "/torso_joint_states", 10, std::bind(&Rx1Motor::torsoJointStateCallback, this, _1)); + head_joint_state_sub_ = this->create_subscription( + "/head_joint_states", 10, std::bind(&Rx1Motor::headJointStateCallback, this, _1)); + right_gripper_sub_ = this->create_subscription( + "/right_gripper", 10, std::bind(&Rx1Motor::rightGripperCallback, this, _1)); + left_gripper_sub_ = this->create_subscription( + "/left_gripper", 10, std::bind(&Rx1Motor::leftGripperCallback, this, _1)); + + if (!sts_servo_.begin(1000000, servo_port_.c_str())) { + RCLCPP_ERROR(get_logger(), "[RX1_MOTOR] Failed initialize sts servo port %s!", servo_port_.c_str()); + } + + if (!scs_servo_.begin(1000000, servo_port_.c_str())) { + RCLCPP_ERROR(get_logger(), "[RX1_MOTOR] Failed initialize scs servo port %s!", servo_port_.c_str()); + } + + for (size_t i = 0; i < right_arm_servo_ids_.size(); ++i) { + const u8 id = static_cast(right_arm_servo_ids_[i]); + sts_servo_.WritePosEx(id, 2048, 200, 20); + } + for (size_t i = 0; i < left_arm_servo_ids_.size(); ++i) { + const u8 id = static_cast(left_arm_servo_ids_[i]); + sts_servo_.WritePosEx(id, 2048, 200, 20); + } + for (size_t i = 0; i < torso_servo_ids_.size(); ++i) { + const u8 id = static_cast(torso_servo_ids_[i]); + sts_servo_.WritePosEx(id, 2048, 200, 20); + } + for (size_t i = 0; i < head_servo_ids_.size(); ++i) { + const u8 id = static_cast(head_servo_ids_[i]); + if (i == 1 || i == 0) { + sts_servo_.WritePosEx(id, 2048, 200, 20); + } else if (i == 2) { + sts_servo_.WritePosEx(id, 2000, 200, 20); + } else { + scs_servo_.WritePos(id, 512, 0, 100); + } + } + + last_spin_time_ = this->now(); +} + +Rx1Motor::~Rx1Motor() +{ + sts_servo_.end(); +} + +void Rx1Motor::spin() +{ + rclcpp::WallRate rate(100.0); + while (rclcpp::ok()) { + rclcpp::spin_some(shared_from_this()); + update(); + rate.sleep(); + + const auto now = this->now(); + const auto delta = now - last_spin_time_; + const double seconds = delta.seconds(); + if (seconds > 0.0) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "[RX1_MOTOR] actual rate is %f", 1.0 / seconds); + } + last_spin_time_ = now; + } +} + +void Rx1Motor::update() +{ +} + +void Rx1Motor::jointStateCallback(const JointState::SharedPtr msg) +{ + std::vector torso_joint_positions(3); + std::vector head_joint_positions(5); + std::vector right_arm_joint_positions(7); + std::vector left_arm_joint_positions(7); + + for (size_t i = 0; i < msg->position.size(); ++i) { + if (i < 3) { + torso_joint_positions[i] = msg->position[i]; + } else if (i < 8) { + head_joint_positions[i - 3] = msg->position[i]; + } else if (i < 15) { + right_arm_joint_positions[i - 8] = msg->position[i]; + } else if (i < 22) { + left_arm_joint_positions[i - 15] = msg->position[i]; + } + } + + const auto angles = torsoIk( + TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, torso_joint_positions[1], torso_joint_positions[2]); + torso_joint_positions[1] = angles[0]; + torso_joint_positions[2] = angles[1]; + + std::vector torso_speeds(torso_servo_ids_.size(), TORSO_SPEED_); + std::vector torso_accs(torso_servo_ids_.size(), TORSO_ACC_); + motorCommand( + torso_servo_ids_, torso_servo_dirs_, torso_servo_gears_, torso_joint_positions, + torso_speeds, torso_accs); + + std::vector head_speeds(head_servo_ids_.size(), HEAD_SPEED_); + std::vector head_accs(head_servo_ids_.size(), HEAD_ACC_); + headMotorCommand(head_joint_positions, head_speeds, head_accs); + + std::vector arm_speeds(right_arm_servo_ids_.size(), ARM_SPEED_); + std::vector arm_accs(right_arm_servo_ids_.size(), ARM_ACC_); + motorCommand( + right_arm_servo_ids_, right_arm_servo_dirs_, right_arm_servo_gears_, + right_arm_joint_positions, arm_speeds, arm_accs); + motorCommand( + left_arm_servo_ids_, left_arm_servo_dirs_, left_arm_servo_gears_, + left_arm_joint_positions, arm_speeds, arm_accs); +} + +void Rx1Motor::rightArmJointStateCallback(const JointState::SharedPtr msg) +{ + std::vector arm_speeds(right_arm_servo_ids_.size(), ARM_SPEED_); + std::vector arm_accs(right_arm_servo_ids_.size(), ARM_ACC_); + motorCommand( + right_arm_servo_ids_, right_arm_servo_dirs_, right_arm_servo_gears_, + msg->position, arm_speeds, arm_accs); +} + +void Rx1Motor::leftArmJointStateCallback(const JointState::SharedPtr msg) +{ + std::vector arm_speeds(left_arm_servo_ids_.size(), ARM_SPEED_); + std::vector arm_accs(left_arm_servo_ids_.size(), ARM_ACC_); + motorCommand( + left_arm_servo_ids_, left_arm_servo_dirs_, left_arm_servo_gears_, + msg->position, arm_speeds, arm_accs); +} + +void Rx1Motor::torsoJointStateCallback(const JointState::SharedPtr msg) +{ + auto joint_positions = msg->position; + const auto angles = torsoIk( + TORSO_D_, TORSO_L1_, TORSO_H1_, TORSO_H2_, joint_positions[2], joint_positions[1]); + + joint_positions[1] = angles[0]; + joint_positions[2] = angles[1]; + + std::vector torso_speeds(torso_servo_ids_.size(), TORSO_SPEED_); + std::vector torso_accs(torso_servo_ids_.size(), TORSO_ACC_); + + motorCommand( + torso_servo_ids_, torso_servo_dirs_, torso_servo_gears_, joint_positions, + torso_speeds, torso_accs); +} + +std::array Rx1Motor::torsoIk( + double d, double l1, double h1, double h2, double roll, double pitch) +{ + const double cx = std::cos(pitch); + const double sx = std::sin(pitch); + const double cy = std::cos(roll); + const double sy = std::sin(roll); + + const double al = -l1 * l1 * cy + l1 * d * sx * sy; + const double bl = -l1 * l1 * sy + l1 * h1 - l1 * d * sx * cy; + const double cl = -(l1 * l1 + d * d - d * d * cx - l1 * h1 * sy - d * h1 * sx * cy); + const double len_l = std::sqrt(al * al + bl * bl); + + const double ar = -l1 * l1 * cy - l1 * d * sx * sy; + const double br = -l1 * l1 * sy + l1 * h2 + l1 * d * sx * cy; + const double cr = -(l1 * l1 + d * d - d * d * cx - l1 * h2 * sy + d * h2 * sx * cy); + const double len_r = std::sqrt(ar * ar + br * br); + + if (len_l <= std::abs(cl) || len_r <= std::abs(cr)) { + return {0.0, 0.0}; + } + + const double tl_1 = std::asin(cl / len_l) - std::asin(al / len_l); + const double tl_2 = std::asin(cl / len_l) + std::acos(bl / len_l); + const double tr_1 = std::asin(cr / len_r) - std::asin(ar / len_r); + const double tr_2 = std::asin(cr / len_r) + std::acos(br / len_r); + + assert(std::fabs(tl_1 - tl_2) < 1e-3); + assert(std::fabs(tr_1 - tr_2) < 1e-3); + + return {tl_1, tr_1}; +} + +void Rx1Motor::headJointStateCallback(const JointState::SharedPtr msg) +{ + std::vector joint_speeds(head_servo_ids_.size(), HEAD_SPEED_); + std::vector joint_accs(head_servo_ids_.size(), HEAD_ACC_); + headMotorCommand(msg->position, joint_speeds, joint_accs); +} + +void Rx1Motor::rightGripperCallback(const Float32::SharedPtr msg) +{ + const double grip_ratio = msg->data; + const int length = static_cast(right_hand_servo_ids_.size()); + const u16 speed = static_cast(HAND_SPEED_ * speed_const_); + const u8 acc = static_cast(HAND_ACC_ * acc_const_); + + for (int i = 0; i < length; ++i) { + const u8 id = static_cast(right_hand_servo_ids_[i]); + const s16 pos = static_cast(right_hand_servo_default_[i] + grip_ratio * right_hand_servo_range_[i]); + + if (i == 1 || i == 2) { + sts_servo_.WritePosEx(id, pos, speed, acc); + } else if (i == 3) { + scs_servo_.WritePos(id, pos, 0, 400); + } else if (i == 4) { + scs_servo_.WritePos(id, pos, 0, 300); + } else if (i == 5) { + scs_servo_.WritePos(id, pos, 0, 200); + } + } + + scs_servo_.WritePos(right_hand_servo_ids_[0], 200, 0, 400); +} + +void Rx1Motor::leftGripperCallback(const Float32::SharedPtr msg) +{ + const double grip_ratio = msg->data; + const int length = static_cast(left_hand_servo_ids_.size()); + const u16 speed = static_cast(HAND_SPEED_ * speed_const_); + const u8 acc = static_cast(HAND_ACC_ * acc_const_); + + for (int i = 0; i < length; ++i) { + const u8 id = static_cast(left_hand_servo_ids_[i]); + const s16 pos = static_cast(left_hand_servo_default_[i] + grip_ratio * left_hand_servo_range_[i]); + + if (i == 1 || i == 2) { + sts_servo_.WritePosEx(id, pos, speed, acc); + } else if (i == 3) { + scs_servo_.WritePos(id, pos, 0, 400); + } else if (i == 4) { + scs_servo_.WritePos(id, pos, 0, 300); + } else if (i == 5) { + scs_servo_.WritePos(id, pos, 0, 200); + } + } + + scs_servo_.WritePos(left_hand_servo_ids_[0], 512, 0, 400); +} + +void Rx1Motor::headMotorCommand( + const std::vector & joint_positions, + const std::vector & joint_speeds, + const std::vector & joint_accs) +{ + for (size_t i = 0; i < joint_positions.size(); ++i) { + const u8 id = static_cast(head_servo_ids_[i]); + const u16 speed = static_cast(joint_speeds[i] * speed_const_); + const u8 acc = static_cast(joint_accs[i] * acc_const_); + + if (i <= 2) { + const s16 pos = static_cast( + joint_positions[i] / 3.14 * 2048 * head_servo_dirs_[i] * head_servo_gears_[i] + 2048); + sts_servo_.WritePosEx(id, pos, speed, acc); + } else { + const s16 pos = static_cast( + joint_positions[i] / 3.14 * 512 * head_servo_dirs_[i] * head_servo_gears_[i] + 512); + scs_servo_.WritePos(id, pos, 0, 0); + } + } +} + +template +void Rx1Motor::motorCommand( + const std::array & joint_ids, + const std::array & joint_dirs, + const std::array & joint_gears, + const std::vector & joint_angles, + const std::vector & joint_speeds, + const std::vector & joint_accs) +{ + const int length = static_cast(joint_ids.size()); + auto ids = std::make_unique(length); + auto pos = std::make_unique(length); + auto speeds = std::make_unique(length); + auto accs = std::make_unique(length); + + for (int i = 0; i < length; ++i) { + ids[i] = static_cast(joint_ids[i]); + speeds[i] = static_cast(joint_speeds[i] * joint_gears[i] * speed_const_); + accs[i] = static_cast(joint_accs[i] * joint_gears[i] * acc_const_); + pos[i] = static_cast(joint_angles[i] / 3.14 * 2048 * joint_dirs[i] * joint_gears[i] + 2048); + + if (i >= 3) { + speeds[i] = 0; + accs[i] = static_cast(accs[i] * 10); + } + } + + sts_servo_.SyncWritePosEx(ids.get(), length, pos.get(), speeds.get(), accs.get()); +} + +} // namespace rx1_motor diff --git a/src/rx1/rx1_motor/src/rx1_motor_node.cpp b/src/rx1/rx1_motor/src/rx1_motor_node.cpp new file mode 100644 index 0000000..cc94a54 --- /dev/null +++ b/src/rx1/rx1_motor/src/rx1_motor_node.cpp @@ -0,0 +1,18 @@ +#include "rx1_motor/rx1_motor.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + try { + node->spin(); + } catch (const std::runtime_error & ex) { + RCLCPP_FATAL(node->get_logger(), "[RX1_MOTOR] Runtime error: %s", ex.what()); + rclcpp::shutdown(); + return 1; + } + + rclcpp::shutdown(); + return 0; +} diff --git a/src/rx1/rx1_motor_lib/CMakeLists.txt b/src/rx1/rx1_motor_lib/CMakeLists.txt new file mode 100644 index 0000000..087645d --- /dev/null +++ b/src/rx1/rx1_motor_lib/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(rx1_motor_lib) + +find_package(ament_cmake REQUIRED) +find_package(feetech_lib REQUIRED) + +add_library(rx1_motor_lib src/rx_motor_lib.cpp) +target_compile_features(rx1_motor_lib PUBLIC cxx_std_17) +target_include_directories(rx1_motor_lib PUBLIC + $ + $ +) +ament_target_dependencies(rx1_motor_lib feetech_lib) + +install(TARGETS rx1_motor_lib + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(feetech_lib) +ament_package() diff --git a/rx1_motor_lib/include/rx1_motor_lib/rx1_motor_lib.hpp b/src/rx1/rx1_motor_lib/include/rx1_motor_lib/rx1_motor_lib.hpp similarity index 100% rename from rx1_motor_lib/include/rx1_motor_lib/rx1_motor_lib.hpp rename to src/rx1/rx1_motor_lib/include/rx1_motor_lib/rx1_motor_lib.hpp diff --git a/src/rx1/rx1_motor_lib/package.xml b/src/rx1/rx1_motor_lib/package.xml new file mode 100644 index 0000000..f6e3db9 --- /dev/null +++ b/src/rx1/rx1_motor_lib/package.xml @@ -0,0 +1,14 @@ + + + rx1_motor_lib + 0.0.1 + RX1 motor driver package + lethic + MIT + ament_cmake + feetech_lib + + + ament_cmake + + diff --git a/rx1_motor_lib/src/rx_motor_lib.cpp b/src/rx1/rx1_motor_lib/src/rx_motor_lib.cpp similarity index 100% rename from rx1_motor_lib/src/rx_motor_lib.cpp rename to src/rx1/rx1_motor_lib/src/rx_motor_lib.cpp diff --git a/src/rx1/rx1_moveit_config/CMakeLists.txt b/src/rx1/rx1_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..7380925 --- /dev/null +++ b/src/rx1/rx1_moveit_config/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(rx1_moveit_config) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/fake_joint_trajectory_controller.py + scripts/joint_state_timestamp_republisher.py + scripts/wait_for_sim_controller.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/rx1/rx1_moveit_config/config/joint_limits.yaml b/src/rx1/rx1_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..7a9d01e --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/joint_limits.yaml @@ -0,0 +1,101 @@ +joint_limits: + base2torso_yaw_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + torso_yaw2pitch_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + torso_pitch2roll_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + right_shoul_base2shoul_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + right_shoul2shoul_rot_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + right_arm2armrot_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + right_armrot2elbow_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + right_forearm2forearmrot_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + right_forearmrot2forearm_pitch_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + right_forearm_pitch2forearm_roll_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + left_shoul_base2shoul_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + left_shoul2shoul_rot_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + left_arm2armrot_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + left_armrot2elbow_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + left_forearm2forearmrot_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + left_forearmrot2forearm_pitch_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + left_forearm_pitch2forearm_roll_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + head_base2neck_yaw_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + neck_yaw2pitch_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 + neck_pitch2head_depth_cam_mount_joint: + has_velocity_limits: true + max_velocity: 15.0 + has_acceleration_limits: true + max_acceleration: 15.0 diff --git a/src/rx1/rx1_moveit_config/config/kinematics.yaml b/src/rx1/rx1_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..76dc3d0 --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/kinematics.yaml @@ -0,0 +1,29 @@ +right_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 + +right_arm_with_torso: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 + +left_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 + +left_arm_with_torso: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 + +torso: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.01 + kinematics_solver_timeout: 0.05 + +head: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.01 + kinematics_solver_timeout: 0.05 diff --git a/src/rx1/rx1_moveit_config/config/moveit.rviz b/src/rx1/rx1_moveit_config/config/moveit.rviz new file mode 100644 index 0000000..1f71c90 --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/moveit.rviz @@ -0,0 +1,166 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 600 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: true + Interrupt Display: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.2 + Joint Violation Color: 255; 0; 255 + Planning Group: "" + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.9 + Scene Color: 50; 230; 50 + Scene Display Time: 0.01 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.8 + Focal Point: + X: 0 + Y: 0 + Z: 0.8 + Pitch: 0.5 + Yaw: 0.8 +Window Geometry: + Height: 1000 + Width: 1600 diff --git a/src/rx1/rx1_moveit_config/config/moveit_controllers.yaml b/src/rx1/rx1_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 0000000..0522438 --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,34 @@ +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + execution_duration_monitoring: false + +moveit_simple_controller_manager: + controller_names: + - rx1_fake_controller + + rx1_fake_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - right_shoul_base2shoul_joint + - right_shoul2shoul_rot_joint + - right_arm2armrot_joint + - right_armrot2elbow_joint + - right_forearm2forearmrot_joint + - right_forearmrot2forearm_pitch_joint + - right_forearm_pitch2forearm_roll_joint + - left_shoul_base2shoul_joint + - left_shoul2shoul_rot_joint + - left_arm2armrot_joint + - left_armrot2elbow_joint + - left_forearm2forearmrot_joint + - left_forearmrot2forearm_pitch_joint + - left_forearm_pitch2forearm_roll_joint + - head_base2neck_yaw_joint + - neck_yaw2pitch_joint + - neck_pitch2head_depth_cam_mount_joint diff --git a/src/rx1/rx1_moveit_config/config/moveit_controllers_gazebo.yaml b/src/rx1/rx1_moveit_config/config/moveit_controllers_gazebo.yaml new file mode 100644 index 0000000..f31e531 --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/moveit_controllers_gazebo.yaml @@ -0,0 +1,37 @@ +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.0 + execution_duration_monitoring: false + +moveit_simple_controller_manager: + controller_names: + - rx1_joint_trajectory_controller + + rx1_joint_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - base2torso_yaw_joint + - torso_yaw2pitch_joint + - torso_pitch2roll_joint + - head_base2neck_yaw_joint + - neck_yaw2pitch_joint + - neck_pitch2head_depth_cam_mount_joint + - right_shoul_base2shoul_joint + - right_shoul2shoul_rot_joint + - right_arm2armrot_joint + - right_armrot2elbow_joint + - right_forearm2forearmrot_joint + - right_forearmrot2forearm_pitch_joint + - right_forearm_pitch2forearm_roll_joint + - left_shoul_base2shoul_joint + - left_shoul2shoul_rot_joint + - left_arm2armrot_joint + - left_armrot2elbow_joint + - left_forearm2forearmrot_joint + - left_forearmrot2forearm_pitch_joint + - left_forearm_pitch2forearm_roll_joint diff --git a/src/rx1/rx1_moveit_config/config/ompl_planning.yaml b/src/rx1/rx1_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..0da201c --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,36 @@ +planner_configs: + RRTConnect: + type: geometric::RRTConnect + range: 0.0 + RRTstar: + type: geometric::RRTstar + range: 0.0 + goal_bias: 0.05 + delay_collision_checking: 1 + PRMstar: + type: geometric::PRMstar + +right_arm: + planner_configs: + - RRTConnect + - RRTstar + - PRMstar + longest_valid_segment_fraction: 0.01 + +left_arm: + planner_configs: + - RRTConnect + - RRTstar + - PRMstar + longest_valid_segment_fraction: 0.01 + +dual_arms: + planner_configs: + - RRTConnect + - PRMstar + longest_valid_segment_fraction: 0.01 + +head: + planner_configs: + - RRTConnect + longest_valid_segment_fraction: 0.02 diff --git a/src/rx1/rx1_moveit_config/config/rx1.srdf b/src/rx1/rx1_moveit_config/config/rx1.srdf new file mode 100644 index 0000000..e4ef37d --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/rx1.srdf @@ -0,0 +1,159 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/rx1/rx1_moveit_config/config/sensors_3d.yaml b/src/rx1/rx1_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..48c2df6 --- /dev/null +++ b/src/rx1/rx1_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,18 @@ +octomap_frame: world +octomap_resolution: 0.05 +max_range: 4.0 + +sensors: + - head_rgbd_camera + +head_rgbd_camera: + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + image_topic: /rx1/rgbd_camera/depth_image + queue_size: 5 + near_clipping_plane_distance: 0.15 + far_clipping_plane_distance: 4.0 + shadow_threshold: 0.2 + padding_scale: 1.0 + padding_offset: 0.03 + max_update_rate: 10.0 + filtered_cloud_topic: /rx1/rgbd_camera/filtered_points diff --git a/src/rx1/rx1_moveit_config/launch/demo.launch.py b/src/rx1/rx1_moveit_config/launch/demo.launch.py new file mode 100644 index 0000000..0be9504 --- /dev/null +++ b/src/rx1/rx1_moveit_config/launch/demo.launch.py @@ -0,0 +1,166 @@ +import os +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + + +def load_yaml(package_name, relative_path): + package_path = get_package_share_directory(package_name) + absolute_path = os.path.join(package_path, relative_path) + with open(absolute_path, "r", encoding="utf-8") as file: + return yaml.safe_load(file) + + +def load_text(package_name, relative_path): + package_path = get_package_share_directory(package_name) + absolute_path = os.path.join(package_path, relative_path) + with open(absolute_path, "r", encoding="utf-8") as file: + return file.read() + + +def generate_launch_description(): + model = LaunchConfiguration("model") + use_rviz = LaunchConfiguration("use_rviz") + use_gui = LaunchConfiguration("use_gui") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + rviz_config = os.path.join( + get_package_share_directory("rx1_moveit_config"), "config", "moveit.rviz" + ) + + robot_description = { + "robot_description": Command( + [FindExecutable(name="xacro"), " ", "--verbosity", " 0 ", model] + ) + } + robot_description_semantic = { + "robot_description_semantic": load_text("rx1_moveit_config", "config/rx1.srdf") + } + robot_description_kinematics = { + "robot_description_kinematics": load_yaml("rx1_moveit_config", "config/kinematics.yaml") + } + ompl_planning = load_yaml("rx1_moveit_config", "config/ompl_planning.yaml") + joint_limits = {"robot_description_planning": load_yaml("rx1_moveit_config", "config/joint_limits.yaml")} + moveit_controllers = load_yaml("rx1_moveit_config", "config/moveit_controllers.yaml") + + planning_pipeline_config = { + "planning_pipelines": ["ompl"], + "default_planning_pipeline": "ompl", + "ompl": { + "planning_plugins": ["ompl_interface/OMPLPlanner"], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], + "start_state_max_bounds_error": 0.1, + }, + } + planning_pipeline_config["ompl"].update(ompl_planning) + + trajectory_execution = { + "allow_trajectory_execution": True, + "moveit_manage_controllers": False, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + "publish_robot_description": True, + "publish_robot_description_semantic": True, + } + + return LaunchDescription([ + DeclareLaunchArgument("use_rviz", default_value="true"), + DeclareLaunchArgument("use_gui", default_value="false"), + DeclareLaunchArgument("use_fake_hardware", default_value="true"), + DeclareLaunchArgument( + "model", + default_value=os.path.join( + get_package_share_directory("rx1_description"), + "urdf", + "rx1_optimized.urdf.xacro", + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + output="screen", + condition=IfCondition( + PythonExpression(["'", use_gui, "' == 'true' and '", use_fake_hardware, "' == 'false'"]) + ), + ), + Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + output="screen", + condition=IfCondition( + PythonExpression(["'", use_gui, "' == 'false' and '", use_fake_hardware, "' == 'false'"]) + ), + ), + Node( + package="rx1_moveit_config", + executable="fake_joint_trajectory_controller.py", + name="rx1_fake_controller", + output="screen", + condition=IfCondition(use_fake_hardware), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[robot_description], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="world_to_base_link", + arguments=["0", "0", "0", "0", "0", "0", "world", "base_link"], + output="screen", + ), + Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + planning_pipeline_config, + joint_limits, + moveit_controllers, + trajectory_execution, + planning_scene_monitor_parameters, + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + condition=IfCondition(use_rviz), + arguments=["-d", rviz_config], + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + planning_pipeline_config, + joint_limits, + ], + ), + ]) diff --git a/src/rx1/rx1_moveit_config/launch/gazebo_demo.launch.py b/src/rx1/rx1_moveit_config/launch/gazebo_demo.launch.py new file mode 100644 index 0000000..302965d --- /dev/null +++ b/src/rx1/rx1_moveit_config/launch/gazebo_demo.launch.py @@ -0,0 +1,215 @@ +import os +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration +from launch_ros.actions import Node +def load_yaml(package_name, relative_path): + package_path = get_package_share_directory(package_name) + absolute_path = os.path.join(package_path, relative_path) + with open(absolute_path, "r", encoding="utf-8") as file: + return yaml.safe_load(file) + + +def load_text(package_name, relative_path): + package_path = get_package_share_directory(package_name) + absolute_path = os.path.join(package_path, relative_path) + with open(absolute_path, "r", encoding="utf-8") as file: + return file.read() + + +def generate_launch_description(): + moveit_model = LaunchConfiguration("moveit_model") + sim_model = LaunchConfiguration("sim_model") + moveit_use_rviz = LaunchConfiguration("moveit_use_rviz") + world = LaunchConfiguration("world") + robot_name = LaunchConfiguration("robot_name") + robot_x = LaunchConfiguration("robot_x") + robot_y = LaunchConfiguration("robot_y") + robot_z = LaunchConfiguration("robot_z") + + rx1_gazebo_share = get_package_share_directory("rx1_gazebo") + + robot_description = { + "robot_description": Command( + [FindExecutable(name="xacro"), " ", "--verbosity", " 0 ", moveit_model] + ), + "use_sim_time": True, + } + robot_description_semantic = { + "robot_description_semantic": load_text("rx1_moveit_config", "config/rx1.srdf") + } + robot_description_kinematics = { + "robot_description_kinematics": load_yaml( + "rx1_moveit_config", "config/kinematics.yaml" + ) + } + ompl_planning = load_yaml("rx1_moveit_config", "config/ompl_planning.yaml") + joint_limits = { + "robot_description_planning": load_yaml( + "rx1_moveit_config", "config/joint_limits.yaml" + ) + } + moveit_controllers = load_yaml( + "rx1_moveit_config", "config/moveit_controllers_gazebo.yaml" + ) + planning_pipeline_config = { + "planning_pipelines": ["ompl"], + "default_planning_pipeline": "ompl", + "ompl": { + "planning_plugins": ["ompl_interface/OMPLPlanner"], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], + "start_state_max_bounds_error": 0.1, + }, + } + planning_pipeline_config["ompl"].update(ompl_planning) + + trajectory_execution = { + "allow_trajectory_execution": True, + "moveit_manage_controllers": False, + "use_sim_time": True, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + "publish_robot_description": True, + "publish_robot_description_semantic": True, + "use_sim_time": True, + } + + sim_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(rx1_gazebo_share, "launch", "rx1_gazebo.launch.py") + ), + launch_arguments={ + "model": sim_model, + "world": world, + "robot_name": robot_name, + "robot_x": robot_x, + "robot_y": robot_y, + "robot_z": robot_z, + "use_rviz": "false", + }.items(), + ) + + joint_state_timestamp_republisher = Node( + package="rx1_moveit_config", + executable="joint_state_timestamp_republisher.py", + name="joint_state_timestamp_republisher", + output="screen", + parameters=[{"use_sim_time": True}], + ) + + wait_for_sim_controller = Node( + package="rx1_moveit_config", + executable="wait_for_sim_controller.py", + name="wait_for_sim_controller", + output="screen", + parameters=[{"use_sim_time": True}], + ) + + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + remappings=[("joint_states", "/joint_states_moveit")], + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + planning_pipeline_config, + joint_limits, + moveit_controllers, + trajectory_execution, + planning_scene_monitor_parameters, + ], + ) + + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + condition=IfCondition(moveit_use_rviz), + remappings=[("joint_states", "/joint_states_moveit")], + arguments=[ + "-d", + os.path.join( + get_package_share_directory("rx1_moveit_config"), + "config", + "moveit.rviz", + ), + ], + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + planning_pipeline_config, + joint_limits, + {"use_sim_time": True}, + ], + ) + + launch_moveit_side = RegisterEventHandler( + OnProcessExit( + target_action=wait_for_sim_controller, + on_exit=[ + joint_state_timestamp_republisher, + move_group, + rviz, + ], + ) + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("moveit_use_rviz", default_value="true"), + DeclareLaunchArgument( + "moveit_model", + default_value=os.path.join( + get_package_share_directory("rx1_description"), + "urdf", + "rx1_optimized.urdf.xacro", + ), + ), + DeclareLaunchArgument( + "sim_model", + default_value=os.path.join( + get_package_share_directory("rx1_description"), + "urdf", + "rx1_optimized.harmonic.urdf.xacro", + ), + ), + DeclareLaunchArgument( + "world", + default_value=os.path.join( + rx1_gazebo_share, "worlds", "rx1_table_world.sdf" + ), + ), + DeclareLaunchArgument("robot_name", default_value="rx1"), + DeclareLaunchArgument("robot_x", default_value="0.0"), + DeclareLaunchArgument("robot_y", default_value="0.0"), + DeclareLaunchArgument("robot_z", default_value="0.0"), + sim_launch, + wait_for_sim_controller, + launch_moveit_side, + ] + ) diff --git a/src/rx1/rx1_moveit_config/package.xml b/src/rx1/rx1_moveit_config/package.xml new file mode 100644 index 0000000..e82e457 --- /dev/null +++ b/src/rx1/rx1_moveit_config/package.xml @@ -0,0 +1,36 @@ + + + rx1_moveit_config + 0.1.0 + MoveIt 2 configuration package for the RX1 humanoid. + lethic + MIT + + ament_cmake + + ament_index_python + joint_state_publisher + control_msgs + launch + launch_ros + moveit_configs_utils + moveit_kinematics + moveit_planners_ompl + moveit_ros_move_group + moveit_ros_planning + moveit_ros_visualization + rclpy + robot_state_publisher + rosgraph_msgs + rviz2 + rx1_description + rx1_gazebo + sensor_msgs + tf2_ros + trajectory_msgs + xacro + + + ament_cmake + + diff --git a/src/rx1/rx1_moveit_config/scripts/fake_joint_trajectory_controller.py b/src/rx1/rx1_moveit_config/scripts/fake_joint_trajectory_controller.py new file mode 100755 index 0000000..07cff35 --- /dev/null +++ b/src/rx1/rx1_moveit_config/scripts/fake_joint_trajectory_controller.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 + +import time +from typing import Dict, List + +import rclpy +from control_msgs.action import FollowJointTrajectory +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.duration import Duration +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +class FakeJointTrajectoryController(Node): + def __init__(self) -> None: + super().__init__("rx1_fake_controller") + self._joint_names = [ + "base2torso_yaw_joint", + "torso_yaw2pitch_joint", + "torso_pitch2roll_joint", + "right_shoul_base2shoul_joint", + "right_shoul2shoul_rot_joint", + "right_arm2armrot_joint", + "right_armrot2elbow_joint", + "right_forearm2forearmrot_joint", + "right_forearmrot2forearm_pitch_joint", + "right_forearm_pitch2forearm_roll_joint", + "left_shoul_base2shoul_joint", + "left_shoul2shoul_rot_joint", + "left_arm2armrot_joint", + "left_armrot2elbow_joint", + "left_forearm2forearmrot_joint", + "left_forearmrot2forearm_pitch_joint", + "left_forearm_pitch2forearm_roll_joint", + "head_base2neck_yaw_joint", + "neck_yaw2pitch_joint", + "neck_pitch2head_depth_cam_mount_joint", + "head_depth_cam_mount2right_ear_joint", + "head_depth_cam_mount2left_ear_joint", + ] + self._positions: Dict[str, float] = {joint: 0.0 for joint in self._joint_names} + self._velocities: Dict[str, float] = {joint: 0.0 for joint in self._joint_names} + self._active_goal = None + + self._joint_state_pub = self.create_publisher(JointState, "joint_states", 10) + self._publish_timer = self.create_timer(0.05, self._publish_joint_states) + self._action_server = ActionServer( + self, + FollowJointTrajectory, + "rx1_fake_controller/follow_joint_trajectory", + goal_callback=self._goal_callback, + cancel_callback=self._cancel_callback, + execute_callback=self._execute_callback, + ) + + def _publish_joint_states(self) -> None: + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = list(self._joint_names) + msg.position = [self._positions[joint] for joint in self._joint_names] + msg.velocity = [self._velocities[joint] for joint in self._joint_names] + self._joint_state_pub.publish(msg) + + def _goal_callback(self, goal_request: FollowJointTrajectory.Goal) -> GoalResponse: + if self._active_goal is not None: + self.get_logger().warn("Rejecting trajectory goal because another goal is active.") + return GoalResponse.REJECT + + unknown_joints = [joint for joint in goal_request.trajectory.joint_names if joint not in self._positions] + if unknown_joints: + self.get_logger().warn(f"Rejecting trajectory with unknown joints: {unknown_joints}") + return GoalResponse.REJECT + + return GoalResponse.ACCEPT + + def _cancel_callback(self, _goal_handle) -> CancelResponse: + return CancelResponse.ACCEPT + + def _execute_callback(self, goal_handle) -> FollowJointTrajectory.Result: + self._active_goal = goal_handle + trajectory = goal_handle.request.trajectory + joint_names: List[str] = list(trajectory.joint_names) + result = FollowJointTrajectory.Result() + + if not trajectory.points: + goal_handle.succeed() + self._active_goal = None + return result + + start_time = self.get_clock().now() + previous_time_sec = 0.0 + + for point in trajectory.points: + if goal_handle.is_cancel_requested: + goal_handle.canceled() + self._active_goal = None + return result + + target_time_sec = ( + float(point.time_from_start.sec) + + float(point.time_from_start.nanosec) / 1e9 + ) + sleep_time = max(0.0, target_time_sec - previous_time_sec) + if sleep_time > 0.0: + time.sleep(sleep_time) + previous_time_sec = target_time_sec + + for index, joint in enumerate(joint_names): + self._positions[joint] = point.positions[index] + if index < len(point.velocities): + self._velocities[joint] = point.velocities[index] + else: + self._velocities[joint] = 0.0 + + feedback = FollowJointTrajectory.Feedback() + feedback.header.stamp = (start_time + Duration(seconds=target_time_sec)).to_msg() + feedback.joint_names = joint_names + feedback.actual.positions = [self._positions[joint] for joint in joint_names] + feedback.actual.velocities = [self._velocities[joint] for joint in joint_names] + feedback.desired = point + feedback.error.positions = [0.0] * len(joint_names) + feedback.error.velocities = [0.0] * len(joint_names) + goal_handle.publish_feedback(feedback) + + goal_handle.succeed() + for joint in joint_names: + self._velocities[joint] = 0.0 + self._active_goal = None + return result + + +def main() -> None: + rclpy.init() + node = FakeJointTrajectoryController() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/rx1/rx1_moveit_config/scripts/joint_state_timestamp_republisher.py b/src/rx1/rx1_moveit_config/scripts/joint_state_timestamp_republisher.py new file mode 100644 index 0000000..6d3d0e6 --- /dev/null +++ b/src/rx1/rx1_moveit_config/scripts/joint_state_timestamp_republisher.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, qos_profile_sensor_data +from rosgraph_msgs.msg import Clock +from sensor_msgs.msg import JointState + + +class JointStateTimestampRepublisher(Node): + def __init__(self) -> None: + super().__init__("joint_state_timestamp_republisher") + self._latest_clock = None + self._latest_joint_state = None + self._logged_clock = False + self._logged_joint_state = False + output_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE) + self._publisher = self.create_publisher( + JointState, "joint_states_moveit", output_qos + ) + self.create_subscription(Clock, "clock", self._handle_clock, qos_profile_sensor_data) + self.create_subscription( + JointState, + "/joint_state_broadcaster/joint_states", + self._handle_joint_state, + qos_profile_sensor_data, + ) + # Some ros2_control setups still publish on the global /joint_states topic + # even when local topics are requested. Listen there too, but republish onto + # a dedicated MoveIt-only topic so the zero-stamped raw stream cannot race + # with the corrected one. + self.create_subscription( + JointState, + "/joint_states", + self._handle_joint_state, + qos_profile_sensor_data, + ) + self.create_timer(0.05, self._publish_latest_joint_state) + + def _handle_clock(self, msg: Clock) -> None: + self._latest_clock = msg.clock + if not self._logged_clock: + self.get_logger().info("Received Gazebo clock; stamping MoveIt joint states") + self._logged_clock = True + + def _handle_joint_state(self, msg: JointState) -> None: + self._latest_joint_state = msg + if not self._logged_joint_state: + self.get_logger().info( + f"Received raw joint states with {len(msg.name)} joints; republishing for MoveIt" + ) + self._logged_joint_state = True + + def _publish_latest_joint_state(self) -> None: + if self._latest_joint_state is None: + return + + stamped = JointState() + stamped.header = self._latest_joint_state.header + stamped.name = list(self._latest_joint_state.name) + stamped.position = list(self._latest_joint_state.position) + stamped.velocity = list(self._latest_joint_state.velocity) + stamped.effort = list(self._latest_joint_state.effort) + + if self._latest_clock is not None: + stamped.header.stamp = self._latest_clock + else: + stamped.header.stamp = self.get_clock().now().to_msg() + + self._publisher.publish(stamped) + + +def main() -> None: + rclpy.init() + node = JointStateTimestampRepublisher() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/rx1/rx1_moveit_config/scripts/wait_for_sim_controller.py b/src/rx1/rx1_moveit_config/scripts/wait_for_sim_controller.py new file mode 100644 index 0000000..07dc28d --- /dev/null +++ b/src/rx1/rx1_moveit_config/scripts/wait_for_sim_controller.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import rclpy +from control_msgs.action import FollowJointTrajectory +from rclpy.action import ActionClient +from rclpy.node import Node + + +class WaitForSimController(Node): + def __init__(self) -> None: + super().__init__("wait_for_sim_controller") + self._action_client = ActionClient( + self, + FollowJointTrajectory, + "rx1_joint_trajectory_controller/follow_joint_trajectory", + ) + + def wait(self) -> None: + self.get_logger().info( + "Waiting for rx1_joint_trajectory_controller action server before starting MoveIt" + ) + attempts = 0 + while rclpy.ok(): + if self._action_client.wait_for_server(timeout_sec=1.0): + self.get_logger().info( + "Trajectory action server is available; starting MoveIt side" + ) + return + + attempts += 1 + if attempts % 5 == 0: + self.get_logger().info( + "Still waiting for rx1_joint_trajectory_controller/follow_joint_trajectory" + ) + + +def main() -> None: + rclpy.init() + node = WaitForSimController() + try: + node.wait() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()