cb_circular_pivot_motion.hpp

Go to the documentation of this file.

// Copyright 2021 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
/*****************************************************************************************************************
 *
 *   Authors: Pablo Inigo Blasco, Brett Aldrich
 *
 *****************************************************************************************************************/
#pragma once
 
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_listener.h>
#include <cl_moveit2z/components/cp_tf_listener.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include "cb_move_end_effector_trajectory.hpp"
 
using namespace std::chrono_literals;
 
namespace cl_moveit2z
{
// executes a circular trajectory of the tipLink around one axis defined
// by the z-axis of the plaenePivotPose
class CbCircularPivotMotion : public CbMoveEndEffectorTrajectory
{
public:
  std::optional<double> angularSpeed_rad_s_;
  std::optional<double> linearSpeed_m_s_;
 
  // if not specified it would be used the current robot pose
  std::optional<geometry_msgs::msg::Pose> relativeInitialPose_;
 
  CbCircularPivotMotion(std::optional<std::string> tipLink = std::nullopt)
  : CbMoveEndEffectorTrajectory(tipLink)
  {
  }
 
  CbCircularPivotMotion(
    const geometry_msgs::msg::PoseStamped & planePivotPose, double deltaRadians,
    std::optional<std::string> tipLink = std::nullopt)
  : CbMoveEndEffectorTrajectory(tipLink),
    planePivotPose_(planePivotPose),
    deltaRadians_(deltaRadians)
  {
    if (tipLink_) planePivotPose_.header.frame_id = *tipLink;
  }
 
  CbCircularPivotMotion(
    const geometry_msgs::msg::PoseStamped & planePivotPose,
    const geometry_msgs::msg::Pose & relativeInitialPose, double deltaRadians,
    std::optional<std::string> tipLink = std::nullopt)
  : CbMoveEndEffectorTrajectory(tipLink),
    relativeInitialPose_(relativeInitialPose),
    planePivotPose_(planePivotPose),
    deltaRadians_(deltaRadians)
  {
  }
 
  virtual void generateTrajectory() override
  {
    if (!relativeInitialPose_)
    {
      this->computeCurrentEndEffectorPoseRelativeToPivot();
    }
 
    // project offset into the xy-plane
    // get the radius
    double radius = sqrt(
      relativeInitialPose_->position.z * relativeInitialPose_->position.z +
      relativeInitialPose_->position.y * relativeInitialPose_->position.y);
    double initialAngle = atan2(relativeInitialPose_->position.z, relativeInitialPose_->position.y);
 
    double totallineardist = fabs(radius * deltaRadians_);
    double totalangulardist = fabs(deltaRadians_);
 
    // at least 1 sample per centimeter (average)
    // at least 1 sample per ~1.1 degrees (average)
 
    const double RADS_PER_SAMPLE = 0.02;
    const double METERS_PER_SAMPLE = 0.01;
 
    int totalSamplesCount =
      std::max(totallineardist / METERS_PER_SAMPLE, totalangulardist / RADS_PER_SAMPLE);
 
    double linearSecondsPerSample;
    double angularSecondsPerSamples;
    double secondsPerSample;
 
    if (linearSpeed_m_s_)
    {
      linearSecondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_);
    }
    else
    {
      linearSecondsPerSample = std::numeric_limits<double>::max();
    }
 
    if (angularSpeed_rad_s_)
    {
      angularSecondsPerSamples = RADS_PER_SAMPLE / (*angularSpeed_rad_s_);
    }
    else
    {
      angularSecondsPerSamples = std::numeric_limits<double>::max();
    }
 
    if (!linearSpeed_m_s_ && !angularSpeed_rad_s_)
    {
      secondsPerSample = 0.5;
    }
    else
    {
      secondsPerSample = std::min(linearSecondsPerSample, angularSecondsPerSamples);
    }
 
    double currentAngle = initialAngle;
 
    double angleStep = deltaRadians_ / (double)totalSamplesCount;
 
    tf2::Transform tfBasePose;
    tf2::fromMsg(planePivotPose_.pose, tfBasePose);
 
    RCLCPP_INFO_STREAM(
      getLogger(),
      "[" << getName() << "] generated trajectory, total samples: " << totalSamplesCount);
    for (int i = 0; i < totalSamplesCount; i++)
    {
      // relativePose i
      double y = radius * cos(currentAngle);
      double z = radius * sin(currentAngle);
 
      geometry_msgs::msg::Pose relativeCurrentPose;
 
      relativeCurrentPose.position.x = relativeInitialPose_->position.x;
      relativeCurrentPose.position.y = y;
      relativeCurrentPose.position.z = z;
 
      tf2::Quaternion localquat;
      localquat.setEuler(currentAngle, 0, 0);
 
      //relativeCurrentPose.orientation = relativeInitialPose_.orientation;
      //tf2::toMsg(localquat, relativeCurrentPose.orientation);
      relativeCurrentPose.orientation.w = 1;
 
      tf2::Transform tfRelativeCurrentPose;
      tf2::fromMsg(relativeCurrentPose, tfRelativeCurrentPose);
 
      tf2::Transform tfGlobalPose = tfRelativeCurrentPose * tfBasePose;
 
      tfGlobalPose.setRotation(tfGlobalPose.getRotation() * localquat);
 
      geometry_msgs::msg::PoseStamped globalPose;
      tf2::toMsg(tfGlobalPose, globalPose.pose);
      globalPose.header.frame_id = planePivotPose_.header.frame_id;
      globalPose.header.stamp = rclcpp::Time(planePivotPose_.header.stamp) +
                                rclcpp::Duration::from_seconds(i * secondsPerSample);
      RCLCPP_INFO_STREAM(
        getLogger(),
        "[" << getName() << "]" << rclcpp::Time(globalPose.header.stamp).nanoseconds());
 
      this->endEffectorTrajectory_.push_back(globalPose);
      currentAngle += angleStep;
    }
  }
 
