43 #ifndef IIROB_FILTERS_GRAVITY_COMPENSATION_H 44 #define IIROB_FILTERS_GRAVITY_COMPENSATION_H 49 #include <boost/scoped_ptr.hpp> 51 #include <geometry_msgs/WrenchStamped.h> 52 #include <geometry_msgs/Vector3Stamped.h> 56 #include <iirob_filters/GravityCompensationParameters.h> 57 #include <iirob_filters/GravityCompensationConfig.h> 58 #include <dynamic_reconfigure/server.h> 79 virtual bool update(
const T & data_in, T& data_out);
82 iirob_filters::GravityCompensationParameters
params_;
85 geometry_msgs::Vector3Stamped
cog_;
104 template <
typename T>
110 template <
typename T>
115 template <
typename T>
120 ROS_ERROR(
"GravityCompensator did not find param world_frame");
121 if(
params_.sensor_frame ==
" ")
122 ROS_DEBUG(
"GravityCompensator did not find param sensor_frame");
124 ROS_DEBUG(
"GravityCompensator did not find param CoG_x");
126 ROS_DEBUG(
"GravityCompensator did not find param CoG_y");
128 ROS_DEBUG(
"GravityCompensator did not find param CoG_z");
130 ROS_DEBUG(
"GravityCompensator did not find param force");
139 ROS_INFO(
"Gravity Compensation Params: world_frame: %s; sensor_frame: %s; CoG_x:%f; CoG_y:%f; CoG_z:%f; force: %f." ,
world_frame_.c_str(),
sensor_frame_.c_str(),
148 template <
typename T>
165 geometry_msgs::Vector3Stamped temp_force_transformed, temp_torque_transformed, temp_vector_in, temp_vector_out;
167 temp_vector_in.vector = data_in.wrench.force;
170 temp_vector_in.vector = data_in.wrench.torque;
174 geometry_msgs::Vector3Stamped cog_transformed;
178 temp_force_transformed.vector.z +=
force_z_;
180 temp_torque_transformed.vector.x += (
force_z_ * cog_transformed.vector.y);
181 temp_torque_transformed.vector.y -= (
force_z_ * cog_transformed.vector.x);
188 data_out.wrench.force = temp_vector_out.vector;
191 data_out.wrench.torque = temp_vector_out.vector;
195 template <
typename T>
void reconfigureConfigurationRequest(iirob_filters::GravityCompensationConfig &config, uint32_t level)
virtual bool update(const T &data_in, T &data_out)
Update the filter and return the data seperately.
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
geometry_msgs::Vector3Stamped cog_
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
geometry_msgs::TransformStamped transform_
std::string sensor_frame_
tf2_ros::TransformListener * p_tf_Listener
tf2_ros::Buffer * p_tf_Buffer_
~GravityCompensator()
Destructor to clean up.
uint _num_transform_errors
dynamic_reconfigure::Server< iirob_filters::GravityCompensationConfig > reconfigCalibrationSrv_
iirob_filters::GravityCompensationParameters params_
geometry_msgs::TransformStamped transform_back_