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);
}
