From 8c4550ee7ca20dbd752c60b6867ca39007807648 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Mon, 7 Jul 2014 20:52:37 +0300 Subject: [PATCH] Added maxEffort parameter to MimicJoint plugin --- README.md | 5 +++++ include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h | 2 +- src/mimic_joint_plugin.cpp | 8 ++++++++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 18c48a3..d8e820b 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,11 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun - offset A **double** specifying the offset parameter of the mimic joint. Defaults to 0.0. + + - maxEffort + + A **double** specifying the max effort the mimic joint can generate. Defaults to 1.0. + - sensitiveness A **double** specifying the sensitiveness of the mimic joint. Defaults to 0.0. It basically is the threshold of the difference between the 2 angles (joint's and mimic's) before applying the "mimicness". diff --git a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h index 08f8b55..92e63db 100644 --- a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h +++ b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h @@ -52,7 +52,7 @@ namespace gazebo private: // Parameters string joint_name_, mimic_joint_name_; - double multiplier_, offset_, sensitiveness_; + double multiplier_, offset_, sensitiveness_, max_effort_; bool kill_sim; diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp index b991463..2846cb7 100644 --- a/src/mimic_joint_plugin.cpp +++ b/src/mimic_joint_plugin.cpp @@ -77,6 +77,11 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) if (_sdf->HasElement("sensitiveness")) sensitiveness_ = _sdf->GetElement("sensitiveness")->Get(); + // Check for max effort + max_effort_ = 1.0; + if (_sdf->HasElement("maxEffort")) + max_effort_ = _sdf->GetElement("maxEffort")->Get(); + // Get pointers to joints joint_ = model_->GetJoint(joint_name_); if(!joint_) @@ -90,6 +95,9 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) ROS_ERROR("No (mimic) joint named %s. MimicJointPlugin could not be loaded.", mimic_joint_name_.c_str()); return; } + + // Set max effort + mimic_joint_->SetMaxForce(0,max_effort_); // Listen to the update event. This event is broadcast every // simulation iteration. -- 2.12.0