void FriExampleAbstract::stopKrlScript(){
//Back to position control
setControlStrategy(10);
//Put 3 in $FRI_FRM_INT[1] to trigger fri_stop()
fri_to_krl.intData[0]=3;
port_fri_to_krl.write(fri_to_krl);
}
void LWRHW::doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list)
{
// at this point, we now that there is only one controller that ones to command joints
ControlStrategy desired_strategy = JOINT_POSITION; // default
// If any of the controllers in the start list works on a velocity interface, the switch can't be done.
for ( std::list<hardware_interface::ControllerInfo>::const_iterator it = start_list.begin(); it != start_list.end(); ++it )
{
if( it->hardware_interface.compare( std::string("hardware_interface::PositionJointInterface") ) == 0 )
{
std::cout << "Request to switch to hardware_interface::PositionJointInterface (JOINT_POSITION)" << std::endl;
desired_strategy = JOINT_POSITION;
break;
}
else if( it->hardware_interface.compare( std::string("hardware_interface::EffortJointInterface") ) == 0 )
{
std::cout << "Request to switch to hardware_interface::EffortJointInterface (JOINT_IMPEDANCE)" << std::endl;
desired_strategy = JOINT_IMPEDANCE;
break;
}
else if( it->hardware_interface.compare( std::string("hardware_interface::PositionCartesianInterface") ) == 0 )
{
std::cout << "Request to switch to hardware_interface::PositionCartesianInterface (CARTESIAN_IMPEDANCE)" << std::endl;
desired_strategy = CARTESIAN_IMPEDANCE;
break;
}
}
for (int j = 0; j < n_joints_; ++j)
{
///semantic Zero
joint_position_command_[j] = joint_position_[j];
joint_effort_command_[j] = 0.0;
///call setCommand once so that the JointLimitsInterface receive the correct value on their getCommand()!
try{ position_interface_.getHandle(joint_names_[j]).setCommand(joint_position_command_[j]); }
catch(const hardware_interface::HardwareInterfaceException&){}
try{ effort_interface_.getHandle(joint_names_[j]).setCommand(joint_effort_command_[j]); }
catch(const hardware_interface::HardwareInterfaceException&){}
///reset joint_limit_interfaces
pj_sat_interface_.reset();
pj_limits_interface_.reset();
}
if(desired_strategy == getControlStrategy())
{
std::cout << "The ControlStrategy didn't change, it is already: " << getControlStrategy() << std::endl;
}
else
{
setControlStrategy(desired_strategy);
std::cout << "The ControlStrategy changed to: " << getControlStrategy() << std::endl;
}
}
