Skip to content

Commit 65b7388

Browse files
livanov93livanov93
committed
Add mimic joint vel pid only support.
1 parent ea36469 commit 65b7388

File tree

2 files changed

+139
-8
lines changed

2 files changed

+139
-8
lines changed

‎ign_ros2_control/src/MimicJointSystem.cc‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -263,12 +263,14 @@ void MimicJointSystem::PreUpdate(
263263
}
264264
}
265265

266-
if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity)
266+
if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) {
267267
return;
268+
}
268269

269270

270-
if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity)
271+
if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) {
271272
return;
273+
}
272274

273275

274276
if (_info.paused) {
@@ -329,8 +331,8 @@ void MimicJointSystem::PreUpdate(
329331

330332
// Get error in position
331333
double error = jointPosComp->Data().at(this->dataPtr->jointIndex) -
332-
(mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) *
333-
this->dataPtr->multiplier + this->dataPtr->offset);
334+
(mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) *
335+
this->dataPtr->multiplier + this->dataPtr->offset);
334336

335337
if (fabs(error) > this->dataPtr->deadZone) {
336338

‎ign_ros2_control/src/ign_system.cpp‎

Lines changed: 133 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -705,7 +705,7 @@ hardware_interface::return_type IgnitionSystem::read(
705705
this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0];
706706
this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0];
707707
// set effort state interface to computed/propagated effort command - passthrough because of ignitionrobotics/ign-physics#124
708-
this->dataPtr->joints_[i].joint_effort = this->dataPtr->joints_[i].joint_effort_cmd;
708+
this->dataPtr->joints_[i].joint_effort = this->dataPtr->joints_[i].joint_effort_cmd;
709709
}
710710

711711
for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) {
@@ -845,8 +845,8 @@ hardware_interface::return_type IgnitionSystem::write(
845845
velocity_error, std::chrono::duration<double>(
846846
period.to_chrono<std::chrono::nanoseconds>()));
847847

848-
// remember for potential effort state interface
849-
this->dataPtr->joints_[i].joint_effort_cmd = target_force;
848+
// remember for potential effort state interface
849+
this->dataPtr->joints_[i].joint_effort_cmd = target_force;
850850

851851
auto forceCmd =
852852
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
@@ -886,7 +886,7 @@ hardware_interface::return_type IgnitionSystem::write(
886886

887887
// calculate target force/torque - output of inner pid
888888
double target_force = this->dataPtr->joints_[i].pid_vel.Update(
889-
velocity_error, std::chrono::duration<double>(
889+
position_error, std::chrono::duration<double>(
890890
period.to_chrono<std::chrono::nanoseconds>()));
891891

892892
// remember for potential effort state interface
@@ -921,6 +921,135 @@ hardware_interface::return_type IgnitionSystem::write(
921921
}
922922
}
923923

924+
// set values of all mimic joints with respect to mimicked joint
925+
for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
926+
for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) {
927+
// assuming every mimic joint has axis
928+
const auto * jointAxis =
929+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
930+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
931+
932+
if (mimic_interface == "position") {
933+
// Get the joint position
934+
double position_mimicked_joint =
935+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
936+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
937+
938+
double position_mimic_joint =
939+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
940+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
941+
942+
double position_error = position_mimic_joint - std::clamp(
943+
position_mimicked_joint *
944+
mimic_joint.multiplier, jointAxis->Data().Lower(),
945+
jointAxis->Data().Upper());
946+
947+
position_error =
948+
copysign(
949+
1.0,
950+
position_error) *
951+
std::clamp(
952+
std::abs(position_error), 0.0,
953+
std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()));
954+
955+
// calculate target velocity - output of outer pid - input to inner pid
956+
double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update(
957+
position_error, std::chrono::duration<double>(
958+
period.to_chrono<std::chrono::nanoseconds>()));
959+
960+
// get mimic joint velocity
961+
double velocity_mimic_joint =
962+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
963+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
964+
965+
double velocity_error = velocity_mimic_joint - std::clamp(
966+
target_vel, -1.0 * jointAxis->Data().MaxVelocity(),
967+
jointAxis->Data().MaxVelocity());
968+
969+
this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
970+
mimic_joint.multiplier *
971+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd);
972+
// calculate target force/torque - output of inner pid
973+
double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update(
974+
position_error, std::chrono::duration<double>(
975+
period.to_chrono<std::chrono::nanoseconds>()));
976+
977+
this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = target_force;
978+
979+
auto forceCmd =
980+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
981+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
982+
983+
if (forceCmd == nullptr) {
984+
this->dataPtr->ecm->CreateComponent(
985+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
986+
ignition::gazebo::components::JointForceCmd({target_force}));
987+
} else {
988+
*forceCmd = ignition::gazebo::components::JointForceCmd(
989+
{target_force});
990+
}
991+
}
992+
if (mimic_interface == "velocity") {
993+
// get the velocity of mimicked joint
994+
double velocity_mimicked_joint =
995+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
996+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
997+
998+
// get mimic joint velocity
999+
double velocity_mimic_joint =
1000+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
1001+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
1002+
1003+
double velocity_error = velocity_mimic_joint - std::clamp(
1004+
mimic_joint.multiplier * velocity_mimicked_joint, -1.0 * jointAxis->Data().MaxVelocity(),
1005+
jointAxis->Data().MaxVelocity());
1006+
1007+
this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
1008+
mimic_joint.multiplier *
1009+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd);
1010+
1011+
// calculate target force/torque - output of inner pid
1012+
double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update(
1013+
velocity_error, std::chrono::duration<double>(
1014+
period.to_chrono<std::chrono::nanoseconds>()));
1015+
1016+
auto forceCmd =
1017+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
1018+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
1019+
1020+
if (forceCmd == nullptr) {
1021+
this->dataPtr->ecm->CreateComponent(
1022+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
1023+
ignition::gazebo::components::JointForceCmd({target_force}));
1024+
} else {
1025+
*forceCmd = ignition::gazebo::components::JointForceCmd(
1026+
{target_force});
1027+
}
1028+
}
1029+
if (mimic_interface == "effort") {
1030+
// TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124
1031+
// Get the joint force
1032+
// const auto * jointForce =
1033+
// _ecm.Component<ignition::gazebo::components::JointForce>(
1034+
// this->dataPtr->sim_joints_[j]);
1035+
if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
1036+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
1037+
{
1038+
this->dataPtr->ecm->CreateComponent(
1039+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
1040+
ignition::gazebo::components::JointForceCmd({0}));
1041+
} else {
1042+
const auto jointEffortCmd =
1043+
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
1044+
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
1045+
*jointEffortCmd = ignition::gazebo::components::JointForceCmd(
1046+
{mimic_joint.multiplier *
1047+
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd});
1048+
}
1049+
}
1050+
}
1051+
}
1052+
9241053
return hardware_interface::return_type::OK;
9251054
}
9261055
} // namespace ign_ros2_control

0 commit comments

Comments
 (0)