GripperRosControl(const std::vector<std::string>& actr_names, const std::vector<std::string>& jnt_names, const std::vector<std::string>& controller_names, const std::vector<double>& reducers) : actr_names_(actr_names) , jnt_names_(jnt_names) , controller_names_(controller_names) { for (int i = 0; i < jnt_names_.size(); i++) { std::string actr = actr_names_[i]; std::string jnt = jnt_names_[i]; // Get transmission boost::shared_ptr<transmission_interface::SimpleTransmission> t_ptr( new transmission_interface::SimpleTransmission(reducers[i])); trans_.push_back(t_ptr); // Initialize and wrap raw current data actr_curr_data_[actr].position.push_back(&actr_curr_pos_[actr]); actr_curr_data_[actr].velocity.push_back(&actr_curr_vel_[actr]); actr_curr_data_[actr].effort.push_back(&actr_curr_eff_[actr]); jnt_curr_data_[jnt].position.push_back(&jnt_curr_pos_[jnt]); jnt_curr_data_[jnt].velocity.push_back(&jnt_curr_vel_[jnt]); jnt_curr_data_[jnt].effort.push_back(&jnt_curr_eff_[jnt]); // Initialize and wrap raw command data actr_cmd_data_[actr].position.push_back(&actr_cmd_pos_[actr]); jnt_cmd_data_[jnt].position.push_back(&jnt_cmd_pos_[jnt]); // Register transmissions to each interface actr_to_jnt_state_.registerHandle(transmission_interface::ActuatorToJointStateHandle( actr + "_trans", trans_[i].get(), actr_curr_data_[actr], jnt_curr_data_[jnt])); jnt_to_actr_pos_.registerHandle(transmission_interface::JointToActuatorPositionHandle( jnt + "_trans", trans_[i].get(), actr_cmd_data_[actr], jnt_cmd_data_[jnt])); // Connect and register the joint state handle hardware_interface::JointStateHandle state_handle(jnt, &jnt_curr_pos_[jnt], &jnt_curr_vel_[jnt], &jnt_curr_eff_[jnt]); jnt_state_interface_.registerHandle(state_handle); // Connect and register the joint position handle hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(jnt), &jnt_cmd_pos_[jnt]); pos_jnt_interface_.registerHandle(pos_handle); // ROS publishers and subscribers actr_cmd_pub_[actr] = nh_.advertise<std_msgs::Float64>("dxl/" + controller_names_[i] + "/command", 5); actr_state_sub_[actr] = nh_.subscribe("dxl/" + controller_names_[i] + "/state", 1, &GripperRosControl::actrStateCallback, this); } // Register interfaces registerInterface(&jnt_state_interface_); registerInterface(&pos_jnt_interface_); // Start spinning nh_.setCallbackQueue(&subscriber_queue_); subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_)); subscriber_spinner_->start(); }
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; }
HRESULT DensoRobotHW::Initialize() { ros::NodeHandle nh; if (!nh.getParam("robot_name", m_robName)) { ROS_WARN("Failed to get robot_name parameter."); } if (!nh.getParam("robot_joints", m_robJoints)) { ROS_WARN("Failed to get robot_joints parameter."); } for (int i = 0; i < m_robJoints; i++) { std::stringstream ss; ss << "joint_" << i+1; if (!nh.getParam(ss.str(), m_type[i])) { ROS_WARN("Failed to get joint_%d parameter.", i+1); ROS_WARN("It was assumed revolute type."); m_type[i] = 1; } hardware_interface::JointStateHandle state_handle(ss.str(), &m_pos[i], &m_vel[i], &m_eff[i]); m_JntStInterface.registerHandle(state_handle); hardware_interface::JointHandle pos_handle( m_JntStInterface.getHandle(ss.str()), &m_cmd[i]); m_PosJntInterface.registerHandle(pos_handle); } int armGroup = 0; if (!nh.getParam("arm_group", armGroup)) { ROS_INFO("Use arm group 0."); armGroup = 0; } int format = 0; if(nh.getParam("send_format", format)) { m_sendfmt = format; } format = 0; if(nh.getParam("recv_format", format)) { m_recvfmt = format; } switch(m_recvfmt & DensoRobotRC8::RECVFMT_POSE) { case DensoRobotRC8::RECVFMT_POSE_J: case DensoRobotRC8::RECVFMT_POSE_PJ: case DensoRobotRC8::RECVFMT_POSE_TJ: break; default: ROS_WARN("Recieve format has to contain joint."); m_recvfmt = ((m_recvfmt & ~DensoRobotRC8::RECVFMT_POSE) | DensoRobotRC8::RECVFMT_POSE_J); break; } registerInterface(&m_JntStInterface); registerInterface(&m_PosJntInterface); HRESULT hr = m_eng->Initialize(); if(FAILED(hr)) { ROS_ERROR("Failed to connect real controller. (%X)", hr); return hr; } m_ctrl = m_eng->get_Controller(); DensoRobot_Ptr pRob; hr = m_ctrl->get_Robot(DensoBase::SRV_ACT, &pRob); if(FAILED(hr)) { ROS_ERROR("Failed to connect real robot. (%X)", hr); return hr; } m_rob = boost::dynamic_pointer_cast<DensoRobotRC8>(pRob); hr = CheckRobotType(); if(FAILED(hr)) { ROS_ERROR("Invalid robot type."); return hr; } m_rob->ChangeArmGroup(armGroup); hr = m_rob->ExecCurJnt(m_joint); if(FAILED(hr)) { ROS_ERROR("Failed to get current joint. (%X)", hr); return hr; } hr = m_ctrl->AddVariable("@ERROR_CODE"); if(SUCCEEDED(hr)) { hr = m_ctrl->get_Variable("@ERROR_CODE", &m_varErr); } if(FAILED(hr)) { ROS_ERROR("Failed to get @ERROR_CODE object. (%X)", hr); return hr; } { // Clear Error VARIANT_Ptr vntVal(new VARIANT()); vntVal->vt = VT_I4; vntVal->lVal = 0L; hr = m_varErr->ExecPutValue(vntVal); if(FAILED(hr)) { ROS_ERROR("Failed to clear error. (%X)", hr); return hr; } } hr = m_rob->AddVariable("@SERVO_ON"); if(SUCCEEDED(hr)) { DensoVariable_Ptr pVar; hr = m_rob->get_Variable("@SERVO_ON", &pVar); if(SUCCEEDED(hr)) { VARIANT_Ptr vntVal(new VARIANT()); vntVal->vt = VT_BOOL; vntVal->boolVal = VARIANT_TRUE; hr = pVar->ExecPutValue(vntVal); } } if(FAILED(hr)) { ROS_ERROR("Failed to motor on. (%X)", hr); return hr; } m_rob->put_SendFormat(m_sendfmt); m_sendfmt = m_rob->get_SendFormat(); m_rob->put_RecvFormat(m_recvfmt); m_recvfmt = m_rob->get_RecvFormat(); m_subChangeMode = nh.subscribe<Int32>( "ChangeMode", 1, &DensoRobotHW::Callback_ChangeMode, this); m_pubCurMode = nh.advertise<Int32>("CurMode", 1); hr = ChangeModeWithClearError(DensoRobotRC8::SLVMODE_SYNC_WAIT | DensoRobotRC8::SLVMODE_POSE_J); if(FAILED(hr)) { ROS_ERROR("Failed to change to slave mode. (%X)", hr); return hr; } return S_OK; }
// Set up the data for ros_controllers int32_t VigirHumanoidHWInterface::init_robot_controllers(boost::shared_ptr<vigir_control::VigirRobotModel>& robot_model, //boost::shared_ptr< std::vector<std::string> >& joint_list, boost::shared_ptr<ros::NodeHandle>& behavior_control_nh, boost::shared_ptr<ros::NodeHandle>& joint_control_nh, boost::shared_ptr<ros::NodeHandle>& private_nh) { ROS_INFO(" Initialize the generic humanoid HW interfaces ..."); // Store the list of controlled joints joint_names_ = robot_model->joint_names_; try { // State inputs std::cout << "Initialize HW interface for " << joint_names_->size() << " joints!" << std::endl; joint_state_positions_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize( joint_names_->size()); joint_state_velocities_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize(joint_names_->size()); joint_state_accelerations_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize(joint_names_->size()); joint_state_efforts_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0);//.resize(joint_names_->size()); // Control outputs joint_command_positions_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_command_velocities_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_command_accelerations_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_command_control_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_command_efforts_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_command_friction_compensation_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_position_errors_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_velocity_errors_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); joint_effort_errors_ = vigir_control::VectorNd::Constant(joint_names_->size(), 0.0); } catch(...) { std::cerr << "Failed to allocate memory for controllers!" << std::endl; return ROBOT_CONTROLLERS_FAILED_TO_INITIALIZE; } for (size_t i = 0; i < joint_names_->size(); ++i){ hardware_interface::JointStateHandle state_handle(joint_names_->at(i), &joint_state_positions_[i], &joint_state_velocities_[i], &joint_state_efforts_[i]); joint_state_interface_.registerHandle(state_handle); hardware_interface::JointHandle pos_handle(joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_positions_[i]); position_joint_interface_.registerHandle(pos_handle); hardware_interface::JointHandle vel_handle(joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_velocities_[i]); velocity_joint_interface_.registerHandle(vel_handle); hardware_interface::JointHandle effort_handle(joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_efforts_[i]); effort_joint_interface_.registerHandle(effort_handle); hardware_interface::PosVelAccJointHandle pos_vel_acc_handle (joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_positions_[i], &joint_command_velocities_[i], &joint_command_accelerations_[i]); pos_vel_acc_joint_interface_.registerHandle(pos_vel_acc_handle); hardware_interface::PosVelAccErrHumanoidJointHandle handle (joint_state_interface_.getHandle(joint_names_->at(i)), &joint_command_positions_[i], &joint_command_velocities_[i], &joint_command_accelerations_[i], &joint_position_errors_[i], &joint_velocity_errors_[i]); pos_vel_acc_err_humanoid_joint_interface_.registerHandle(handle); } try { ROS_INFO(" Load the whole robot controller handle ..."); hardware_interface::VigirHumanoidControllerHandle controller_handle(std::string("controller_handle"), &joint_state_positions_, &joint_state_velocities_, &joint_state_accelerations_, &joint_state_efforts_, &joint_command_positions_, //!< desired position &joint_command_velocities_, //!< desired velocity &joint_command_accelerations_, //!< desired acceleration &joint_command_efforts_, //!< desired effort &joint_command_control_, //!< desired control command (acceleration) &joint_command_friction_compensation_, //!< effort to compensate for friction &joint_position_errors_, &joint_velocity_errors_, &joint_effort_errors_, &robot_l_foot_wrench_, &robot_r_foot_wrench_, &robot_l_hand_wrench_, &robot_r_hand_wrench_, &robot_pose_, robot_model); humanoid_controller_interface_.registerHandle(controller_handle); ROS_INFO(" Register the controller interface..."); registerInterface(&humanoid_controller_interface_); } catch(...) { ROS_ERROR("Exception: could not initialize the Atlas pelvis control interface"); return ROBOT_EXCEPTION_CONTROLLERS_FAILED_TO_INITIALIZE; } registerInterface(&joint_state_interface_); registerInterface(&position_joint_interface_); registerInterface(&velocity_joint_interface_); registerInterface(&effort_joint_interface_); registerInterface(&pos_vel_acc_joint_interface_); registerInterface(&pos_vel_acc_err_humanoid_joint_interface_); ROS_INFO(" Done initializing the base HWInterface class for the humanoid controller ..."); return ROBOT_INITIALIZED_OK; }
inline void distributeGridAndData( Dune::CpGrid& grid, Opm::DeckConstPtr deck, EclipseStateConstPtr eclipseState, BlackoilState& state, BlackoilPropsAdFromDeck& properties, DerivedGeology& geology, std::shared_ptr<BlackoilPropsAdFromDeck::MaterialLawManager>& material_law_manager, std::vector<double>& threshold_pressures, boost::any& parallelInformation, const bool useLocalPerm) { Dune::CpGrid global_grid ( grid ); global_grid.switchToGlobalView(); // distribute the grid and switch to the distributed view grid.loadBalance(eclipseState, geology.transmissibility().data()); grid.switchToDistributedView(); std::vector<int> compressedToCartesianIdx; Opm::createGlobalCellArray(grid, compressedToCartesianIdx); typedef BlackoilPropsAdFromDeck::MaterialLawManager MaterialLawManager; auto distributed_material_law_manager = std::make_shared<MaterialLawManager>(); distributed_material_law_manager->initFromDeck(deck, eclipseState, compressedToCartesianIdx); // copy the values from the global to the local MaterialLawManager // We should actually communicate these to be future proof. But that is // really, really cumbersome for the underlying vector<shared_ptr> // where the classes pointed to even have more shared_ptr stored in them. typedef Dune::CpGrid::ParallelIndexSet IndexSet; const IndexSet& local_indices = grid.getCellIndexSet(); for ( auto index : local_indices ) { distributed_material_law_manager->materialLawParamsPointerReferenceHack(index.local()) = material_law_manager->materialLawParamsPointerReferenceHack(index.global()); distributed_material_law_manager->oilWaterScaledEpsInfoDrainagePointerReferenceHack(index.local()) = material_law_manager->oilWaterScaledEpsInfoDrainagePointerReferenceHack(index.global()); } BlackoilPropsAdFromDeck distributed_props(properties, distributed_material_law_manager, grid.numCells()); BlackoilState distributed_state(grid.numCells(), grid.numFaces(), state.numPhases()); BlackoilStateDataHandle state_handle(global_grid, grid, state, distributed_state); BlackoilPropsDataHandle props_handle(properties, distributed_props); grid.scatterData(state_handle); grid.scatterData(props_handle); // Create a distributed Geology. Some values will be updated using communication // below DerivedGeology distributed_geology(grid, distributed_props, eclipseState, useLocalPerm, geology.gravity()); GeologyDataHandle geo_handle(global_grid, grid, geology, distributed_geology); grid.scatterData(geo_handle); std::vector<double> distributed_pressures; if( !threshold_pressures.empty() ) // Might be empty if not specified { if( threshold_pressures.size() != static_cast<std::size_t>(UgGridHelpers::numFaces(global_grid)) ) { OPM_THROW(std::runtime_error, "NNCs not yet supported for parallel runs. " << UgGridHelpers::numFaces(grid) << " faces but " << threshold_pressures.size()<<" threshold pressure values"); } distributed_pressures.resize(UgGridHelpers::numFaces(grid)); ThresholdPressureDataHandle press_handle(global_grid, grid, threshold_pressures, distributed_pressures); grid.scatterData(press_handle); } // copy states properties = distributed_props; geology = distributed_geology; state = distributed_state; material_law_manager = distributed_material_law_manager; threshold_pressures = distributed_pressures; extractParallelGridInformationToISTL(grid, parallelInformation); }