Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Cartesian goal conversion #24

Open
wants to merge 2 commits into
base: admittance-robot-description-param
Choose a base branch
from
Open
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
[WIP] convert PoseStamped goal to JointTrajectoryPoint on msg arrival
Nibanovic committed Aug 27, 2024
commit 4ee139ddc95b3e42d7191979b41a0828a1f41af2
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"