Skip to content

Commit 01e6f68

Browse files
livanov93livanov93
committed
Move all control to force - based output.
1 parent 25187ea commit 01e6f68

File tree

1 file changed

+103
-56
lines changed

1 file changed

+103
-56
lines changed

‎ign_ros2_control/src/ign_system.cpp‎

Lines changed: 103 additions & 56 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,8 @@
3030
#include <ignition/gazebo/components/JointPosition.hh>
3131
#include <ignition/gazebo/components/JointVelocity.hh>
3232
#include <ignition/gazebo/components/JointVelocityCmd.hh>
33+
#include <ignition/gazebo/components/JointVelocityReset.hh>
34+
3335
#include <ignition/gazebo/components/JointAxis.hh>
3436
#include <ignition/gazebo/components/JointType.hh>
3537
#include <ignition/gazebo/components/Joint.hh>
@@ -66,9 +68,11 @@ struct jointData
6668

6769
/// \brief Current cmd joint position
6870
double joint_position_cmd;
71+
double joint_position_to_force_cmd;
6972

7073
/// \brief Current cmd joint velocity
7174
double joint_velocity_cmd;
75+
double joint_velocity_to_force_cmd;
7276

7377
/// \brief Current cmd joint effort
7478
double joint_effort_cmd;
@@ -247,10 +251,21 @@ bool IgnitionSystem::initSim(
247251
_ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce());
248252
}
249253

