void Servo::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
// parse SDF Properries
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("torque")) {
torque = sdf->Get<double>("torque");
} else {
torque = 5;
}
gzmsg << "initializing servo: " << topic << " joint=" << joint->GetName()
<< " torque=" << torque << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &Servo::Callback, this);
// connect to the world update event
// this will call update every iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&Servo::Update, this, _1));
}
void DCMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
// Parse SDF properties
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("multiplier")) {
multiplier = sdf->Get<double>("multiplier");
} else {
multiplier = 1;
}
gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
<< " multiplier=" << multiplier << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &DCMotor::Callback, this);
// Connect to the world update event.
// This will trigger the Update function every Gazebo iteration
updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&DCMotor::Update, this, _1));
}
gazebo::physics::LinkPtr WorldProxy::HELPER_getLinkInModel(gazebo::physics::ModelPtr model, std::string link_name)
{
gazebo::physics::Link_V model_links = model->GetLinks();
for(int i=0; i < model_links.size(); i++ )
{
std::string candidate_link = model_links[i]->GetScopedName();
if( HELPER_hasEnding(candidate_link,"::"+link_name) )
{
return model_links[i];
}
}
return gazebo::physics::LinkPtr();
}
bool initSim(ros::NodeHandle nh, gazebo::physics::ModelPtr model)
{
// Get the gazebo joints that correspond to the robot joints
for(unsigned int j=0; j < n_dof_; j++) {
ROS_INFO_STREAM("Getting pointer to gazebo joint: "<<joint_name_[j]);
gazebo::physics::JointPtr joint = model->GetJoint(joint_name_[j]);
if (joint) {
sim_joints_.push_back(joint);
} else {
ROS_ERROR_STREAM(
"This robot has a joint named \""<<joint_name_[j]
<<"\" which is not in the gazebo model.");
return false;
}
}
return true;
}
bool initSim(const std::string& _robot_namespace,
ros::NodeHandle _nh,
gazebo::physics::ModelPtr _model,
const urdf::Model* const _urdf_model,
std::vector<transmission_interface::TransmissionInfo> _transmissions)
{
using gazebo::physics::JointPtr;
this->CleanUp();
// simulation joints
sim_joints_ = _model->GetJoints();
int n_dof = sim_joints_.size();
#ifdef MULTIPLE_JOINTS
this->GetJointNames(_nh);
this->Resize();
#endif
this->RegisterHardwareInterfaces();
}
virtual void Load( gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
// Start up ROS
int argc = 0;
ros::init( argc, NULL, "pendulum_robot" );
//ros::init( argc, NULL, "pendulum" );
this->model = _model;
this->world = _model->GetWorld();
this->joints = this->model->GetJoints();
// ROS Nodehandle
//this->node = new ros::NodeHandle( "~" );
this->node = new ros::NodeHandle( "pendulum" );
// ROS Subscriber
//this->sub = this->node->subscribe<pendulum::command>( "ros_pendulum_standup_controller", 1, &ros_pendulum_controller_c::ros_callback, this );
//this->client = this->node->serviceClient<pendulum::command>( "ros_pendulum_standup_controller" );
this->client = this->node->serviceClient<pendulum::rpc_command>( "pendulum_standup_command" );
/*
if( !client.isValid() ) {
std::cerr << "client not valid\n";
client.waitForExistence();
}
*/
//this->sub = this->node->subscribe<pendulum::command>( "ros_pendulum_standup_controller_state", 1, ros_pendulum_controller_c::request_state )
//this->server = this->node->advertiseService( "ros_pendulum_standup_controller_command", &ros_pendulum_controller_c::issue_command );
start_time = world->GetSimTime();
last_time = start_time;
this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind( &ros_pendulum_controller_c::Update, this ) );
std::cerr << "Pendulum Loaded" << std::endl;
}
Sensor::Sensor(
::gazebo::physics::ModelPtr _model,
sdf::ElementPtr _sensor,
std::string _partId,
std::string _sensorId,
unsigned int _inputs)
: VirtualSensor(_model, _partId, _sensorId, _inputs)
{
if (not _sensor->HasAttribute("sensor") or not _sensor->HasAttribute("link"))
{
std::cerr << "Sensor is missing required attributes (`link` or `sensor`)."
<< std::endl;
throw std::runtime_error("Sensor error");
}
auto sensorName = _sensor->GetAttribute("sensor")->GetAsString();
auto linkName = _sensor->GetAttribute("link")->GetAsString();
auto link = _model->GetLink(linkName);
if (not link)
{
std::cerr << "Link '" << linkName << "' for sensor '" << sensorName
<< "' is not present in model." << std::endl;
throw std::runtime_error("Sensor error");
}
std::string scopedName = link->GetScopedName(true) + "::" + sensorName;
this->sensor_ = gz::sensors::get_sensor(scopedName);
if (not this->sensor_)
{
std::cerr << "Sensor with scoped name '" << scopedName
<< "' could not be found." << std::endl;
throw std::runtime_error("Sensor error");
}
}
JointMotor::JointMotor(
::gazebo::physics::ModelPtr _model,
const std::string &_partId,
const std::string &_motorId,
sdf::ElementPtr _motor,
const unsigned int _outputs)
: Motor(_model, _partId, _motorId, _outputs)
{
if (not _motor->HasAttribute("joint"))
{
std::cerr << "JointMotor requires a `joint` attribute." << std::endl;
throw std::runtime_error("Motor error");
}
auto jointName = _motor->GetAttribute("joint")->GetAsString();
this->joint_ = _model->GetJoint(jointName);
if (not this->joint_)
{
std::cerr << "Cannot locate joint motor `" << jointName << "`" << std::endl;
throw std::runtime_error("Motor error");
}
this->jointName_ = this->joint_->GetScopedName();
}
DifferentialCPG::DifferentialCPG(
const ::gazebo::physics::ModelPtr &_model,
const sdf::ElementPtr _settings,
const std::vector< revolve::gazebo::MotorPtr > &_motors,
const std::vector< revolve::gazebo::SensorPtr > &_sensors)
: nextState_(nullptr)
, input_(new double[_sensors.size()])
, output_(new double[_motors.size()])
{
// Create transport node
this->node_.reset(new gz::transport::Node());
this->node_->Init();
auto name = _model->GetName();
// Listen to network modification requests
// alterSub_ = node_->Subscribe(
// "~/" + name + "/modify_diff_cpg", &DifferentialCPG::Modify,
// this);
if (not _settings->HasElement("rv:brain"))
{
std::cerr << "No robot brain detected, this is probably an error."
<< std::endl;
throw std::runtime_error("DifferentialCPG brain did not receive settings");
}
std::cout << _settings->GetDescription() << std::endl;
auto motor = _settings->HasElement("rv:motor")
? _settings->GetElement("rv:motor")
: sdf::ElementPtr();
while(motor)
{
if (not motor->HasAttribute("x") or not motor->HasAttribute("y"))
{
std::cerr << "Missing required motor attributes (x- and/or y- coordinate)"
<< std::endl;
throw std::runtime_error("Robot brain error");
}
auto motorId = motor->GetAttribute("part_id")->GetAsString();
auto coordX = std::atoi(motor->GetAttribute("x")->GetAsString().c_str());
auto coordY = std::atoi(motor->GetAttribute("y")->GetAsString().c_str());
this->positions_[motorId] = {coordX, coordY};
this->neurons_[{coordX, coordY, 1}] = {1.f, 0.f, 0.f};
this->neurons_[{coordX, coordY, -1}] = {1.f, 0.f, 0.f};
// TODO: Add check for duplicate coordinates
motor = motor->GetNextElement("rv:motor");
}
// Add connections between neighbouring neurons
for (const auto &position : this->positions_)
{
auto name = position.first;
int x, y; std::tie(x, y) = position.second;
if (this->connections_.count({x, y, 1, x, y, -1}))
{
continue;
}
if (this->connections_.count({x, y, -1, x, y, 1}))
{
continue;
}
this->connections_[{x, y, 1, x, y, -1}] = 1.f;
this->connections_[{x, y, -1, x, y, 1}] = 1.f;
for (const auto &neighbour : this->positions_)
{
int nearX, nearY;
std::tie(nearX, nearY) = neighbour.second;
if ((x+1) == nearX or (y+1) == nearY or (x-1) == nearX or (y-1) == nearY)
{
this->connections_[{x, y, 1, nearX, nearY, 1}] = 1.f;
this->connections_[{nearX, nearY, 1, x, y, 1}] = 1.f;
}
}
}
// Initialise array of neuron states for Update() method
this->nextState_ = new double[this->neurons_.size()];
}
//Here we already have the model!
bool robotSim::gazeboConfigureHook(gazebo::physics::ModelPtr model) {
if (model.get() == NULL) {
RTT::log(RTT::Error) << "No model could be loaded" << RTT::endlog();
return false;
}
if (!_models_loaded) {
RTT::log(RTT::Error) << "URDF and SRDF models has not been passed. Call loadURDFAndSRDF(URDF_path, SRDF_path)" << RTT::endlog();
return false;
}
// Get the joints
gazebo_joints_ = model->GetJoints();
model_links_ = model->GetLinks();
RTT::log(RTT::Info) << "Model name "<< model->GetName() << RTT::endlog();
RTT::log(RTT::Info) << "Model has " << gazebo_joints_.size() << " joints" << RTT::endlog();
RTT::log(RTT::Info) << "Model has " << model_links_.size() << " links" << RTT::endlog();
for(unsigned int i = 0; i < _xbotcore_model.get_chain_names().size(); ++i){
std::string chain_name = _xbotcore_model.get_chain_names()[i];
std::vector<std::string> enabled_joints_in_chain;
_xbotcore_model.get_enabled_joints_in_chain(chain_name,enabled_joints_in_chain);
kinematic_chains.insert(std::pair<std::string, boost::shared_ptr<KinematicChain>>(
chain_name, boost::shared_ptr<KinematicChain>(
new KinematicChain(chain_name, enabled_joints_in_chain, *(this->ports()), model
#ifdef USE_INTROSPECTION
,this
#endif
))));
}
RTT::log(RTT::Info) << "Kinematic Chains map created!" << RTT::endlog();
for(std::map<std::string, boost::shared_ptr<KinematicChain>>::iterator it = kinematic_chains.begin();
it != kinematic_chains.end(); it++){
if(!(it->second->initKinematicChain(gains.Gains))){
RTT::log(RTT::Warning) << "Problem Init Kinematic Chain" <<
it->second->getKinematicChainName() << RTT::endlog();
return false;
}
}
RTT::log(RTT::Info) << "Kinematic Chains Initialized!" << RTT::endlog();
RTT::log(RTT::Info) << "Checking Sensors (URDF/SRDF)"<<RTT::endlog();
std::map<std::string,int> ft_srdf = _xbotcore_model.get_ft_sensors();
std::map<std::string,int> imu_srdf = _xbotcore_model.get_imu_sensors();
RTT::log(RTT::Info) << "Force Update of SensorManager (before accessing sensors)!" << RTT::endlog();
gazebo::sensors::SensorManager::Instance()->Update(true);
RTT::log(RTT::Info) << "Checking Sensors (Gazebo)"<<RTT::endlog();
gazebo::sensors::Sensor_V sensors = gazebo::sensors::SensorManager::Instance()->GetSensors();
gazebo::sensors::Sensor_V sensors_attached_to_robot;
for(unsigned int i = 0; i < sensors.size(); ++i){
if(sensors[i]->ScopedName().find("::"+model->GetName()+"::") != std::string::npos)
sensors_attached_to_robot.push_back(sensors[i]);
}
for(std::map<std::string,int>::iterator i = ft_srdf.begin(); i != ft_srdf.end(); i++)
{
force_torque_sensor ft(i->first, model, _xbotcore_model.get_urdf_model(),
sensors_attached_to_robot,
*(this->ports())
#ifdef USE_INTROSPECTION
,this
#endif
);
if(ft.isInited())
force_torque_sensors.push_back(ft);
}
for(std::map<std::string,int>::iterator i = imu_srdf.begin(); i != imu_srdf.end(); ++i){
imu_sensor imu_(i->first,sensors_attached_to_robot,*(this->ports()));
if(imu_.isInited())
imu_sensors.push_back(imu_);
}
RTT::log(RTT::Warning) << "Done Configuring Component" << RTT::endlog();
return true;
}
bool SteerBotHardwareGazebo::initSim(const std::string& robot_namespace,
ros::NodeHandle nh,
gazebo::physics::ModelPtr model,
const urdf::Model* const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
using gazebo::physics::JointPtr;
nh_ = nh;
// Simulation joints
sim_joints_ = model->GetJoints();
n_dof_ = sim_joints_.size();
this->CleanUp();
this->GetJointNames(nh_);
this->RegisterHardwareInterfaces();
nh_.param(ns_ + "wheel_separation_w", wheel_separation_w_);
nh_.param(ns_ + "wheel_separation_h", wheel_separation_h_);
ROS_INFO_STREAM("wheel_separation_w = " << wheel_separation_w_);
ROS_INFO_STREAM("wheel_separation_h = " << wheel_separation_h_);
nh_.param(ns_ + "enable_ackermann_link", true);
ROS_INFO_STREAM("enable_ackermann_link = " << (enable_ackermann_link_ ? "true" : "false"));
#ifdef JOINT_LIMIT
// Position joint limits interface
std::vector<std::string> cmd_handle_names = steer_jnt_pos_cmd_interface_.getNames();
for (size_t i = 0; i < n_dof_; ++i)
{
const std::string name = cmd_handle_names[i];
// unless current handle is not pos interface for steer, skip
if(name != virtual_steer_jnt_names_[INDEX_RIGHT] && name != virtual_steer_jnt_names_[INDEX_LEFT])
continue;
JointHandle cmd_handle = steer_jnt_pos_cmd_interface_.getHandle(name);
using namespace joint_limits_interface;
boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(name);
JointLimits limits;
SoftJointLimits soft_limits;
if (!getJointLimits(urdf_joint, limits) || !getSoftJointLimits(urdf_joint, soft_limits))
{
ROS_WARN_STREAM("Joint limits won't be enforced for joint '" << name << "'.");
}
else
{
jnt_limits_interface_.registerHandle(
PositionJointSoftLimitsHandle(cmd_handle, limits, soft_limits));
ROS_DEBUG_STREAM("Joint limits will be enforced for joint '" << name << "'.");
}
}
#endif
// PID controllers for wheel
const int virtual_jnt_cnt_ = virtual_rear_wheel_jnt_names_.size();
pids_.resize(virtual_jnt_cnt_);
for (size_t i = 0; i < virtual_jnt_cnt_; ++i)
{
const std::string jnt_name = virtual_rear_wheel_jnt_names_[i];
const ros::NodeHandle joint_nh(nh, "gains/" + jnt_name);
ROS_INFO_STREAM("Trying to set pid param of '" << jnt_name << " ' at PID proc in init()");
if (!pids_[i].init(joint_nh))
{
ROS_INFO_STREAM("Faied to set pid param of '" << jnt_name << " ' at PID proc in init()");
return false;
}
ROS_INFO_STREAM("Succeeded to set pid param of '" << jnt_name << " ' at PID proc in init()");
}
return true;
}
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;
}
