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; } }
void write(ros::Time time, ros::Duration period) { enforceLimits(period); switch (getControlStrategy()) { case JOINT_POSITION: for(int j=0; j < n_joints_; j++) { // according to the gazebo_ros_control plugin, this must *not* be called if SetForce is going to be called // but should be called when SetPostion is going to be called // so enable this when I find the SetMaxForce reset. // sim_joints_[j]->SetMaxForce(0, joint_effort_limits_[j]); #if GAZEBO_MAJOR_VERSION >= 4 sim_joints_[j]->SetPosition(0, joint_position_command_[j]); #else sim_joints_[j]->SetAngle(0, joint_position_command_[j]); #endif } break; case CARTESIAN_IMPEDANCE: ROS_WARN("CARTESIAN IMPEDANCE NOT IMPLEMENTED"); break; case JOINT_IMPEDANCE: // compute the gracity term f_dyn_solver_->JntToGravity(joint_position_kdl_, gravity_effort_); for(int j=0; j < n_joints_; j++) { // replicate the joint impedance control strategy // tau = k (q_FRI - q_msr) + tau_FRI + D(q_msr) + f_dyn(q_msr) const double stiffness_effort = 0.0;//10.0*( joint_position_command_[j] - joint_position_[j] ); // joint_stiffness_command_[j]*( joint_position_command_[j] - joint_position_[j] ); //double damping_effort = joint_damping_command_[j]*( joint_velocity_[j] ); const double effort = stiffness_effort + joint_effort_command_[j] + gravity_effort_(j); sim_joints_[j]->SetForce(0, effort); } break; case GRAVITY_COMPENSATION: ROS_WARN("CARTESIAN IMPEDANCE NOT IMPLEMENTED"); break; } }