feat: accept frame transform from non-base frame

CHANGELOG.md

@@ -9,6 +9,9 @@ Release Versions:
 - [6.3.0](#630)
 - [6.2.0](#620)
 
+## Upcoming changes (in development)
+- feat(robot_model): accept frame transform from non-base frame (#145)
+
 ## 7.2.0
 
 Version 7.2.0 contains improvements for the Python bindings of control libraries.

VERSION

@@ -1 +1 @@
-7.2.0
+7.2.1

demos/CMakeLists.txt

@@ -15,7 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(control_libraries 7.2.0 CONFIG REQUIRED)
+find_package(control_libraries 7.2.1 CONFIG REQUIRED)
 
 set(DEMOS_SCRIPTS
   task_space_control_loop

doxygen/doxygen.conf

@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 7.2.0
+PROJECT_NUMBER = 7.2.1
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

protocol/clproto_cpp/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(clproto VERSION 7.2.0)
+project(clproto VERSION 7.2.1)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

python/setup.py

@@ -11,7 +11,7 @@
 # names of the environment variables that define osqp and openrobots include directories
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 
-__version__ = "7.2.0"
+__version__ = "7.2.1"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

source/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 7.2.0)
+project(control_libraries VERSION 7.2.1)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

source/robot_model/include/robot_model/Model.hpp

@@ -122,19 +122,23 @@ class Model {
    * @brief Compute the forward kinematics, i.e. the pose of certain frames from the joint positions
    * @param joint_positions the joint state of the robot
    * @param frame_ids ids of the frames at which to extract the pose
+   * @param ref_frame_id id of the reference frame
    * @return the desired poses
    */
   std::vector<state_representation::CartesianPose> forward_kinematics(const state_representation::JointPositions& joint_positions,
-                                                                      const std::vector<unsigned int>& frame_ids);
+                                                                      const std::vector<unsigned int>& frame_ids,
+                                                                      const unsigned int ref_frame_id = 0);
 
   /**
    * @brief Compute the forward kinematics, i.e. the pose of certain frames from the joint positions for a single frame
    * @param joint_positions the joint state of the robot
    * @param frame_id id of the frames at which to extract the pose
+   * @param ref_frame_id id of the reference frame
    * @return the desired pose
    */
   state_representation::CartesianPose forward_kinematics(const state_representation::JointPositions& joint_positions,
-                                                         unsigned int frame_id);
+                                                         const unsigned int frame_id,
+                                                         const unsigned int ref_frame_id = 0);
 
   /**
    * @brief Check if the vector's elements are inside the parameter limits

source/robot_model/src/Model.cpp

@@ -221,17 +221,36 @@ Model::compute_gravity_torques(const state_representation::JointPositions& joint
 }
 
 state_representation::CartesianPose Model::forward_kinematics(const state_representation::JointPositions& joint_positions,
-                                                              unsigned int frame_id) {
-  return this->forward_kinematics(joint_positions, std::vector<unsigned int>{frame_id}).front();
+                                                              const unsigned int frame_id,
+                                                              const unsigned int ref_frame_id) {
+  return this->forward_kinematics(joint_positions, std::vector<unsigned int>{frame_id}, ref_frame_id).front();
 }
 
 std::vector<state_representation::CartesianPose> Model::forward_kinematics(const state_representation::JointPositions& joint_positions,
-                                                                           const std::vector<unsigned int>& frame_ids) {
+                                                                           const std::vector<unsigned int>& frame_ids,
+                                                                           const unsigned int ref_frame_id) {
   if (joint_positions.get_size() != this->get_number_of_joints()) {
     throw (exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints()));
   }
   std::vector<state_representation::CartesianPose> pose_vector;
   pinocchio::forwardKinematics(this->robot_model_, this->robot_data_, joint_positions.data());
+
+  if (ref_frame_id >= static_cast<unsigned int>(this->robot_model_.nframes)) {
+    throw (exceptions::FrameNotFoundException(std::to_string(ref_frame_id)));
+    }
+  state_representation::CartesianPose ref_frame_pose = state_representation::CartesianPose::Identity(
+                                                        this->robot_model_.frames[ref_frame_id].name);
+  if (ref_frame_id > 0) {
+    pinocchio::updateFramePlacement(this->robot_model_, this->robot_data_, ref_frame_id);
+    pinocchio::SE3 ref_pose = this->robot_data_.oMf[ref_frame_id];
+    Eigen::Vector3d ref_translation = ref_pose.translation();
+    Eigen::Quaterniond ref_quaternion;
+    pinocchio::quaternion::assignQuaternion(ref_quaternion, ref_pose.rotation());
+    ref_frame_pose = state_representation::CartesianPose(this->robot_model_.frames[ref_frame_id].name,
+                                                    ref_translation,
+                                                    ref_quaternion,
+                                                    this->get_base_frame());
+  }
   for (unsigned int id : frame_ids) {
     if (id >= static_cast<unsigned int>(this->robot_model_.nframes)) {
       throw (exceptions::FrameNotFoundException(std::to_string(id)));
@@ -245,6 +264,9 @@ std::vector<state_representation::CartesianPose> Model::forward_kinematics(const
                                                    translation,
                                                    quaternion,
                                                    this->get_base_frame());
+    if (ref_frame_id > 0) {
+      frame_pose = ref_frame_pose.inverse() * frame_pose;
+    }
     pose_vector.push_back(frame_pose);
   }
   return pose_vector;