static std::string create_unit_mass_RP_xml() throw(runtime_error)
{
static char const * xml =
"<?xml version=\"1.0\" ?>\n"
"<dynworld>\n"
" <baseNode>\n"
" <gravity>0, 0, -9.81</gravity>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>1, 0, 0, 0</rot>\n"
" <jointNode>\n"
" <ID>0</ID>\n"
" <type>R</type>\n"
" <axis>X</axis>\n"
" <mass>1</mass>\n"
" <inertia>0, 0, 0</inertia>\n"
" <com>0, 1, 0</com>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>0, 0, 1, 0</rot>\n"
" <jointNode>\n"
" <ID>1</ID>\n"
" <type>P</type>\n"
" <axis>Z</axis>\n"
" <mass>1</mass>\n"
" <inertia>0, 0, 0</inertia>\n"
" <com>0, 0, 0</com>\n"
" <pos>0, 1, 0</pos>\n"
" <rot>0, 0, 1, 0</rot>\n"
" </jointNode>\n"
" </jointNode>\n"
" </baseNode>\n"
"</dynworld>\n";
std::string result(create_tmpfile("unit_mass_RP.xml.XXXXXX", xml));
return result;
}
//----------------------------------------------------------------------------------
// Plots a 2d graph from a list of doubles: x y
Gnuplot& Gnuplot::plot_xy(const std::vector<double> &x,
const std::vector<double> &y,
const std::string &title)
{
if (x.size() == 0 || y.size() == 0)
{
throw GnuplotException("std::vectors too small");
return *this;
}
if (x.size() != y.size())
{
throw GnuplotException("Length of the std::vectors differs");
return *this;
}
std::ofstream tmp;
std::string name = create_tmpfile(tmp);
if (name == "")
return *this;
// write the data to file
for (unsigned int i = 0; i < x.size(); i++)
tmp << x[i] << " " << y[i] << std::endl;
tmp.flush();
tmp.close();
this->plotfile_xy(name, 1, 2, title);
return *this;
}
Gnuplot& plot_xyz(const X &x,
const Y &y,
const Z &z,
const std::string &title = "")
{
if(x.size() == 0 || y.size() == 0 || z.size() == 0)
{
// throw GnuplotException("std::vectors too small");
return *this;
}
if(x.size() != y.size() || x.size() != z.size())
{
// throw GnuplotException("Length of the std::vectors differs");
return *this;
}
std::ofstream tmp;
std::string name = create_tmpfile(tmp);
if(name == "")
return *this;
//
// write the data to file
//
for (unsigned int i = 0; i < x.size(); i++)
tmp << x[i] << " " << y[i] << " " << z[i] <<std::endl;
tmp.flush();
tmp.close();
plotfile_xyz(name, 1, 2, 3, title);
return *this;
};
void
create_file(wzfile * file, char * buffer, size_t len) {
file->raw = create_tmpfile(buffer, len);
file->pos = 0;
file->size = len;
file->ver.hash = 0;
file->key = NULL;
}
static std::string create_fork_4R_xml() throw(runtime_error)
{
static char const * xml =
"<?xml version=\"1.0\" ?>\n"
"<dynworld>\n"
" <baseNode>\n"
" <gravity>0, 0, -9.81</gravity>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>1, 0, 0, 0</rot>\n"
" <jointNode>\n"
" <ID>0</ID>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>1</mass>\n"
" <inertia>0.3, 0.2, 0.1</inertia>\n"
" <com>0.5, 0, 0</com>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>1, 0, 0, 0</rot>\n"
" <jointNode>\n"
" <ID>1</ID>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>1</mass>\n"
" <inertia>0.1, 0.2, 0.3</inertia>\n"
" <com>0, 0, 0</com>\n"
" <pos>1, 0, 0</pos>\n"
" <!-- Beware: the axis is not renormalized by the legacy SAI\n"
" parser! So, 0.57735026919=sqrt(1/3) and\n"
" 2.09439510239=2pi/3 will rotate Z onto X, X onto Y, and Y\n"
" onto Z -->\n"
" <rot>0.57735026919, 0.57735026919, 0.57735026919, 2.09439510239 </rot>\n"
" <jointNode>\n"
" <ID>2</ID>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>1</mass>\n"
" <inertia>0.2, 0.2, 0.1</inertia>\n"
" <com>0.5, 0, 0</com>\n"
" <pos>-1, 0, 0</pos>\n"
" <rot>0, 1, 0, -1.57079632679 </rot> -->\n"
" </jointNode>\n"
" <jointNode>\n"
" <ID>3</ID>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>1</mass>\n"
" <inertia>0.1, 0.2, 0.1</inertia>\n"
" <com>0.5, 0, 0</com>\n"
" <pos>1, 0, 0</pos>\n"
" <rot>0.707106781187, 0, 0.707106781187, 3.14159265359 </rot>\n"
" </jointNode>\n"
" </jointNode>\n"
" </jointNode>\n"
" </baseNode>\n"
"</dynworld>\n";
std::string result(create_tmpfile("fork_4R.xml.XXXXXX", xml));
return result;
}
//------------------------------------------------------------------------------
//
/// * note that this function is not valid for versions of GNUPlot below 4.2
//
Gnuplot& Gnuplot::plot_image(const unsigned char * ucPicBuf,
const unsigned int iWidth,
const unsigned int iHeight,
const std::string &title)
{
std::ofstream tmp;
std::string name = create_tmpfile(tmp);
if (name == "")
return *this;
//
// write the data to file
//
int iIndex = 0;
for(int iRow = 0; iRow < iHeight; iRow++)
{
for(int iColumn = 0; iColumn < iWidth; iColumn++)
{
tmp << iColumn << " " << iRow << " "
<< static_cast<float>(ucPicBuf[iIndex++]) << std::endl;
}
}
tmp.flush();
tmp.close();
std::ostringstream cmdstr;
//
// command to be sent to gnuplot
//
if (nplots > 0 && two_dim == true)
cmdstr << "replot ";
else
cmdstr << "plot ";
if (title == "")
cmdstr << "\"" << name << "\" with image";
else
cmdstr << "\"" << name << "\" title \"" << title << "\" with image";
//
// Do the actual plot
//
cmd(cmdstr.str());
return *this;
}
Gnuplot& plot_x(const X& x, const std::string &title = "")
{
if (x.size() == 0)
{
// throw GnuplotException("std::vector too small");
return *this;
}
std::ofstream tmp;
std::string name = create_tmpfile(tmp);
if (name == "")
return *this;
//
// write the data to file
//
for (unsigned int i = 0; i < x.size(); i++)
tmp << x[i] << std::endl;
tmp.flush();
tmp.close();
plotfile_x(name, 1, title);
return *this;
};
Gnuplot& Gnuplot::plot_xy_err(const X &x,
const Y &y,
const E &dy,
const std::string &title)
{
if (x.size() == 0 || y.size() == 0 || dy.size() == 0)
{
throw GnuplotException("std::vectors too small");
return *this;
}
if (x.size() != y.size() || y.size() != dy.size())
{
throw GnuplotException("Length of the std::vectors differs");
return *this;
}
std::ofstream tmp;
std::string name = create_tmpfile(tmp);
if (name == "")
return *this;
//
// write the data to file
//
for (unsigned int i = 0; i < x.size(); i++)
tmp << x[i] << " " << y[i] << " " << dy[i] << std::endl;
tmp.flush();
tmp.close();
// Do the actual plot
plotfile_xy_err(name, 1, 2, 3, title);
return *this;
}
static std::string create_puma_xml() throw(runtime_error)
{
static char const * xml =
"<?xml version=\"1.0\" ?>\n"
"<dynworld>\n"
" <baseNode>\n"
" <robotName>Puma</robotName>\n"
" <gravity>0, 0, -9.81</gravity>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>1, 0, 0, 0</rot>\n"
" <name>base</name>\n"
" <ID>-1</ID>\n"
" <jointNode>\n"
" <jointName>shoulder-yaw</jointName>\n"
" <linkName>base</linkName>\n"
" <upperJointLimit>0.1</upperJointLimit>\n"
" <lowerJointLimit>-0.1</lowerJointLimit>\n"
" <defaultJointPosition>0</defaultJointPosition>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>34.40</mass>\n"
" <inertia>0.00, 0.00, 1.49</inertia>\n"
" <com>0, 0, 0</com>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>0, 0, 1, 0</rot>\n"
" <name>NoName</name>\n"
" <ID>0</ID>\n"
" <jointNode>\n"
" <jointName>shoulder-pitch</jointName>\n"
" <linkName>upper_arm</linkName>\n"
" <upperJointLimit>0.1</upperJointLimit>\n"
" <lowerJointLimit>-0.1</lowerJointLimit>\n"
" <defaultJointPosition>0</defaultJointPosition>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>17.40</mass>\n"
" <inertia>0.13, 0.524, 5.249</inertia>\n"
" <com>0.068, 0.006, -0.016</com>\n"
" <pos>0.0, 0.2435, 0.0</pos>\n"
" <rot>1, 0, 0, -1.57079632679489661923</rot>\n"
" <name>upper_arm</name>\n"
" <ID>1</ID>\n"
" <jointNode>\n"
" <jointName>elbow</jointName>\n"
" <linkName>lower_arm</linkName>\n"
" <upperJointLimit>0.1</upperJointLimit>\n"
" <lowerJointLimit>-0.1</lowerJointLimit>\n"
" <defaultJointPosition>0</defaultJointPosition>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>6.04</mass>\n"
" <inertia>0.192, 0.0154, 1.042</inertia>\n"
" <com>0, -0.143, 0.014</com>\n"
" <pos>0.4318, 0, -0.0934</pos>\n"
" <rot>1, 0, 0, 0</rot>\n"
" <name>lower_arm</name>\n"
" <ID>2</ID>\n"
" <jointNode>\n"
" <jointName>wrist-roll1</jointName>\n"
" <linkName>wrist-hand</linkName>\n"
" <upperJointLimit>0.1</upperJointLimit>\n"
" <lowerJointLimit>-0.1</lowerJointLimit>\n"
" <defaultJointPosition>0</defaultJointPosition>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>0.82</mass>\n"
" <inertia>0.0018, 0.0018, 0.2013</inertia>\n"
" <com>0.0, 0.0, -0.019</com>\n"
" <pos>-0.0203, -0.4331, 0.0</pos>\n"
" <rot>1, 0, 0, 1.57079632679489661923</rot>\n"
" <name>wrist-hand</name>\n"
" <ID>3</ID>\n"
" <jointNode>\n"
" <jointName>wrist-pitch</jointName>\n"
" <linkName>wrist-finger</linkName>\n"
" <upperJointLimit>0.1</upperJointLimit>\n"
" <lowerJointLimit>-0.1</lowerJointLimit>\n"
" <defaultJointPosition>0</defaultJointPosition>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>0.34</mass>\n"
" <inertia>0.0003, 0.0003, 0.1794</inertia>\n"
" <com>0.0, 0.0, 0.0</com>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>1, 0, 0, -1.57079632679489661923</rot>\n"
" <name>wrist-finger</name>\n"
" <ID>4</ID>\n"
" <jointNode>\n"
" <jointName>wrist-roll2</jointName>\n"
" <linkName>end-effector</linkName>\n"
" <upperJointLimit>0.1</upperJointLimit>\n"
" <lowerJointLimit>-0.1</lowerJointLimit>\n"
" <defaultJointPosition>0</defaultJointPosition>\n"
" <type>R</type>\n"
" <axis>Z</axis>\n"
" <mass>0.09</mass>\n"
" <inertia>0.00015, 0.00015, 0.19304</inertia>\n"
" <com>0.0, 0.0, 0.032</com>\n"
" <pos>0, 0, 0</pos>\n"
" <rot>1, 0, 0, 1.57079632679489661923</rot>\n"
" <name>end-effector</name>\n"
" <ID>5</ID>\n"
" </jointNode>\n"
" </jointNode>\n"
" </jointNode>\n"
" </jointNode>\n"
" </jointNode>\n"
" </jointNode>\n"
" </baseNode>\n"
"</dynworld>\n";
std::string result(create_tmpfile("puma.xml.XXXXXX", xml));
return result;
}