  virtual void createMarkers() override
  {
    CbMoveEndEffectorTrajectory::createMarkers();
 
    tf2::Transform localdirection;
    localdirection.setIdentity();
    localdirection.setOrigin(tf2::Vector3(0.12, 0, 0));
    auto frameid = planePivotPose_.header.frame_id;
 
    visualization_msgs::msg::Marker marker;
    marker.header.frame_id = frameid;
    marker.header.stamp = getNode()->now();
    marker.ns = "trajectory";
    marker.id = beahiorMarkers_.markers.size();
    marker.type = visualization_msgs::msg::Marker::ARROW;
    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.scale.x = 0.01;
    marker.scale.y = 0.02;
    marker.scale.z = 0.02;
    marker.color.a = 1.0;
    marker.color.r = 0.0;
    marker.color.g = 0;
    marker.color.b = 1.0;
 
    geometry_msgs::msg::Point start, end;
    start.x = planePivotPose_.pose.position.x;
    start.y = planePivotPose_.pose.position.y;
    start.z = planePivotPose_.pose.position.z;
 
    tf2::Transform basetransform;
    tf2::fromMsg(planePivotPose_.pose, basetransform);
    tf2::Transform endarrow = localdirection * basetransform;
 
    end.x = endarrow.getOrigin().x();
    end.y = endarrow.getOrigin().y();
    end.z = endarrow.getOrigin().z();
 
    marker.pose.orientation.w = 1;
    marker.points.push_back(start);
    marker.points.push_back(end);
 
    beahiorMarkers_.markers.push_back(marker);
  }
 
protected:
  // the rotation axis (z-axis) and the center of rotation in the space
  geometry_msgs::msg::PoseStamped planePivotPose_;
 
  double deltaRadians_;
 
private:
  void computeCurrentEndEffectorPoseRelativeToPivot()
  {
    // Use CpTfListener component for transform lookups
    CpTfListener * tfListener = nullptr;
    this->requiresComponent(tfListener, smacc2::ComponentRequirement::SOFT);  // Optional component
 
    tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
 
    try
    {
      if (!tipLink_ || *tipLink_ == "")
      {
        tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
      }
 
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << getName() << "] waiting transform, pivot: '"
                         << planePivotPose_.header.frame_id << "' tipLink: '" << *tipLink_ << "'");
 
      if (tfListener != nullptr)
      {
        // Use component-based TF listener (preferred)
        auto transformOpt =
          tfListener->lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time());
 
        if (transformOpt)
        {
          tf2::fromMsg(transformOpt.value(), endEffectorInPivotFrame);
        }
        else
        {
          RCLCPP_ERROR_STREAM(
            getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_
                             << " to " << planePivotPose_.header.frame_id);
          return;
        }
      }
      else
      {
        // Fallback to legacy TF2 usage if component not available
        RCLCPP_WARN_STREAM(
          getLogger(), "[" << getName()
                           << "] CpTfListener component not available, using legacy TF2 (consider "
                              "adding CpTfListener component)");
        tf2_ros::Buffer tfBuffer(getNode()->get_clock());
        tf2_ros::TransformListener tfListenerLegacy(tfBuffer);
 
        tf2::fromMsg(
          tfBuffer.lookupTransform(
            planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
          endEffectorInPivotFrame);
      }
    }
    catch (const std::exception & e)
    {
      RCLCPP_ERROR_STREAM(
        getLogger(),
        "[" << getName()
            << "] Exception in computeCurrentEndEffectorPoseRelativeToPivot: " << e.what());
      return;
    }
 
    // tf2::Transform endEffectorInBaseLinkFrame;
    // tf2::fromMsg(currentRobotEndEffectorPose.pose, endEffectorInBaseLinkFrame);
 
    // tf2::Transform endEffectorInPivotFrame = globalBaseLink * endEffectorInBaseLinkFrame; // pose composition
 
    // now pivot and EndEffector share a common reference frame (let say map)
    // now get the current pose from the pivot reference frame with inverse composition
    tf2::Transform pivotTransform;
    tf2::fromMsg(planePivotPose_.pose, pivotTransform);
    tf2::Transform invertedNewReferenceFrame = pivotTransform.inverse();
 
    tf2::Transform currentPoseRelativeToPivot = invertedNewReferenceFrame * endEffectorInPivotFrame;
 
    geometry_msgs::msg::Pose finalEndEffectorRelativePose;
    tf2::toMsg(currentPoseRelativeToPivot, finalEndEffectorRelativePose);
    relativeInitialPose_ = finalEndEffectorRelativePose;
  }
};
 
}  // namespace cl_moveit2z