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.

Sign up for GitHub

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

F fix j0 pos vel filter effort tau #262

Open
wants to merge 3 commits into
base: noetic-devel
Choose a base branch
from
Open

F fix j0 pos vel filter effort tau #262

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9c0edfe
Fixing J0 (J1 and J2) position and velocity filter
toliver Sep 13, 2021
b6a493a
Read ROS param to set the tau for the effort filter
toliver May 11, 2022
5ec5a3e
Merge branch 'noetic-devel' into F_fix_J0_pos_vel_filter_effort_tau
carebare47 May 13, 2022
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
2 changes: 2 additions & 0 deletions sr_robot_lib/include/sr_robot_lib/sr_joint_motor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,8 @@ struct Joint
sr_math_utils::filters::LowPassFilter pos_filter;
// used to filter the effort
sr_math_utils::filters::LowPassFilter effort_filter;
// used to filter the position and the velocity of a second joint
sr_math_utils::filters::LowPassFilter pos_filter_2;

bool has_actuator;
boost::shared_ptr<SrActuatorWrapper> actuator_wrapper;
Expand Down
11 changes: 11 additions & 0 deletions sr_robot_lib/src/sr_motor_hand_lib.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -150,6 +150,17 @@ namespace shadow_robot
joint.joint_name = joint_names[index];
joint.joint_to_sensor = joint_to_sensors[index];

if ((!joint.joint_to_sensor.calibrate_after_combining_sensors)
&& (joint.joint_to_sensor.joint_to_sensor_vector.size() == 2))
{
joint.pos_filter_2 = sr_math_utils::filters::LowPassFilter(tau);
}

full_param << act_name << "/effort_filter/tau";
this->nodehandle_.template param<float>(full_param.str(), tau, 0.05);
full_param.str("");
joint.effort_filter = sr_math_utils::filters::LowPassFilter(tau);

if (actuator_ids[index] != -1)
{
joint.has_actuator = true;
Expand Down
39 changes: 32 additions & 7 deletions sr_robot_lib/src/sr_motor_robot_lib.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1024,13 +1024,38 @@ namespace shadow_robot
// calibrate the joint and update the position.
calibrate_joint(joint_tmp, status_data);

// filter the position and velocity
pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(actuator->motor_state_.position_unfiltered_,
timestamp);
// reset the position to the filtered value
actuator->state_.position_ = pos_and_velocity.first;
// set the velocity to the filtered velocity
actuator->state_.velocity_ = pos_and_velocity.second;
if ((!joint_tmp->joint_to_sensor.calibrate_after_combining_sensors)
&& (joint_tmp->joint_to_sensor.joint_to_sensor_vector.size() == 2))
{
actuator->motor_state_.filtered_calibrated_position_values_.clear();
actuator->motor_state_.filtered_calibrated_velocity_values_.clear();

// filter the position and velocity
pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(actuator->motor_state_.calibrated_sensor_values_[0],
timestamp);
// set the position to the filtered value
actuator->motor_state_.filtered_calibrated_position_values_.push_back(pos_and_velocity.first);
// set the velocity to the filtered velocity
actuator->motor_state_.filtered_calibrated_velocity_values_.push_back(pos_and_velocity.second);

// filter the position and velocity for the second joint
pos_and_velocity = joint_tmp->pos_filter_2.compute(actuator->motor_state_.calibrated_sensor_values_[1],
timestamp);
// set the position to the filtered value for the second joint
actuator->motor_state_.filtered_calibrated_position_values_.push_back(pos_and_velocity.first);
// set the velocity to the filtered velocity for the second joint
actuator->motor_state_.filtered_calibrated_velocity_values_.push_back(pos_and_velocity.second);
}
else
{
// filter the position and velocity
pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(actuator->motor_state_.position_unfiltered_,
timestamp);
// reset the position to the filtered value
actuator->state_.position_ = pos_and_velocity.first;
// set the velocity to the filtered velocity
actuator->state_.velocity_ = pos_and_velocity.second;
}
}

template<class StatusType, class CommandType>
Expand Down