Commit c15c2110 authored by Noëlie Ramuzat's avatar Noëlie Ramuzat

Fix bug for initJoints failure in EFFORT

parent d5f27702
Pipeline #2912 failed with stage
in 8 seconds
......@@ -626,6 +626,7 @@ namespace sot_controller
{
bool notok=true;
SotControlMode lcontrol_mode = control_mode_;
bool failure=false;
while (notok)
{
......@@ -651,6 +652,7 @@ namespace sot_controller
}
catch (...)
{
failure=true;
ROS_ERROR_STREAM("Could not find joint "
<< joints_name_[i]);
if (lcontrol_mode==POSITION)
......@@ -663,7 +665,8 @@ namespace sot_controller
else if (lcontrol_mode==EFFORT)
lcontrol_mode = POSITION;
}
desired_init_pose_[i] = joints_[i].getPosition();
if (!failure)
desired_init_pose_[i] = joints_[i].getPosition();
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment