Merge remote-tracking branch 'origin/main' into rolling

Author: danielsanjosepro

CHANGELOG.md

@@ -1,5 +1,19 @@
 # Changelog
 
+## [1.2.1](https://github.com/utiasDSL/crisp_controllers/compare/v1.2.0...v1.2.1) (2025-10-21)
+
+
+### Bug Fixes
+
+* Rollback jittery controller ([5d78af9](https://github.com/utiasDSL/crisp_controllers/commit/5d78af9f9f1b2b2aa3bfe089eb6b33a03ce33858))
+
+## [1.2.0](https://github.com/utiasDSL/crisp_controllers/compare/v1.1.0...v1.2.0) (2025-10-18)
+
+
+### Features
+
+* Fix Cartesian Controller to work with respect to the  base_frame indicated in the parameters and not just world frame ([#10](https://github.com/utiasDSL/crisp_controllers/issues/10)) ([da805ab](https://github.com/utiasDSL/crisp_controllers/commit/da805ab45691a66279ca88138fc2583d5e77319b))
+
 ## [1.1.0](https://github.com/utiasDSL/crisp_controllers/compare/v1.0.0...v1.1.0) (2025-09-08)
 
 

CMakeLists.txt

@@ -18,11 +18,37 @@ endif()
 # Enable precompiled headers
 option(USE_PRECOMPILED_HEADERS "Use precompiled headers" ON)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra -Wpedantic)
+option(CHECK_TIDY "Adds clang-tidy tests" OFF)
+
+option(ENABLE_NATIVE_OPTIMIZATION
+  "Enable CPU-specific optimizations (-march=native)"
+  OFF
+)
+
+# Default to release build
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE)
 endif()
 
-option(CHECK_TIDY "Adds clang-tidy tests" OFF)
+# Compiler flags by build type
+add_library(project_compile_options INTERFACE)
+target_compile_options(project_compile_options INTERFACE
+  # Warnings (build only)
+  $<$<CXX_COMPILER_ID:GNU,Clang>:-Wall -Wextra -Wpedantic>
+
+  # Debug
+  $<$<AND:$<CONFIG:Debug>,$<CXX_COMPILER_ID:GNU,Clang>>:-O0 -g>
+
+  # Release
+  $<$<AND:$<CONFIG:Release>,$<CXX_COMPILER_ID:GNU,Clang>>:-O3 -DNDEBUG>
+)
+
+# Optional CPU-specific optimizations (non-portable)
+if(ENABLE_NATIVE_OPTIMIZATION)
+  target_compile_options(project_compile_options INTERFACE
+    $<$<AND:$<CONFIG:Release>,$<CXX_COMPILER_ID:GNU,Clang>>:-march=native>
+)
+endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -65,6 +91,11 @@ add_library(
         src/twist_broadcaster.cpp
 )
 
+target_link_libraries(${PROJECT_NAME}
+  PRIVATE
+    project_compile_options
+)
+
 target_include_directories(${PROJECT_NAME}
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

docs/getting_started.md

@@ -245,6 +245,12 @@ python -c "import crisp_gym"
 
 1. This will set some environment variable pre-installation as well as checking that you defined the previous script properly.
 
+You can also check that your configs are set up with:
+
+```sh
+pixi shell crisp-check-config
+```
+
 If the previous steps worked, then you are good to go.
 
 ### Teleoperation: Record data in [LeRobotFormat](https://github.com/huggingface/lerobot)
@@ -263,7 +269,7 @@ For your specific setup you need to:
 
 Then, to record data use:
 ```sh
-pixi run -e humble-lerobot python scripts/record_lerobot_format_leader_follower.py \
+pixi run -e humble-lerobot crisp-record-leader-follower \
    --repo-id <your_account>/<repo_name> # (1)!
 ```
 
@@ -366,7 +372,7 @@ a similar record script to [`scripts/record_lerobot_format_leader_follower.py`](
 
 You can use LeRobot train scripts to train a policy simply by running:
 ```sh
-pixi run -e lerobot python -m lerobot.scripts.train \
+pixi run -e lerobot python -m lerobot.scripts.lerobot-train \
           --dataset.repo_id=<your_account>/<repo_name> \
           --policy.type=diffusion \
           --policy.push_to_hub=false
@@ -382,7 +388,7 @@ Check [LeRobot](https://github.com/huggingface/lerobot) for more information.
 
 After training with LeRobot, you can deploy the policy with:
 ```sh
-pixi run -e humble-lerobot python scripts/deploy_policy.py # (1)!
+pixi run -e humble-lerobot crisp-deploy-policy  # (1)!
 ```
 
 1. The script will interactively allow you to choose a model inside `outputs/train`. If you want to explicitly pass a path you can override it with `--path`

docs/index.md

@@ -20,6 +20,10 @@ _If you use this work, please cite it using the [bibtex](#citing) below._
 
 Check the [controllers (CRISP controllers) :simple-github:](https://github.com/utiasDSL/crisp_controllers) , robot [demos (CRISP controllers demos) :simple-github:](https://github.com/utiasDSL/crisp_controllers_demos), a simple [python interface (CRISP_PY) :simple-github:](https://github.com/utiasDSL/crisp_py), and a [Gymnasium wrapper (CRISP_GYM) :simple-github:](https://github.com/utiasDSL/crisp_gym) for real-world experiments.
 
+!!! info "Aloha gripper for Manipulators"
+    Check out [aloha4franka](https://tum-lsy.github.io/aloha4franka/) for the gripper used in the videos.
+
+
 | <video src="media/pap_demo.mp4" playsinline muted controls loop autoplay width="800"/> | <video src="media/policy.mp4" playsinline controls="true" loop="true" autoplay="true" width="800"/> |
 |:--:|:--:|
 | Robot teleoperated using a Follower-Leader system in [CRISP_GYM :simple-github:](https://github.com/utiasDSL/crisp_gym) | Diffusion Policy trained and deployed from the same demonstrations | 

docs/misc/create_own_config.md

@@ -1,6 +1,7 @@
 # How to create your own config
 
 You can bring your own config to CRISP, so that you are able to create your own environments, teleoperation setup, controllers...
+
 1. First create a config folder and give it the following structure (you do not need to add all config folders):
 
 ```bash
@@ -30,10 +31,10 @@ export CRISP_CONFIG_PATH=/path/to/my_crisp_configs
 3. Check that the config works.
 
 ```bash
-pixi run python scripts/check_config.py
+crisp-check-config  # Do this inside an environment with crisp_py or crisp_gym installed
 ```
 
-This should output your config if it can loaded properly
+This should output your config if it can be loaded properly
 
 ---
 

include/crisp_controllers/pose_broadcaster.hpp

@@ -64,7 +64,9 @@ class PoseBroadcaster
     {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"};
 
   Eigen::VectorXd q;
-  rclcpp::Time last_publish_time_;
+  
+  rclcpp::Duration publish_elapsed_{0,0};
+  rclcpp::Duration publish_interval_{0,0};
 };
 
 } // namespace crisp_controllers

include/crisp_controllers/twist_broadcaster.hpp

@@ -65,7 +65,9 @@ class TwistBroadcaster
 
   Eigen::VectorXd q;
   Eigen::VectorXd q_dot;
-  rclcpp::Time last_publish_time_;
+  
+  rclcpp::Duration publish_elapsed_{0,0};
+  rclcpp::Duration publish_interval_{0,0};
 };
 
 } // namespace crisp_controllers

src/pose_broadcaster.cpp

@@ -35,7 +35,7 @@ PoseBroadcaster::state_interface_configuration() const {
 
 controller_interface::return_type
 PoseBroadcaster::update(const rclcpp::Time &time,
-                                   const rclcpp::Duration & /*period*/) {
+                                   const rclcpp::Duration &period) {
 
   size_t num_joints = params_.joints.size();
   Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq);
@@ -64,28 +64,27 @@ PoseBroadcaster::update(const rclcpp::Time &time,
       Eigen::Quaterniond(current_pose.rotation());
 
   // Decide whether to publish the pose or not
-  bool should_publish = true;
-  if (params_.publish_frequency > 0.0) {
-    auto time_since_last = time - last_publish_time_;
-    auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
-    should_publish = time_since_last >= min_interval;
-  }
-
+  publish_elapsed_ = publish_elapsed_ + period;
+  bool should_publish = (publish_elapsed_ >= publish_interval_) || 
+                          (publish_interval_.nanoseconds() == 0); 
   if (should_publish && realtime_pose_publisher_)
   {
-    geometry_msgs::msg::PoseStamped pose_msg;
-    pose_msg.header.stamp = time;
-    pose_msg.header.frame_id = params_.base_frame;
-    pose_msg.pose.position.x = current_pose.translation()[0];
-    pose_msg.pose.position.y = current_pose.translation()[1];
-    pose_msg.pose.position.z = current_pose.translation()[2];
-    pose_msg.pose.orientation.x = current_quaternion.x();
-    pose_msg.pose.orientation.y = current_quaternion.y();
-    pose_msg.pose.orientation.z = current_quaternion.z();
-    pose_msg.pose.orientation.w = current_quaternion.w();
-    
-    if (realtime_pose_publisher_->try_publish(pose_msg)) {
-      last_publish_time_ = time;
+    if (realtime_pose_publisher_->trylock()) {
+      auto & pose_msg = realtime_pose_publisher_->msg_;
+      pose_msg.header.stamp = time;
+      pose_msg.header.frame_id = params_.base_frame;
+      pose_msg.pose.position.x = current_pose.translation()[0];
+      pose_msg.pose.position.y = current_pose.translation()[1];
+      pose_msg.pose.position.z = current_pose.translation()[2];
+      pose_msg.pose.orientation.x = current_quaternion.x();
+      pose_msg.pose.orientation.y = current_quaternion.y();
+      pose_msg.pose.orientation.z = current_quaternion.z();
+      pose_msg.pose.orientation.w = current_quaternion.w();
+      realtime_pose_publisher_->unlockAndPublish();
+      
+      publish_elapsed_ = publish_elapsed_ - publish_interval_;
+      // clamp to publish only 1 time even if missed multiple intervals
+      publish_elapsed_ = std::min(publish_elapsed_, publish_interval_);      
     }
   }
 
@@ -170,12 +169,19 @@ CallbackReturn PoseBroadcaster::on_configure(
       std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>(
         pose_publisher_);
 
-  last_publish_time_ = this->get_node()->now();
+  if (params_.publish_frequency > 0.0) {
+    publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
+  } else {
+    publish_interval_ = rclcpp::Duration(0, 0);  // publish every cycle
+  }
+  
   return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn PoseBroadcaster::on_activate(
     const rclcpp_lifecycle::State & /*previous_state*/) {
+  // reset publish time accumulation
+  publish_elapsed_ = rclcpp::Duration(0, 0); 
   return CallbackReturn::SUCCESS;
 }
 

src/twist_broadcaster.cpp

@@ -36,7 +36,7 @@ TwistBroadcaster::state_interface_configuration() const {
 
 controller_interface::return_type
 TwistBroadcaster::update(const rclcpp::Time &time,
-                         const rclcpp::Duration & /*period*/) {
+                         const rclcpp::Duration &period) {
 
     size_t num_joints = params_.joints.size();
     Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq);
@@ -69,27 +69,27 @@ TwistBroadcaster::update(const rclcpp::Time &time,
     auto current_velocity = pinocchio::getFrameVelocity(model_, data_, end_effector_frame_id);
 
     // Decide whether to publish the twist or not
-    bool should_publish = true;
-    if (params_.publish_frequency > 0.0) {
-        auto time_since_last = time - last_publish_time_;
-        auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
-        should_publish = time_since_last >= min_interval;
-    }
+    publish_elapsed_ = publish_elapsed_ + period;
+    bool should_publish = (publish_elapsed_ >= publish_interval_) || 
+                            (publish_interval_.nanoseconds() == 0); 
 
     if (should_publish && realtime_twist_publisher_)
     {
-        geometry_msgs::msg::TwistStamped twist_msg;
-        twist_msg.header.stamp = time;
-        twist_msg.header.frame_id = params_.end_effector_frame;
-        twist_msg.twist.linear.x = current_velocity.linear()[0];
-        twist_msg.twist.linear.y = current_velocity.linear()[1];
-        twist_msg.twist.linear.z = current_velocity.linear()[2];
-        twist_msg.twist.angular.x = current_velocity.angular()[0];
-        twist_msg.twist.angular.y = current_velocity.angular()[1];
-        twist_msg.twist.angular.z = current_velocity.angular()[2];
-        
-        if (realtime_twist_publisher_->try_publish(twist_msg)) {
-            last_publish_time_ = time;
+        if (realtime_twist_publisher_->trylock()) {
+            auto & twist_msg = realtime_twist_publisher_->msg_;
+            twist_msg.header.stamp = time;
+            twist_msg.header.frame_id = params_.end_effector_frame;
+            twist_msg.twist.linear.x = current_velocity.linear()[0];
+            twist_msg.twist.linear.y = current_velocity.linear()[1];
+            twist_msg.twist.linear.z = current_velocity.linear()[2];
+            twist_msg.twist.angular.x = current_velocity.angular()[0];
+            twist_msg.twist.angular.y = current_velocity.angular()[1];
+            twist_msg.twist.angular.z = current_velocity.angular()[2];
+            realtime_twist_publisher_->unlockAndPublish();
+            
+            publish_elapsed_ = publish_elapsed_ - publish_interval_;
+            // clamp to publish only 1 time even if missed multiple intervals
+            publish_elapsed_ = std::min(publish_elapsed_, publish_interval_);  
         }
     }
 
@@ -175,12 +175,19 @@ CallbackReturn TwistBroadcaster::on_configure(
         std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped>>(
             twist_publisher_);
 
-    last_publish_time_ = this->get_node()->now();
+    if (params_.publish_frequency > 0.0) {
+        publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency);
+    } else {
+        publish_interval_ = rclcpp::Duration(0, 0);  // publish every cycle
+    }
+
     return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn TwistBroadcaster::on_activate(
     const rclcpp_lifecycle::State & /*previous_state*/) {
+    // reset publish time accumulation
+    publish_elapsed_ = rclcpp::Duration(0, 0); 
     return CallbackReturn::SUCCESS;
 }