Skip to content

Commit

Permalink
[WIP] convert PoseStamped goal to JointTrajectoryPoint on msg arrival
Browse files Browse the repository at this point in the history
Nibanovic committed Aug 27, 2024
1 parent 5af2373 commit 4ee139d
Showing 4 changed files with 110 additions and 3 deletions.
Original file line number Diff line number Diff line change
@@ -143,6 +143,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
// ROS messages
std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
std::shared_ptr<geometry_msgs::msg::PoseStamped> goal_pose_msg_;
geometry_msgs::msg::PoseStamped last_goal_pose_;

// real-time buffer
realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>>
@@ -159,9 +160,17 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
// joint_state_: current joint readings from the hardware
// reference_admittance_: reference value used by the controller after the admittance values are
// applied ft_values_: values read from the force torque sensor
trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_;
trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_, joint_space_goal_;
geometry_msgs::msg::Wrench ft_values_;

/**
* @brief Check whether two poses differ within a certain threshold.
*/
bool is_same_pose(
const geometry_msgs::msg::Pose & a,
const geometry_msgs::msg::Pose & b,
double eps = 1e-5);

/**
* @brief Read values from hardware interfaces and set corresponding fields of state_current and
* ft_values
Original file line number Diff line number Diff line change
@@ -160,6 +160,17 @@ class AdmittanceRule
* \param[in] current_joint_state message
*/
geometry_msgs::msg::Pose initialize_goal_pose(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state);

/**
* @brief Convert cartesian goal pose to joint-space goal.
* \param[in] input PoseStamped goal expressed as ft_sensor link pose in base_link frame
* \param[in] current_state current joint state
* \param[out] output = current_state + delta_theta required to reach the goal pose
*/
void cartesian_goal_to_joint_space(
const geometry_msgs::msg::PoseStamped & input,
trajectory_msgs::msg::JointTrajectoryPoint & current_state,
trajectory_msgs::msg::JointTrajectoryPoint & output);

public:
// admittance config parameters
Original file line number Diff line number Diff line change
@@ -126,6 +126,58 @@ geometry_msgs::msg::Pose AdmittanceRule::initialize_goal_pose(
return tf2::toMsg(admittance_transforms_.ref_base_ft_);
}

void AdmittanceRule::cartesian_goal_to_joint_space(
const geometry_msgs::msg::PoseStamped & input,
trajectory_msgs::msg::JointTrajectoryPoint & current_state,
trajectory_msgs::msg::JointTrajectoryPoint & output)
{
// calculate difference between `tool0` current and goal pose,
// both expressed in `base_link` frame.
Eigen::Isometry3d goal_base_ft;
tf2::fromMsg(input.pose, goal_base_ft);

Eigen::Matrix<double, 6, 1> delta_X;
delta_X.block<3, 1>(0, 0) =
goal_base_ft.translation() - admittance_transforms_.base_ft_.translation();

const auto R_current = admittance_transforms_.base_ft_.rotation();
const auto R_desired = goal_base_ft.rotation();
const auto R = R_desired * R_current.transpose();
const auto angle_axis = Eigen::AngleAxisd(R);
delta_X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis();


std::cout<<input.header.frame_id<<" delta_X translation:"<<std::endl;
std::cout<<delta_X(0,0)<<std::endl;
std::cout<<delta_X(1,0)<<std::endl;
std::cout<<delta_X(2,0)<<std::endl;

std::cout<<input.header.frame_id<<" delta_X rotation:"<<std::endl;
std::cout<<delta_X(3,0)<<std::endl;
std::cout<<delta_X(4,0)<<std::endl;
std::cout<<delta_X(5,0)<<std::endl;
std::cout<<"-------------"<<std::endl;

Eigen::VectorXd delta_joints = Eigen::VectorXd::Zero(6);
bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(
admittance_state_.current_joint_pos, delta_X, parameters_.control.frame.id, delta_joints);

/**/
if(success){
std::cout<<"VectorXd: "<<std::endl;
for(size_t i = 0; i < 6; i++)
{
std::cout<<delta_joints(i)<<std::endl;
}
}else{
std::cout<<"couldn't convert."<<std::endl;
}


for(size_t i = 0; i < output.positions.size(); i++)
output.positions[i] = current_state.positions[i] + delta_joints(i);
}

void AdmittanceRule::apply_parameters_update()
{
if (parameter_handler_->is_old(parameters_))
39 changes: 37 additions & 2 deletions admittance_controller/src/admittance_controller.cpp
Original file line number Diff line number Diff line change
@@ -360,10 +360,12 @@ controller_interface::CallbackReturn AdmittanceController::on_activate(
RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize goal_pose from current joint positions.\n");
return controller_interface::CallbackReturn::ERROR;
}
last_goal_pose_.pose = goal_pose_msg_->pose;

// Use current joint_state as a default reference
last_reference_ = joint_state_;
last_commanded_ = joint_state_;
joint_space_goal_ = joint_state_;
reference_ = joint_state_;
reference_admittance_ = joint_state_;

@@ -394,11 +396,25 @@ controller_interface::return_type AdmittanceController::update_reference_from_su
}
}

// after initializing goal_pose_msg_, update it from subscribers only
// if another message exists
// if another message exists...
if(*input_goal_pose_.readFromRT())
{
goal_pose_msg_ = *input_goal_pose_.readFromRT();
// ... and its a different goal...
if(is_same_pose(last_goal_pose_.pose, goal_pose_msg_->pose) == false)
{
// ... convert goal to joint-space and apply it.
admittance_->cartesian_goal_to_joint_space(*goal_pose_msg_, joint_state_, joint_space_goal_);
for (size_t i = 0; i < joint_space_goal_.positions.size(); ++i)
{
position_reference_[i].get() = joint_space_goal_.positions[i];
}
for (size_t i = 0; i < joint_space_goal_.velocities.size(); ++i)
{
velocity_reference_[i].get() = joint_space_goal_.velocities[i];
}
}
last_goal_pose_.pose = goal_pose_msg_->pose;
}


@@ -599,6 +615,25 @@ void AdmittanceController::read_state_reference_interfaces(
last_reference_.velocities = state_reference.velocities;
}

bool AdmittanceController::is_same_pose(const geometry_msgs::msg::Pose & a,
const geometry_msgs::msg::Pose & b,
double eps)
{
// compare position
bool pos = std::abs(a.position.x - b.position.x) <= eps &&
std::abs(a.position.y - b.position.y) <= eps &&
std::abs(a.position.z - b.position.z) <= eps;
if (!pos) return false;

// compare quaternion
bool rot = std::abs(a.orientation.w - b.orientation.w) <= eps &&
std::abs(a.orientation.x - b.orientation.x) <= eps &&
std::abs(a.orientation.y - b.orientation.y) <= eps &&
std::abs(a.orientation.z - b.orientation.z) <= eps;

return rot;
}

} // namespace admittance_controller

#include "pluginlib/class_list_macros.hpp"

0 comments on commit 4ee139d

Please sign in to comment.