From 5e6d70c79ebd977377dc20f3e7907c040d5c2d7f Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Feb 2023 12:46:58 +0100 Subject: [PATCH] Reorganize calcs. --- ign_ros2_control/src/ign_system.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index ec8a8eab..3ed2de5a 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -246,10 +246,10 @@ bool IgnitionSystem::initSim( this->dataPtr->ecm->Component( this->dataPtr->joints_[j].sim_joint); // PID parameters - double p_gain = 100.0; - double i_gain = 1.0; - double d_gain = 50.0; - // set integral max and min component to 10 percent of the max effort + double p_gain = 10.0 * jointAxis->Data().Effort(); + double i_gain = 0.0; + double d_gain = 0.0; + // set integral max and min component to 50 percent of the max effort double iMax = std::isnan(jointAxis->Data().Effort()) ? 500.0 : jointAxis->Data().Effort() * 0.5; double iMin = std::isnan(jointAxis->Data().Effort()) ? 500.0 : -jointAxis->Data().Effort() * @@ -710,14 +710,13 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - // error can be maximal 10 percent of the range error = copysign( 1.0, error) * std::clamp( std::abs(error), 0.0, - std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 0.1); + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); // calculate target force/torque double target_force = this->dataPtr->joints_[i].pid.Update( @@ -791,11 +790,10 @@ hardware_interface::return_type IgnitionSystem::write( position_mimicked_joint * mimic_joint.multiplier, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - // error can be maximal 10 percent of the range position_error = copysign(1.0, position_error) * std::clamp( std::abs( position_error), 0.0, std::abs( - jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 0.1); + jointAxis->Data().Upper() - jointAxis->Data().Lower())); // calculate target force/torque double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update(