Commit 4026dd9a authored by Noëlie Ramuzat's avatar Noëlie Ramuzat Committed by Olivier Stasse
Browse files

Fix bug for initJoints failure in EFFORT

parent 6c0059c5
......@@ -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