254+
const auto * jointAxis =
255+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
256+
this->dataPtr->joints_[j].sim_joint);
257+
258+
double upper = jointAxis->Data().Upper();
259+
double lower = jointAxis->Data().Lower();
260+
double max_velocity = jointAxis->Data().MaxVelocity();
261+
double max_effort = jointAxis->Data().Effort();
262+
263+
double dummy_guess_p_pos = 10 * max_velocity / abs(upper - lower);
264+
250265
// PID parameters
251266
double p_gain_pos =
252267
(hardware_info.joints[j].parameters.find("p_pos") ==
253-
hardware_info.joints[j].parameters.end() ) ? 100.0 : stod(
268+
hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos : stod(
254269
hardware_info.joints[j].parameters.at(
255270
"p_pos"));
256271
double i_gain_pos =
@@ -260,28 +275,28 @@ bool IgnitionSystem::initSim(
260275
"i_pos"));
261276
double d_gain_pos =
262277
(hardware_info.joints[j].parameters.find("d_pos") ==
263-
hardware_info.joints[j].parameters.end() ) ? 5.0 : stod(
278+
hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 100.0 : stod(
264279
hardware_info.joints[j].parameters.at(
265280
"d_pos"));
266281
// set integral max and min component to 50 percent of the max effort
267282
double iMax_pos =
268283
(hardware_info.joints[j].parameters.find("iMax_pos") ==
269-
hardware_info.joints[j].parameters.end() ) ? 500.0 : stod(
284+
hardware_info.joints[j].parameters.end() ) ? 0.0 : stod(
270285
hardware_info.joints[j].parameters.at(
271286
"iMax_pos"));
272287
double iMin_pos =
273288
(hardware_info.joints[j].parameters.find("iMin_pos") ==
274-
hardware_info.joints[j].parameters.end() ) ? -500 : stod(
289+
hardware_info.joints[j].parameters.end() ) ? 0.0 : stod(
275290
hardware_info.joints[j].parameters.at(
276291
"iMin_pos"));
277292
double cmdMax_pos =
278293
(hardware_info.joints[j].parameters.find("cmdMax_pos") ==
279-
hardware_info.joints[j].parameters.end() ) ? 1000.0 : stod(
294+
hardware_info.joints[j].parameters.end() ) ? max_velocity : stod(
280295
hardware_info.joints[j].parameters.at(
281296
"cmdMax_pos"));
282297
double cmdMin_pos =
283298
(hardware_info.joints[j].parameters.find("cmdMin_pos") ==
284-
hardware_info.joints[j].parameters.end() ) ? -1000.0 : stod(
299+
hardware_info.joints[j].parameters.end() ) ? -1.0 * max_velocity : stod(
285300
hardware_info.joints[j].parameters.at(
286301
"cmdMin_pos"));
287302
double cmdOffset_pos =
@@ -305,38 +320,38 @@ bool IgnitionSystem::initSim(
305320

306321
double p_gain_vel =
307322
(hardware_info.joints[j].parameters.find("p_vel") ==
308-
hardware_info.joints[j].parameters.end() ) ? 100.0 : stod(
323+
hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 100.0 : stod(
309324
hardware_info.joints[j].parameters.at(
310325
"p_vel"));
311326
double i_gain_vel =
312327
(hardware_info.joints[j].parameters.find("i_vel") ==
313-
hardware_info.joints[j].parameters.end() ) ? 0.0 : stod(
328+
hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 1000.0 : stod(
314329
hardware_info.joints[j].parameters.at(
315330
"i_vel"));
316331
double d_gain_vel =
317332
(hardware_info.joints[j].parameters.find("d_vel") ==
318-
hardware_info.joints[j].parameters.end() ) ? 5.0 : stod(
333+
hardware_info.joints[j].parameters.end() ) ? 0.0 : stod(
319334
hardware_info.joints[j].parameters.at(
320335
"d_vel"));
321336
// set integral max and min component to 50 percent of the max effort
322337
double iMax_vel =
323338
(hardware_info.joints[j].parameters.find("iMax_vel") ==
324-
hardware_info.joints[j].parameters.end() ) ? 500.0 : stod(
339+
hardware_info.joints[j].parameters.end() ) ? max_effort / 2.0 : stod(
325340
hardware_info.joints[j].parameters.at(
326341
"iMax_vel"));
327342
double iMin_vel =
328343
(hardware_info.joints[j].parameters.find("iMin_vel") ==
329-
hardware_info.joints[j].parameters.end() ) ? -500 : stod(
344+
hardware_info.joints[j].parameters.end() ) ? -1.0 * max_effort / 2.0 : stod(
330345
hardware_info.joints[j].parameters.at(
331346
"iMin_vel"));
332347
double cmdMax_vel =
333348
(hardware_info.joints[j].parameters.find("cmdMax_vel") ==
334-
hardware_info.joints[j].parameters.end() ) ? 1000.0 : stod(
349+
hardware_info.joints[j].parameters.end() ) ? max_effort : stod(
335350
hardware_info.joints[j].parameters.at(
336351
"cmdMax_vel"));
337352
double cmdMin_vel =
338353
(hardware_info.joints[j].parameters.find("cmdMin_vel") ==
339-
hardware_info.joints[j].parameters.end() ) ? -1000.0 : stod(
354+
hardware_info.joints[j].parameters.end() ) ? -1.0 * max_effort : stod(
340355
hardware_info.joints[j].parameters.at(
341356
"cmdMin_vel"));
342357
double cmdOffset_vel =
@@ -814,19 +829,31 @@ hardware_interface::return_type IgnitionSystem::write(
814829
name].cmdOffset_vel);
815830

816831
if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) {
817-
if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
818-
this->dataPtr->joints_[i].sim_joint))
819-
{
832+
833+
double velocity_error = this->dataPtr->joints_[i].joint_velocity - std::clamp(
834+
this->dataPtr->joints_[i].joint_velocity_cmd, -1.0 * jointAxis->Data().MaxVelocity(),
835+
jointAxis->Data().MaxVelocity());
836+
837+
// calculate target force/torque - output of inner pid
838+
double target_force = this->dataPtr->joints_[i].pid_vel.Update(
839+
velocity_error, std::chrono::duration<double>(
840+
period.to_chrono<std::chrono::nanoseconds>()));
841+
842+
this->dataPtr->joints_[i].joint_velocity_to_force_cmd = target_force;
843+
844+
auto forceCmd =
845+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
846+
this->dataPtr->joints_[i].sim_joint);
847+
848+
if (forceCmd == nullptr) {
820849
this->dataPtr->ecm->CreateComponent(
821850
this->dataPtr->joints_[i].sim_joint,
822-
ignition::gazebo::components::JointVelocityCmd({0}));
851+
ignition::gazebo::components::JointForceCmd({target_force}));
823852
} else {
824-
const auto jointVelCmd =
825-
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
826-
this->dataPtr->joints_[i].sim_joint);
827-
*jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
828-
{this->dataPtr->joints_[i].joint_velocity_cmd});
853+
*forceCmd = ignition::gazebo::components::JointForceCmd(
854+
{target_force});
829855
}
856+
830857
} else if (this->dataPtr->joints_[i].joint_control_method & POSITION) {
831858

832859
// calculate error with clamped position command
@@ -855,6 +882,8 @@ hardware_interface::return_type IgnitionSystem::write(
855882
velocity_error, std::chrono::duration<double>(
856883
period.to_chrono<std::chrono::nanoseconds>()));
857884

885+
this->dataPtr->joints_[i].joint_position_to_force_cmd = target_force;
886+
858887
auto forceCmd =
859888
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
860889
this->dataPtr->joints_[i].sim_joint);
@@ -892,49 +921,51 @@ hardware_interface::return_type IgnitionSystem::write(
892921
this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
893922
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
894923

895-
const auto * jointAxisMimicked =
896-
this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
897-
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint);
898-
899924
if (mimic_interface == "position") {
900925
// Get the joint position
901-
const auto * jp_mimicked =
926+
double position_mimicked_joint =
902927
this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
903-
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint);
904-
double position_mimicked_joint = jp_mimicked->Data()[0];
928+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
905929

