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.
}
