Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Multiple controlboards #12

Merged
merged 4 commits into from
Oct 29, 2013
Merged

Multiple controlboards #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
42a502f
First support of multiple controlBoards
traversaro Oct 29, 2013
371c101
Added instruction for using the robotMotorGui with coman simulation
traversaro Oct 29, 2013
0c419f9
Fixed bugs related to controlBoard implementation
traversaro Oct 29, 2013
389561b
Merge branch 'multiple_controlboards' of https://github.com/EnricoMin…
traversaro Oct 29, 2013
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 18 additions & 0 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,24 @@ Set the new position

This is all you can do with coman inside gazebo at the moment.

Testing and moving coman joints with the robotMotorGui
------------------
You can also use the robotMotorGui for controlling the coman simulation.

You should start the coman simulation with
```
gazebo coman_parts.world
```
To instantiate a coman simulating the 5 different control boards (torso,left_leg,right_leg,left_arm,right_arm).

Please make sure you have the right configuration files for the control boards (they should be in your coman_urdf package in the conf/ subdirectory), if necessary
download the latest coman_urdf.tar.gz.

You can then execute the robotMotorGui:
```
robotMotorGui --name coman --parts "(torso left_arm right_arm left_leg right_leg)"
```

Troubleshooting
=============
- If gazebo complains about not finding the libyarp, check if you exported the GAZEBO_PLUGIN_PATH in the same shell where you are launching gazebo
Expand Down
28 changes: 12 additions & 16 deletions include/coman.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,6 @@ class yarp::dev::coman : public DeviceDriver,

~coman()
{
delete [] control_mode;
delete [] motion_done;
}

/**
Expand All @@ -84,15 +82,12 @@ class yarp::dev::coman : public DeviceDriver,
}
}
pos_lock.lock();
// read sensors (for now only joints angle)
auto joints=_robot->GetJoints();
int j=0;
for (auto joint:joints)
{
pos[j]=joint->GetAngle(0).Degree(); //TODO: if zero_pos=0, it works, if zero_pos=pos[j], pos[j] return 0, if zero_pos=k, pos[j]return 0-k, but since it is an angle, you may get 2*pi
//std::cout<<"joint"<<j<<" pos"<<pos[j]<<std::endl;
j++;

for(int jnt_cnt=0; jnt_cnt < joint_names.size(); jnt_cnt++ ) {
/** \todo consider multi-dof joint ? */
pos[jnt_cnt] = this->_robot->GetJoint(joint_names[jnt_cnt])->GetAngle(0).Degree();
}

pos_lock.unlock();
// send positions to the actuators
_clock++;
Expand Down Expand Up @@ -120,8 +115,9 @@ class yarp::dev::coman : public DeviceDriver,
else
motion_done[j]=true;
std::cout<<"pos: "<<pos[j]<<" ref_pos: "<<ref_pos[j]<<" ref_speed: "<<ref_speed[j]<<" period: "<<robot_refresh_period<<" result: "<<temp<<std::endl;
sendPositionToGazebo(j,temp);
}
sendPositionToGazebo(j,temp);

}
}
}
}
Expand Down Expand Up @@ -317,6 +313,8 @@ class yarp::dev::coman : public DeviceDriver,

virtual bool close() //NOT IMPLEMENTED
{
delete [] control_mode;
delete [] motion_done;
return true;
}

Expand Down Expand Up @@ -586,12 +584,10 @@ class yarp::dev::coman : public DeviceDriver,
void setMinMaxPos() //NOT TESTED
{
std::cout<<"Joint Limits"<<std::endl;
gazebo::physics::Joint_V joints = _robot->GetJoints();
for(unsigned int i = 0; i < _robot_number_of_joints; ++i)
{
gazebo::physics::JointPtr j = joints[i];
max_pos[i] = j->GetUpperLimit(0).Degree();
min_pos[i] = j->GetLowerLimit(0).Degree();
max_pos[i] = this->_robot->GetJoint(joint_names[i])->GetUpperLimit(0).Degree();
min_pos[i] = this->_robot->GetJoint(joint_names[i])->GetLowerLimit(0).Degree();
std::cout<<joint_names[i]<<" max_pos: "<<max_pos[i]<<" min_pos: "<<min_pos[i]<<std::endl;
}
}
Expand Down
10 changes: 4 additions & 6 deletions src/coman.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ void coman::gazebo_init()
this->robot_refresh_period=this->_robot->GetWorld()->GetPhysicsEngine()->GetUpdatePeriod()*1000.0;
setJointNames();

_robot_number_of_joints = _robot->GetJoints().size();
_robot_number_of_joints = joint_names.size();
pos_lock.unlock();
pos.size(_robot_number_of_joints);
zero_pos.size(_robot_number_of_joints);
Expand All @@ -71,7 +71,6 @@ void coman::gazebo_init()
ref_acc.size(_robot_number_of_joints);
max_pos.resize(_robot_number_of_joints);
min_pos.size(_robot_number_of_joints);
joint_names.reserve(_robot_number_of_joints);
_p.reserve(_robot_number_of_joints);
_i.reserve(_robot_number_of_joints);
_d.reserve(_robot_number_of_joints);
Expand All @@ -97,13 +96,12 @@ void coman::gazebo_init()

