Skip to content

Commit

Permalink
Reorganize calcs.
Browse files Browse the repository at this point in the history
  • Loading branch information
livanov93 committed Feb 24, 2023
1 parent c018f7c commit cb75106
Showing 1 changed file with 6 additions and 8 deletions.
14 changes: 6 additions & 8 deletions ign_ros2_control/src/ign_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -246,10 +246,10 @@ bool IgnitionSystem::initSim(
this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
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 = 1000.0;
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() *
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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(
Expand Down

0 comments on commit cb75106

Please sign in to comment.