906-
const auto * jp_mimic =
930+
double position_mimic_joint =
907931
this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
908-
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
909-
double position_mimic_joint = jp_mimic->Data()[0];
932+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
910933

911-
// calculate error with clamped position command
912-
double position_error = position_mimic_joint - position_mimicked_joint *
913-
mimic_joint.multiplier;
934+
double position_error = position_mimic_joint - std::clamp(
935+
position_mimicked_joint *
936+
mimic_joint.multiplier, jointAxis->Data().Lower(),
937+
jointAxis->Data().Upper());
914938

915-
double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
939+
position_error =
940+
copysign(
941+
1.0,
942+
position_error) *
943+
std::clamp(
944+
std::abs(position_error), 0.0,
945+
std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()));
916946

917947
// calculate target velocity - output of outer pid - input to inner pid
918948
double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update(
919949
position_error, std::chrono::duration<double>(
920950
period.to_chrono<std::chrono::nanoseconds>()));
921951

922-
const auto * jv_mimic =
952+
// get mimic joint velocity
953+
double velocity_mimic_joint =
923954
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
924-
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
925-
double velocity_mimic_joint = jv_mimic->Data()[0];
955+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
926956

927-
double velocity_error = velocity_mimic_joint - target_vel;
957+
double velocity_error = velocity_mimic_joint - std::clamp(
958+
target_vel, -1.0 * jointAxis->Data().MaxVelocity(),
959+
jointAxis->Data().MaxVelocity());
928960

961+
this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
962+
mimic_joint.multiplier *
963+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_to_force_cmd);
929964
// calculate target force/torque - output of inner pid
930965
double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update(
931966
velocity_error, std::chrono::duration<double>(
932967
period.to_chrono<std::chrono::nanoseconds>()));
933968

934-
igndbg << "[" << this->dataPtr->joints_[mimic_joint.joint_index].name << "]\t" <<
935-
position_mimicked_joint << " \t" << position_mimic_joint << " \t" << target_force <<
936-
"\n";
937-
938969
auto forceCmd =
939970
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
940971
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
@@ -948,25 +979,41 @@ hardware_interface::return_type IgnitionSystem::write(
948979
{target_force});
949980
}
950981
}
951-
/*
952982
if (mimic_interface == "velocity") {
953983
// get the velocity of mimicked joint
954984
double velocity_mimicked_joint =
955985
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
956986
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
957987

958-
if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
959-
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
960-
{
988+
// get mimic joint velocity
989+
double velocity_mimic_joint =
990+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
991+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
992+
993+
double velocity_error = velocity_mimic_joint - std::clamp(
994+
mimic_joint.multiplier * velocity_mimicked_joint, -1.0 * jointAxis->Data().MaxVelocity(),
995+
jointAxis->Data().MaxVelocity());
996+
997+
this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
998+
mimic_joint.multiplier *
999+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_to_force_cmd);
1000+
1001+
// calculate target force/torque - output of inner pid
1002+
double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update(
1003+
velocity_error, std::chrono::duration<double>(
1004+
period.to_chrono<std::chrono::nanoseconds>()));
1005+
1006+
auto forceCmd =
1007+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
1008+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
1009+
1010+
if (forceCmd == nullptr) {
9611011
this->dataPtr->ecm->CreateComponent(
9621012
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
963-
ignition::gazebo::components::JointVelocityCmd({0}));
1013+
ignition::gazebo::components::JointForceCmd({target_force}));
9641014
} else {
965-
const auto jointVelCmd =
966-
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
967-
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
968-
*jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
969-
{mimic_joint.multiplier * velocity_mimicked_joint});
1015+
*forceCmd = ignition::gazebo::components::JointForceCmd(
1016+
{target_force});
9701017
}
9711018
}
9721019
if (mimic_interface == "effort") {
@@ -987,9 +1034,9 @@ hardware_interface::return_type IgnitionSystem::write(
9871034
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
9881035
*jointEffortCmd = ignition::gazebo::components::JointForceCmd(
9891036
{mimic_joint.multiplier *
990-
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort});
1037+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd});
9911038
}
992-
}*/
1039+
}
9931040
}
9941041
}
9951042

0 commit comments

Comments
 (0)