void LWRHW::registerInterfaces(const urdf::Model *const urdf_model, std::vector<transmission_interface::TransmissionInfo> transmissions) { // Check that this transmission has one joint if( transmissions.empty() ) { std::cout << "lwr_hw: " << "There are no transmission in this robot, all are non-driven joints? " << std::endl; return; } // Initialize values for(int j=0; j < n_joints_; j++) { // Check that this transmission has one joint if(transmissions[j].joints_.size() == 0) { std::cout << "lwr_hw: " << "Transmission " << transmissions[j].name_ << " has no associated joints." << std::endl; continue; } else if(transmissions[j].joints_.size() > 1) { std::cout << "lwr_hw: " << "Transmission " << transmissions[j].name_ << " has more than one joint, and they can't be controlled simultaneously" << std::endl; continue; } std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; if( joint_interfaces.empty() ) { std::cout << "lwr_hw: " << "Joint " << transmissions[j].joints_[0].name_ << " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << "You need to, otherwise the joint can't be controlled." << std::endl; continue; } const std::string& hardware_interface = joint_interfaces.front(); // Debug std::cout << "\x1B[37m" << "lwr_hw: " << "Loading joint '" << joint_names_[j] << "' of type '" << hardware_interface << "'" << "\x1B[0m" << std::endl; // Create joint state interface for all joints state_interface_.registerHandle(hardware_interface::JointStateHandle( joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); // Decide what kind of command interface this actuator/joint has hardware_interface::JointHandle joint_handle_effort; joint_handle_effort = hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]), &joint_effort_command_[j]); effort_interface_.registerHandle(joint_handle_effort); hardware_interface::JointHandle joint_handle_position; joint_handle_position = hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]), &joint_position_command_[j]); position_interface_.registerHandle(joint_handle_position); // the stiffness is not actually a different joint, so the state handle is only used for handle hardware_interface::JointHandle joint_handle_stiffness; joint_handle_stiffness = hardware_interface::JointHandle(hardware_interface::JointStateHandle( joint_names_[j]+std::string("_stiffness"), &joint_stiffness_[j], &joint_stiffness_[j], &joint_stiffness_[j]), &joint_stiffness_command_[j]); position_interface_.registerHandle(joint_handle_stiffness); // velocity command handle, recall it is fake, there is no actual velocity interface hardware_interface::JointHandle joint_handle_velocity; joint_handle_velocity = hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]), &joint_velocity_command_[j]); registerJointLimits(joint_names_[j], joint_handle_effort, joint_handle_position, joint_handle_velocity, joint_handle_stiffness, urdf_model, &joint_lower_limits_[j], &joint_upper_limits_[j], &joint_lower_limits_stiffness_[j], &joint_upper_limits_stiffness_[j], &joint_effort_limits_[j]); } // Register interfaces registerInterface(&state_interface_); registerInterface(&effort_interface_); registerInterface(&position_interface_); }
bool DefaultRobotHWSim::initSim( const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model, const urdf::Model *const urdf_model, std::vector<transmission_interface::TransmissionInfo> transmissions) { // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each // parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint". const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace); // Resize vectors to our DOF n_dof_ = transmissions.size(); joint_names_.resize(n_dof_); joint_types_.resize(n_dof_); joint_lower_limits_.resize(n_dof_); joint_upper_limits_.resize(n_dof_); joint_effort_limits_.resize(n_dof_); joint_control_methods_.resize(n_dof_); pid_controllers_.resize(n_dof_); joint_position_.resize(n_dof_); joint_velocity_.resize(n_dof_); joint_effort_.resize(n_dof_); joint_effort_command_.resize(n_dof_); joint_position_command_.resize(n_dof_); joint_velocity_command_.resize(n_dof_); // Initialize values for(unsigned int j=0; j < n_dof_; j++) { // Check that this transmission has one joint if(transmissions[j].joints_.size() == 0) { ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ << " has no associated joints."); continue; } else if(transmissions[j].joints_.size() > 1) { ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ << " has more than one joint. Currently the default robot hardware simulation " << " interface only supports one."); continue; } std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; if (joint_interfaces.empty() && !(transmissions[j].actuators_.empty()) && !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) { // TODO: Deprecate HW interface specification in actuators in ROS J joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " << transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " << "The transmission will be properly loaded, but please update " << "your robot model to remain compatible with future versions of the plugin."); } if (joint_interfaces.empty()) { ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << "Not adding it to the robot hardware simulation."); continue; } else if (joint_interfaces.size() > 1) { ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " << "Currently the default robot hardware simulation interface only supports one. Using the first entry!"); //continue; } // Add data from transmission joint_names_[j] = transmissions[j].joints_[0].name_; joint_position_[j] = 1.0; joint_velocity_[j] = 0.0; joint_effort_[j] = 1.0; // N/m for continuous joints joint_effort_command_[j] = 0.0; joint_position_command_[j] = 0.0; joint_velocity_command_[j] = 0.0; const std::string& hardware_interface = joint_interfaces.front(); // Debug ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] << "' of type '" << hardware_interface << "'"); // Create joint state interface for all joints js_interface_.registerHandle(hardware_interface::JointStateHandle( joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); // Decide what kind of command interface this actuator/joint has hardware_interface::JointHandle joint_handle; if(hardware_interface == "EffortJointInterface") { // Create effort joint interface joint_control_methods_[j] = EFFORT; joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), &joint_effort_command_[j]); ej_interface_.registerHandle(joint_handle); } else if(hardware_interface == "PositionJointInterface") { // Create position joint interface joint_control_methods_[j] = POSITION; joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), &joint_position_command_[j]); pj_interface_.registerHandle(joint_handle); } else if(hardware_interface == "VelocityJointInterface") { // Create velocity joint interface joint_control_methods_[j] = VELOCITY; joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), &joint_velocity_command_[j]); vj_interface_.registerHandle(joint_handle); } else { ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '" << hardware_interface ); return false; } // Get the gazebo joint that corresponds to the robot joint. //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " // << joint_names_[j]); gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); if (!joint) { ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j] << "\" which is not in the gazebo model."); return false; } sim_joints_.push_back(joint); registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], joint_limit_nh, urdf_model, &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], &joint_effort_limits_[j]); if (joint_control_methods_[j] != EFFORT) { // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or // joint->SetVelocity() to control the joint. const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + joint_names_[j]); if (pid_controllers_[j].init(nh, true)) { switch (joint_control_methods_[j]) { case POSITION: joint_control_methods_[j] = POSITION_PID; break; case VELOCITY: joint_control_methods_[j] = VELOCITY_PID; break; } } else { // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is // going to be called. //joint->SetMaxForce(0, joint_effort_limits_[j]); joint->SetParam("max_force", 0, joint_effort_limits_[j]); } } } // Register interfaces registerInterface(&js_interface_); registerInterface(&ej_interface_); registerInterface(&pj_interface_); registerInterface(&vj_interface_); // Initialize the emergency stop code. e_stop_active_ = false; last_e_stop_active_ = false; return true; }
bool LWRHWreal::start() { // get params or give default values nh_.param("port", port_, 49939); nh_.param("ip", hintToRemoteHost_, std::string("192.168.0.10") ); // TODO: use transmission configuration to get names directly from the URDF model if( ros::param::get("joints", joint_names_) ) { if( !(joint_names_.size()==LBR_MNJ) ) { ROS_ERROR("This robot has 7 joints, you must specify 7 names for each one"); } } else { ROS_ERROR("No joints to be handled, ensure you load a yaml file naming the joint names this hardware interface refers to."); throw std::runtime_error("No joint name specification"); } if( !(urdf_model_.initParam("/robot_description")) ) { ROS_ERROR("No URDF model in the robot_description parameter, this is required to define the joint limits."); throw std::runtime_error("No URDF model available"); } // construct a low-level lwr device_.reset( new friRemote( port_, const_cast<char*>(hintToRemoteHost_.c_str()) ) ); // initialize FRI values lastQuality_ = FRI_QUALITY_BAD; lastCtrlScheme_ = FRI_CTRL_OTHER; // initialize and set to zero the state and command values init(LBR_MNJ); reset(); // general joint to store information boost::shared_ptr<const urdf::Joint> joint; // create joint handles given the list for(int i = 0; i < LBR_MNJ; ++i) { ROS_INFO_STREAM("Handling joint: " << joint_names_[i]); // get current joint configuration joint = urdf_model_.getJoint(joint_names_[i]); if(!joint.get()) { ROS_ERROR_STREAM("The specified joint "<< joint_names_[i] << " can't be found in the URDF model. Check that you loaded an URDF model in the robot description, or that you spelled correctly the joint name."); throw std::runtime_error("Wrong joint name specification"); } // joint state handle hardware_interface::JointStateHandle state_handle(joint_names_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]); state_interface_.registerHandle(state_handle); // effort command handle hardware_interface::JointHandle joint_handle_effort = hardware_interface::JointHandle( state_interface_.getHandle(joint_names_[i]), &joint_effort_command_[i]); effort_interface_.registerHandle(joint_handle_effort); // position command handle hardware_interface::JointHandle joint_handle_position = hardware_interface::JointHandle( state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]); position_interface_.registerHandle(joint_handle_position); // stiffness command handle, registered in the position interface as well hardware_interface::JointHandle joint_handle_stiffness; joint_handle_stiffness = hardware_interface::JointHandle(hardware_interface::JointStateHandle( joint_names_[i]+std::string("_stiffness"), &joint_stiffness_[i], &joint_stiffness_[i], &joint_stiffness_[i]), &joint_stiffness_command_[i]); position_interface_.registerHandle(joint_handle_stiffness); // velocity command handle, recall it is fake, there is no actual velocity interface hardware_interface::JointHandle joint_handle_velocity = hardware_interface::JointHandle( state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]); registerJointLimits(joint_names_[i], joint_handle_effort, joint_handle_position, joint_handle_velocity, joint_handle_stiffness, &urdf_model_, &joint_lower_limits_[i], &joint_upper_limits_[i], &joint_lower_limits_stiffness_[i], &joint_upper_limits_stiffness_[i], &joint_effort_limits_[i]); } ROS_INFO("Register state and effort interfaces"); // register ros-controls interfaces this->registerInterface(&state_interface_); this->registerInterface(&effort_interface_); this->registerInterface(&position_interface_); this->registerInterface(&velocity_interface_); // note that the velocity interface is not registrered, since the velocity command is computed within this implementation. std::cout << "Opening FRI Version " << FRI_MAJOR_VERSION << "." << FRI_SUB_VERSION << "." <<FRI_DATAGRAM_ID_CMD << "." <<FRI_DATAGRAM_ID_MSR << " Interface for LWR ROS server" << std::endl; ROS_INFO("Performing handshake to KRL"); // perform some arbitrary handshake to KRL -- possible in monitor mode already // send to krl int a value device_->setToKRLInt(0,1); if ( device_->getQuality() >= FRI_QUALITY_OK) { // send a second marker device_->setToKRLInt(0,10); } // // just mirror the real value.. // device_->setToKRLReal(0,device_->getFrmKRLReal(1)); ROS_INFO_STREAM("LWR Status:\n" << device_->getMsrBuf().intf); device_->doDataExchange(); ROS_INFO("Done handshake !"); return true; }
void GenericHWInterface::init() { ROS_INFO_STREAM_NAMED("generic_hw_interface", "Reading rosparams from namespace: " << nh_.getNamespace()); // Get joint names nh_.getParam("hardware_interface/joints", joint_names_); if (joint_names_.size() == 0) { ROS_FATAL_STREAM_NAMED("generic_hw_interface", "No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace() << "/hardware_interface/joints"); exit(-1); } num_joints_ = joint_names_.size(); // Status joint_position_.resize(num_joints_, 0.0); joint_velocity_.resize(num_joints_, 0.0); joint_effort_.resize(num_joints_, 0.0); // Command joint_position_command_.resize(num_joints_, 0.0); joint_velocity_command_.resize(num_joints_, 0.0); joint_effort_command_.resize(num_joints_, 0.0); // Limits joint_position_lower_limits_.resize(num_joints_, 0.0); joint_position_upper_limits_.resize(num_joints_, 0.0); joint_velocity_limits_.resize(num_joints_, 0.0); joint_effort_limits_.resize(num_joints_, 0.0); // Initialize interfaces for each joint for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) { ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Loading joint name: " << joint_names_[joint_id]); // Create joint state interface joint_state_interface_.registerHandle(hardware_interface::JointStateHandle( joint_names_[joint_id], &joint_position_[joint_id], &joint_velocity_[joint_id], &joint_effort_[joint_id])); // Add command interfaces to joints // TODO: decide based on transmissions? hardware_interface::JointHandle joint_handle_position = hardware_interface::JointHandle( joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_position_command_[joint_id]); position_joint_interface_.registerHandle(joint_handle_position); hardware_interface::JointHandle joint_handle_velocity = hardware_interface::JointHandle( joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_velocity_command_[joint_id]); velocity_joint_interface_.registerHandle(joint_handle_velocity); hardware_interface::JointHandle joint_handle_effort = hardware_interface::JointHandle( joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_effort_command_[joint_id]); effort_joint_interface_.registerHandle(joint_handle_effort); // Load the joint limits registerJointLimits(joint_handle_position, joint_handle_velocity, joint_handle_effort, joint_id); } // end for each joint registerInterface(&joint_state_interface_); // From RobotHW base class. registerInterface(&position_joint_interface_); // From RobotHW base class. registerInterface(&velocity_joint_interface_); // From RobotHW base class. registerInterface(&effort_joint_interface_); // From RobotHW base class. }