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