From 98f326568c52c203002cce8fba9350348d09e8ae Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Fri, 6 Jul 2018 11:54:47 -0500 Subject: [PATCH 1/5] Apply low-pass filter to force output --- hebiros_gazebo_plugin/include/hebiros_gazebo_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hebiros_gazebo_plugin/include/hebiros_gazebo_controller.cpp b/hebiros_gazebo_plugin/include/hebiros_gazebo_controller.cpp index a31c3f7..9f299c6 100644 --- a/hebiros_gazebo_plugin/include/hebiros_gazebo_controller.cpp +++ b/hebiros_gazebo_plugin/include/hebiros_gazebo_controller.cpp @@ -366,9 +366,9 @@ double HebirosGazeboController::ComputeForce(std::shared_ptr hebiros_joint->temperature.update(power_in, iteration_time.toSec()); hebiros_joint->temperature_safety.update(hebiros_joint->temperature.getMotorWindingTemperature()); - //alpha = hebiros_joint->low_pass_alpha; - //force = (force * alpha) + hebiros_joint->prev_force * (1 - alpha); - //hebiros_joint->prev_force = force; + alpha = hebiros_joint->low_pass_alpha; + force = (force * alpha) + hebiros_joint->prev_force * (1 - alpha); + hebiros_joint->prev_force = force; return force; } From 4080881825cb236e66b4f0f2ff4c061934684bfe Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Fri, 6 Jul 2018 11:56:03 -0500 Subject: [PATCH 2/5] Use GetForce instead of GetForceTorque. GetForce simply returns the previous SetForce value. --- hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp index 27213c1..924b0ab 100644 --- a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp +++ b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp @@ -68,9 +68,7 @@ void HebirosGazeboPlugin::UpdateGroup(std::shared_ptr hebiro joint->SetProvideFeedback(true); double position = joint->GetAngle(0).Radian(); double velocity = joint->GetVelocity(0); - physics::JointWrench wrench = joint->GetForceTorque(0); - auto trans = joint->GetChild()->GetInitialRelativePose().rot; - double effort = (-1 * (trans * wrench.body1Torque)).z; + double effort = joint->GetForce(0); hebiros_group->feedback.position[i] = position; hebiros_group->feedback.velocity[i] = velocity; From cfe9695b89a9cd9e3f58a0e3130418d5813b8964 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Fri, 6 Jul 2018 11:57:07 -0500 Subject: [PATCH 3/5] Some (commented) experimental code for future reference. --- hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp index 924b0ab..caf2069 100644 --- a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp +++ b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp @@ -68,6 +68,10 @@ void HebirosGazeboPlugin::UpdateGroup(std::shared_ptr hebiro joint->SetProvideFeedback(true); double position = joint->GetAngle(0).Radian(); double velocity = joint->GetVelocity(0); + //physics::JointWrench wrench = joint->GetForceTorque(0u); // Net torque (motor + environment). Non-zero values represent transient effects. + //double effort = -1*wrench.body2Torque.z; // reactive torque from motor + //effort = std::max(-20.0, std::min(20.0, effort)); + //effort += joint->GetForce(0); // torque from motor double effort = joint->GetForce(0); hebiros_group->feedback.position[i] = position; From affc60b97525b87230d9578a5b2263a4c7897995 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Fri, 6 Jul 2018 11:57:51 -0500 Subject: [PATCH 4/5] Apply low-pass filter to Effort Feedback before Controller --- hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp index caf2069..1dc6639 100644 --- a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp +++ b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp @@ -24,6 +24,9 @@ void HebirosGazeboPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { this->update_connection = event::Events::ConnectWorldUpdateBegin ( boost::bind(&HebirosGazeboPlugin::OnUpdate, this, _1)); + this->prev_effort = 0.0; + this->low_pass_alpha = 0.1; + ROS_INFO("Loaded hebiros gazebo plugin"); } @@ -73,6 +76,9 @@ void HebirosGazeboPlugin::UpdateGroup(std::shared_ptr hebiro //effort = std::max(-20.0, std::min(20.0, effort)); //effort += joint->GetForce(0); // torque from motor double effort = joint->GetForce(0); + double alpha = this->low_pass_alpha; + effort = (effort * alpha) + this->prev_effort * (1 - alpha); + this->prev_effort = effort; hebiros_group->feedback.position[i] = position; hebiros_group->feedback.velocity[i] = velocity; From 49a1d7fee0caa3d34a9ae718ec9c68d8aee68ef8 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Fri, 6 Jul 2018 14:51:36 -0500 Subject: [PATCH 5/5] Add missing variables to header file --- hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h index 6ec767e..16a2fdd 100644 --- a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h +++ b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h @@ -48,6 +48,9 @@ class HebirosGazeboPlugin: public ModelPlugin { void AddJointToGroup(std::shared_ptr hebiros_group, std::string joint_name); void UpdateGroup(std::shared_ptr hebiros_group, const ros::Duration& iteration_time); + double prev_effort; + double low_pass_alpha; + bool SrvAddGroup(AddGroupFromNamesSrv::Request &req, AddGroupFromNamesSrv::Response &res);