Skip to content

Commit

Permalink
Add optional transform offset to the cartesian force controller
Browse files Browse the repository at this point in the history
Only update the internal end-effector transform offset when a new pose
has been been received from the dynamic reconfigure server.

The offset affects:
- The Ft sensor reference frame
- The target wrench, only when given from the end-effector link coordinates
  • Loading branch information
captain-yoshi committed Dec 18, 2023
1 parent a80621a commit b8f5c72
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@

// Project
#include <cartesian_controller_base/cartesian_controller_base.h>
#include <cartesian_controller_base/PoseParameterHandle.h>

// ROS
#include <std_srvs/Trigger.h>
Expand Down Expand Up @@ -122,6 +123,14 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
std::string m_ft_sensor_ref_link;
KDL::Frame m_ft_sensor_transform;

/**
* Allow users to specify a transform offset from the end-effector frame. Affects
* FT Sensor reference frame as well as the target wrench (m_hand_frame_control = True)
*/
KDL::Frame m_end_effector_transform_offset; // only used for target wrench when m_hand_frame_control = True

cartesian_controller_base::PoseParameterHandle m_pose_parameter_handle;

/**
* Allow users to choose whether to specify their target wrenches in the
* end-effector frame (= True) or the base frame (= False). The first one
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
}

// Make sure sensor wrenches are interpreted correctly
setFtSensorReferenceFrame(Base::m_end_effector_link);
setFtSensorReferenceFrame(Base::m_end_effector_link,m_end_effector_transform_offset);

m_signal_taring_server = nh.advertiseService("signal_taring",&CartesianForceController<HardwareInterface>::signalTaringCallback,this);
m_target_wrench_subscriber = nh.subscribe("target_wrench",2,&CartesianForceController<HardwareInterface>::targetWrenchCallback,this);
Expand Down Expand Up @@ -113,6 +113,12 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
m_target_wrench.setZero();
m_ft_sensor_wrench.setZero();


// Initialize spatial pose for end effector transform offset
std::string spatial_pose_config = nh.getNamespace() + "/end_effector_transform_offset";
m_pose_parameter_handle.init(spatial_pose_config);


// Connect dynamic reconfigure and overwrite the default values with values
// on the parameter server. This is done automatically if parameters with
// the according names exist.
Expand Down Expand Up @@ -175,18 +181,27 @@ template <class HardwareInterface>
ctrl::Vector6D CartesianForceController<HardwareInterface>::
computeForceError()
{
// Update end effector transform offset is updated through dynamic reconfigure
if(m_pose_parameter_handle.has_new_pose())
{
m_end_effector_transform_offset = m_pose_parameter_handle.get_pose();

// Reset FTS Reference frame
setFtSensorReferenceFrame(Base::m_end_effector_link,m_end_effector_transform_offset);
}

ctrl::Vector6D target_wrench;
if (m_hand_frame_control) // Assume end-effector frame by convention
{
target_wrench = Base::displayInBaseLink(m_target_wrench,Base::m_end_effector_link);
target_wrench = Base::displayInBaseLink(m_target_wrench,Base::m_end_effector_link,m_end_effector_transform_offset);
}
else // Default to robot base frame
{
target_wrench = m_target_wrench;
}

// Superimpose target wrench and sensor wrench in base frame
return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref)
return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref,m_end_effector_transform_offset)
+ target_wrench
+ compensateGravity();
}
Expand Down

0 comments on commit b8f5c72

Please sign in to comment.