int
main(int argc, char *argv[])
{
int largc;
char **largv;
osInit();
subsumeImplicitArgs(argc, argv, &largc, &largv);
initState(argv[0]);
uclSysName = getCfgName(largc, largv);
uclOptFile = getCfgFile(largc, largv);
if (!uclOptFile) exit(2);
loadConfiguration();
handleOptions(largc, largv);
if (!setupState())
exit(2);
if (dbgLevel > 2) {
int i;
printf("Command:");
for (i=0; i<largc; i++) {
printf(" %s", largv[i]);
}
printf("\n");
}
generateCommands();
if (!executeCommands())
exit(1);
exit(0);
/* gack */
return 0;
}
bool Testbench::init()
{
ROS_ERROR("Testbench::init");
boost::shared_ptr<urdf::Model> model = boost::make_shared<urdf::Model>();
if(!model->initParam("robot_description"))
{
ROS_ERROR("Could not get robot description");
return false;
}
if(!kdl_parser::treeFromUrdfModel(*model, m_tree))
{
ROS_ERROR("Could not get KDL tree from URDF model");
return false;
}
boost::shared_ptr<KDL::Chain> chain = boost::make_shared<KDL::Chain>();
// FIXME
if(!m_tree.getChain("trunk_link", "left_foot_link", *chain))
{
ROS_ERROR("Could not get chain");
return false;
}
ROS_ERROR("Initializing torque estimator...");
fflush(stderr);
m_estimator.reset(new TorqueEstimator);
if(!m_estimator->init(chain))
{
ROS_ERROR("Could not initialize torque estimator");
return false;
}
QMap<QString, JointSpec> specs;
specs["left_hip_yaw"] = JointSpec(8, 2048, false, false);
specs["left_hip_roll"] = JointSpec(10, 1792, false, false);
specs["left_hip_pitch"] = JointSpec(12, 2040, false, true);
specs["left_knee_pitch"] = JointSpec(14, 1205, false, true);
specs["left_ankle_pitch"] = JointSpec(16, 2046, true, false);
specs["left_ankle_roll"] = JointSpec(18, 2304, true, false);
std::vector<boost::shared_ptr<urdf::Link> > links;
model->getLinks(links);
for(int i = 0; i < links.size(); ++i)
{
const urdf::Link& link = *links[i];
if(!link.parent_joint)
continue;
const urdf::Joint& joint = *link.parent_joint;
QString joint_name = joint.name.c_str();
const KDL::Joint* kdlJoint = 0;
for(int i = 0; i < chain->getNrOfSegments(); ++i)
{
const KDL::Segment& segment = chain->getSegment(i);
if(segment.getJoint().getName() == joint.name)
{
kdlJoint = &segment.getJoint();
break;
}
}
if(!kdlJoint)
continue;
if(!specs.contains(joint_name))
{
ROS_ERROR("Unknown joint '%s'", joint.name.c_str());
return false;
}
const JointSpec& spec = specs[joint_name];
DXLJointInterface* iface = new DXLJointInterface(spec.id);
iface->setOffsetTicks(spec.offset);
if(!iface->init())
{
ROS_ERROR("Could not initialize joint interface for joint '%s'\n", joint.name.c_str());
return false;
}
if(spec.active)
{
SingleServoTestbench* tb = new SingleServoTestbench(iface, m_estimator, this);
m_ui->tabWidget->addTab(tb, joint_name);
tb->takeTrajectory(new TrajectoryPart(m_traj, spec.id));
tb->setObjectName(QString("Testbench_%1").arg(joint_name));
tb->loadSettings();
m_benches << tb;
m_jointNames << joint_name;
connect(tb, SIGNAL(trajectoryChanged()), SLOT(generateCommands()));
}
else
{
iface->setGoalPosition(0);
iface->setPValue(5);
delete iface;
}
}
generateCommands();
return true;
}
