mrp2_hardware_gazebo: mrp2_hardware

Go to the documentation of this file.
00001 #include <cassert>
00002 #include <boost/foreach.hpp>
00003 
00004 #include <gazebo/sensors/SensorManager.hh>
00005 
00006 #include <urdf_parser/urdf_parser.h>
00007 
00008 #include <joint_limits_interface/joint_limits_urdf.h>
00009 #include <joint_limits_interface/joint_limits_rosparam.h>
00010 
00011 #include <mrp2_hardware_gazebo/mrp2_hardware_gazebo.h>
00012 
00013 using std::string;
00014 using std::vector;
00015 
00016 namespace mrp2_hardware_gazebo
00017 {
00018   using namespace hardware_interface;
00019 
00020   Mrp2HardwareGazebo::Mrp2HardwareGazebo()
00021     : gazebo_ros_control::RobotHWSim()
00022   {}
00023 
00024 
00025   bool Mrp2HardwareGazebo::initSim(const std::string& robot_namespace,
00026       ros::NodeHandle nh,
00027       gazebo::physics::ModelPtr model,
00028       const urdf::Model* const urdf_model,
00029       std::vector<transmission_interface::TransmissionInfo> transmissions)
00030   {
00031     using gazebo::physics::JointPtr;
00032 
00033     // Cleanup
00034     pos_sim_joints_.clear();
00035     vel_sim_joints_.clear();
00036     sim_joints_.clear();
00037     jnt_pos_.clear();
00038     jnt_vel_.clear();
00039     jnt_eff_.clear();
00040     jnt_pos_cmd_.clear();
00041     jnt_pos_cmd_curr_.clear();
00042     jnt_vel_cmd_.clear();
00043 
00044     // Simulation joints
00045     std::vector<gazebo::physics::JointPtr> sim_joints_tmp = model->GetJoints();
00046 
00047     std::vector<std::string> pos_jnt_names;
00048     std::vector<std::string> vel_jnt_names;
00049     std::vector<std::string> jnt_names;
00050     for (size_t i = 0; i < sim_joints_tmp.size(); ++i)
00051     {
00052 
00053       const std::string unscoped_name = sim_joints_tmp[i]->GetName();
00054       if ( !(unscoped_name.size() >= 6 && 0 == unscoped_name.compare(0, 6, "caster")) )
00055       {
00056         if(0 == unscoped_name.compare(0, 5, "wheel"))
00057         {
00058           boost::shared_ptr<const urdf::Joint> joint(urdf_model->getJoint(unscoped_name));
00059           if(!joint)
00060           {
00061             ROS_ERROR_STREAM(unscoped_name
00062                              << " couldn't be retrieved from model description");
00063             return false;
00064           }
00065 
00066           sim_joints_tmp[i]->SetMaxForce(0u, joint->limits->effort);
00067           vel_sim_joints_.push_back(sim_joints_tmp[i]);
00068           vel_jnt_names.push_back(unscoped_name);
00069         }
00070         else
00071         {
00072           pos_sim_joints_.push_back(sim_joints_tmp[i]);
00073           pos_jnt_names.push_back(unscoped_name);
00074         }
00075       }
00076 
00077       sim_joints_.push_back(sim_joints_tmp[i]);
00078       jnt_names.push_back(unscoped_name);
00079     }
00080 
00081     pos_n_dof_ = pos_sim_joints_.size();
00082     vel_n_dof_ = vel_sim_joints_.size();
00083     n_dof_ = sim_joints_.size();
00084 
00085     // Raw data
00086     jnt_pos_.resize(n_dof_);
00087     jnt_vel_.resize(n_dof_);
00088     jnt_eff_.resize(n_dof_);
00089     jnt_pos_cmd_.resize(pos_n_dof_);
00090     jnt_pos_cmd_curr_.resize(pos_n_dof_);
00091     jnt_vel_cmd_.resize(vel_n_dof_);
00092 
00093     // Hardware interfaces
00094     for (size_t i = 0; i < n_dof_; ++i)
00095     {
00096 
00097       jnt_state_interface_.registerHandle(JointStateHandle(jnt_names[i],
00098                                                            &jnt_pos_[i],
00099                                                            &jnt_vel_[i],
00100                                                            &jnt_eff_[i]));
00101     }
00102     for (size_t i = 0; i < pos_n_dof_; ++i)
00103     {
00104 
00105       jnt_pos_cmd_interface_.registerHandle(JointHandle(jnt_state_interface_.getHandle(pos_jnt_names[i]),
00106                                                         &jnt_pos_cmd_[i]));
00107       ROS_DEBUG_STREAM("Registered joint '" << pos_jnt_names[i] << "' in the PositionJointInterface.");
00108     }
00109     for (size_t i = 0; i < vel_n_dof_; ++i)
00110     {
00111 
00112       jnt_vel_cmd_interface_.registerHandle(JointHandle(jnt_state_interface_.getHandle(vel_jnt_names[i]),
00113                                                         &jnt_vel_cmd_[i]));
00114       ROS_DEBUG_STREAM("Registered joint '" << pos_jnt_names[i] << "' in the VelocityJointInterface.");
00115     }
00116     registerInterface(&jnt_state_interface_);
00117     registerInterface(&jnt_pos_cmd_interface_);
00118     registerInterface(&jnt_vel_cmd_interface_);
00119 
00120     // Position joint limits interface
00121     vector<string> pos_joints_with_limits, pos_joints_without_limits;
00122     for (size_t i = 0; i < pos_n_dof_; ++i)
00123     {
00124       JointHandle cmd_handle = jnt_pos_cmd_interface_.getHandle(pos_sim_joints_[i]->GetName());
00125       const string name = cmd_handle.getName();
00126 
00127       using namespace joint_limits_interface;
00128       boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(name);
00129       JointLimits limits;
00130       SoftJointLimits soft_limits;
00131       if (!getJointLimits(urdf_joint, limits) || !getSoftJointLimits(urdf_joint, soft_limits))
00132       {
00133         pos_joints_without_limits.push_back(name);
00134         continue;
00135       }
00136       pos_jnt_limits_interface_.registerHandle(PositionJointSoftLimitsHandle(cmd_handle, limits, soft_limits));
00137       pos_joints_with_limits.push_back(name);
00138     }
00139     if (!pos_joints_with_limits.empty())
00140     {
00141       ROS_DEBUG_STREAM("Joint limits will be enforced for position-controlled joints:" <<
00142                         containerToString(pos_joints_with_limits, "\n - "));
00143     }
00144     if (!pos_joints_without_limits.empty())
00145     {
00146       ROS_WARN_STREAM("Joint limits will not be enforced for position-controlled joints:" <<
00147                       containerToString(pos_joints_without_limits, "\n - "));
00148     }
00149 
00150     // Velocity joint limits interface
00151     vector<string> vel_joints_with_limits, vel_joints_without_limits;
00152     for (unsigned int i = 0; i < vel_n_dof_; ++i)
00153     {
00154       JointHandle cmd_handle = jnt_vel_cmd_interface_.getHandle(vel_sim_joints_[i]->GetName());
00155       const string name = cmd_handle.getName();
00156 
00157       using namespace joint_limits_interface;
00158       boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(name);
00159       JointLimits limits;
00160       if (!getJointLimits(urdf_joint, limits) || !getJointLimits(name, nh, limits))
00161       {
00162         vel_joints_without_limits.push_back(name);
00163         continue;
00164       }
00165       vel_jnt_limits_interface_.registerHandle(VelocityJointSaturationHandle(cmd_handle, limits));
00166       vel_joints_with_limits.push_back(name);
00167     }
00168     if (!vel_joints_with_limits.empty())
00169     {
00170       ROS_DEBUG_STREAM("Joint limits will be enforced for velocity-controlled joints:" <<
00171                         containerToString(vel_joints_with_limits, "\n - "));
00172     }
00173     if (!vel_joints_without_limits.empty())
00174     {
00175       ROS_WARN_STREAM("Joint limits will not be enforced for velocity-controlled joints:" <<
00176                       containerToString(vel_joints_without_limits, "\n - "));
00177     }
00178 
00179     // Hardware interfaces: Base IMU sensors
00180     const string imu_name = "base_imu";
00181     imu_sensor_  = (gazebo::sensors::ImuSensor*)gazebo::sensors::SensorManager::Instance()->GetSensor(imu_name+"_sensor").get(); // TODO: Fetch from URDF?
00182     if (!this->imu_sensor_)
00183     {
00184       ROS_ERROR_STREAM("Could not find base IMU sensor.");
00185       return false;
00186     }
00187 
00188     ImuSensorHandle::Data data;
00189     data.name     = imu_name;           
00190     data.frame_id = imu_name + "_link"; 
00191     data.orientation = &base_orientation_[0];
00192     imu_sensor_interface_.registerHandle(ImuSensorHandle(data));
00193     registerInterface(&imu_sensor_interface_);
00194     ROS_DEBUG_STREAM("Registered IMU sensor.");
00195 
00196     // PID controllers
00197     pids_.resize(pos_n_dof_);
00198     for (size_t i = 0; i < pos_n_dof_; ++i)
00199     {
00200       ros::NodeHandle joint_nh(nh, "gains/" + pos_jnt_names[i]);
00201       if (!pids_[i].init(joint_nh)) {return false;}
00202     }
00203 
00204     return true;
00205   }
00206 
00207   void Mrp2HardwareGazebo::readSim(ros::Time time, ros::Duration period)
00208   {
00209     for(unsigned int j = 0; j < n_dof_; ++j)
00210     {
00211       jnt_pos_[j] += angles::shortest_angular_distance
00212           (jnt_pos_[j], sim_joints_[j]->GetAngle(0u).Radian());
00213       jnt_vel_[j] = sim_joints_[j]->GetVelocity(0u);
00214       jnt_eff_[j] = sim_joints_[j]->GetForce(0u);
00215     }
00216 
00217     for(unsigned int j = 0; j < pos_n_dof_; ++j)
00218     {
00219       jnt_pos_cmd_curr_[j] += angles::shortest_angular_distance
00220           (jnt_pos_cmd_curr_[j], pos_sim_joints_[j]->GetAngle(0u).Radian());
00221     }
00222 
00223     // Read IMU sensor
00224     gazebo::math::Quaternion imu_quat = imu_sensor_->GetOrientation();
00225     base_orientation_[0] = imu_quat.x;
00226     base_orientation_[1] = imu_quat.y;
00227     base_orientation_[2] = imu_quat.z;
00228     base_orientation_[3] = imu_quat.w;
00229 
00230     gazebo::math::Vector3 imu_ang_vel = imu_sensor_->GetAngularVelocity();
00231     base_ang_vel_[0] = imu_ang_vel.x;
00232     base_ang_vel_[1] = imu_ang_vel.y;
00233     base_ang_vel_[2] = imu_ang_vel.z;
00234 
00235     gazebo::math::Vector3 imu_lin_acc = imu_sensor_->GetLinearAcceleration();
00236     base_lin_acc_[0] =  imu_lin_acc.x;
00237     base_lin_acc_[1] =  imu_lin_acc.y;
00238     base_lin_acc_[2] =  imu_lin_acc.z;
00239   }
00240 
00241   void Mrp2HardwareGazebo::writeSim(ros::Time time, ros::Duration period)
00242   {
00243     // Enforce joint limits
00244     pos_jnt_limits_interface_.enforceLimits(period);
00245     vel_jnt_limits_interface_.enforceLimits(period);
00246 
00247     // Compute and send commands
00248     for(unsigned int j = 0; j < pos_n_dof_; ++j)
00249     {
00250       const double error = jnt_pos_cmd_[j] - jnt_pos_cmd_curr_[j];
00251       const double effort = pids_[j].computeCommand(error, period);
00252 
00253       pos_sim_joints_[j]->SetForce(0u, effort);
00254     }
00255     for(unsigned int j = 0; j < vel_n_dof_; ++j)
00256     {
00257       vel_sim_joints_[j]->SetVelocity(0u, jnt_vel_cmd_[j]);
00258     }
00259   }
00260 
00261 } // mrp2_hardware_gazebo
00262 
00263 PLUGINLIB_EXPORT_CLASS( mrp2_hardware_gazebo::Mrp2HardwareGazebo, gazebo_ros_control::RobotHWSim)