this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(&coman::onUpdate, this, _1));
gazebo_node_ptr = gazebo::transport::NodePtr(new gazebo::transport::Node);
gazebo_node_ptr = gazebo::transport::NodePtr(new gazebo::transport::Node);
gazebo_node_ptr->Init(this->_robot->GetWorld()->GetName());
jointCmdPub = gazebo_node_ptr->Advertise<gazebo::msgs::JointCmd>
(std::string("~/") + this->_robot->GetName() + "/joint_cmd");








Expand Down
29 changes: 19 additions & 10 deletions src/coman_yarp_gazebo_plugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@
#define toRad(X) (X*M_PI/180.0)
#define ROBOT_NAME "COMAN"

#define GAZEBO_YARP_CONTROLBOARD_DEVICE_NAME "coman"

namespace gazebo
{

Expand All @@ -40,7 +42,7 @@ class coman_yarp_gazebo_plugin : public ModelPlugin

void Init()
{
std::cout<<"*** COMAN GAZEBO YARP PLUGIN ***"<<std::endl;
std::cout<<"*** GazeboYarpControlBoard plugin started ***"<<std::endl;
if (!_yarp.checkNetwork())
std::cout<<"Sorry YARP network does not seem to be available, is the yarp server available?"<<std::endl;
else
Expand All @@ -66,8 +68,14 @@ class coman_yarp_gazebo_plugin : public ModelPlugin

//gazebo_pointer_wrapper::setModel(this->_robot);

//add the GazeboYarpControlBoard device only if is not already added
if( yarp::dev::Drivers::factory().find(GAZEBO_YARP_CONTROLBOARD_DEVICE_NAME) == NULL )
{
yarp::dev::Drivers::factory().add(new yarp::dev::DriverCreatorOf<yarp::dev::coman>
("coman", "controlboard", "coman"));
(GAZEBO_YARP_CONTROLBOARD_DEVICE_NAME, "controlboard", "coman"));
}



//Getting .ini configuration file from sdf
bool configuration_loaded = false;
Expand Down Expand Up @@ -95,16 +103,17 @@ class coman_yarp_gazebo_plugin : public ModelPlugin
_parameters.put("name", "/coman/test");//TODO what's this?
std::cout << "File .ini not found, loading default parameters" << std::endl;
}

//Now I love everything and every interface
std::ostringstream archive_stream;
boost::archive::text_oarchive archive(archive_stream);
uintptr_t cast_boost_to_pointer=(uintptr_t)_parent.get();
archive<<cast_boost_to_pointer;
_parameters.put("loving_gazebo_pointer",archive_stream.str().c_str());


//Now I love everything and every interface
std::ostringstream archive_stream;
boost::archive::text_oarchive archive(archive_stream);
uintptr_t cast_boost_to_pointer=(uintptr_t)_parent.get();
archive<<cast_boost_to_pointer;
_parameters.put("loving_gazebo_pointer",archive_stream.str().c_str());

_driver.open(_parameters);

if (!_driver.isValid())
fprintf(stderr, "Device did not open\n");

Expand Down
39 changes: 39 additions & 0 deletions world/coman_parts.world
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
<?xml version="1.0"?>
<sdf version="1.4">
<world name="default">

<!-- Light -->
<include>
<uri>model://sun</uri>
</include>

<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>

<!-- COMAN -->
<model name="COMAN">
<include>
<uri>model://coman_urdf</uri>
<pose>0 0 0.53 0 0 0 </pose>
</include>
<plugin name="coman_yarp_gazebo_plugin_torso" filename="libyarp_gazebo.so">
<yarpConfigurationFile>model://coman_urdf/conf/coman_gazebo_torso.ini</yarpConfigurationFile>
</plugin>
<plugin name="coman_yarp_gazebo_plugin_left_arm" filename="libyarp_gazebo.so">
<yarpConfigurationFile>model://coman_urdf/conf/coman_gazebo_left_arm.ini</yarpConfigurationFile>
</plugin>
<plugin name="coman_yarp_gazebo_plugin_right_arm" filename="libyarp_gazebo.so">
<yarpConfigurationFile>model://coman_urdf/conf/coman_gazebo_right_arm.ini</yarpConfigurationFile>
</plugin>
<plugin name="coman_yarp_gazebo_plugin_left_leg" filename="libyarp_gazebo.so">
<yarpConfigurationFile>model://coman_urdf/conf/coman_gazebo_left_leg.ini</yarpConfigurationFile>
</plugin>
<plugin name="coman_yarp_gazebo_plugin_right_leg" filename="libyarp_gazebo.so">
<yarpConfigurationFile>model://coman_urdf/conf/coman_gazebo_right_leg.ini</yarpConfigurationFile>
</plugin>
</model>

</world>
</sdf>