static RobotPtr createRobot(WorldPtr world)
{
RobotPtr robot = boost::make_shared<Robot>(world);
robot->setInitPosition(25,25);
robot->setInitAngle(0);
return robot;
}
void RobotNode::updatePose(bool updateChildren)
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices();
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
// apply propagated joint values
if (propagatedJointValues.size()>0)
{
RobotPtr r = robot.lock();
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
while (it!=propagatedJointValues.end())
{
RobotNodePtr rn = r->getRobotNode(it->first);
if (!rn)
{
VR_WARNING << "Could not propagate joint value from " << name << " to " << it->first << " because dependent joint does not exist...";
} else
{
rn->setJointValue(jointValue*it->second);
}
it++;
}
}
}
bool RobotConfig::setJointValues( RobotPtr r )
{
if (!r)
return false;
WriteLockPtr lock = r->getWriteLock();
std::map < std::string, float > jv = getRobotNodeJointValueMap();
std::map< std::string, float >::const_iterator i = jv.begin();
// first check if all nodes are present
while (i != jv.end())
{
if (!r->hasRobotNode(i->first))
return false;
i++;
}
// apply jv
i = jv.begin();
while (i != jv.end())
{
RobotNodePtr rn = r->getRobotNode(i->first);
if (!rn)
return false;
rn->setJointValueNoUpdate(i->second);
i++;
}
r->applyJointValues();
return true;
}
bool RobotNode::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr> &children)
{
RobotPtr rob = robot.lock();
THROW_VR_EXCEPTION_IF(!rob, "Could not init RobotNode without robot" );
// robot
if (!rob->hasRobotNode(static_pointer_cast<RobotNode>(shared_from_this())))
rob->registerRobotNode(static_pointer_cast<RobotNode>(shared_from_this()));
// update visualization of coordinate systems
if (visualizationModel && visualizationModel->hasAttachedVisualization("CoordinateSystem"))
{
VisualizationNodePtr v = visualizationModel->getAttachedVisualization("CoordinateSystem");
// not needed any more!
// this is a little hack: The globalPose is used to set the "local" position of the attached Visualization:
// Since the attached visualizations are already positioned at the global pose of the visualizationModel,
// we just need the local postJointTransform
//v->setGlobalPose(postJointTransformation);
}
checkValidRobotNodeType();
for (size_t i=0;i<sensors.size();i++)
{
sensors[i]->initialize(shared_from_this());
}
return SceneObject::initialize(parent,children);
}
bool Environment::simulateAction(oomdp_msgs::SimulateAction::Request& req, oomdp_msgs::SimulateAction::Response& res)
{
// NB: For now, this method does not use the simulator, so it can only naively simulate the robot's movement
vector<EntityPtr> entities = Environment::makeEntityList(req.state.object_states);
// Find the robot:
RobotPtr robot = findRobot(entities);
if (robot == NULL)
{
ROS_ERROR("CANNOT SIMULATE STATE WITH NO ROBOT");
ROS_ERROR_STREAM(req.state);
res.new_state = req.state; // The state is unchanged
}
else
{
vector<EntityPtr> nonRobots;
for (vector<EntityPtr>::iterator ent_it = entities.begin(); ent_it != entities.end(); ++ent_it)
{
if ((*ent_it)->getClassString() != "Robot")
{
nonRobots.push_back(*ent_it);
}
}
robot->simulateAction(req.action, nonRobots);
}
res.new_state = Environment::makeState(entities);
return true;
}
std::vector<Eigen::Matrix4f > Trajectory::createWorkspaceTrajectory( VirtualRobot::RobotNodePtr r )
{
VR_ASSERT(rns);
if (!r)
r = rns->getTCP();
VR_ASSERT(r);
RobotPtr robot = rns->getRobot();
VR_ASSERT(robot);
Eigen::VectorXf jv;
rns->getJointValues(jv);
std::vector<Eigen::Matrix4f > result;
for (size_t i = 0; i < path.size(); i++)
{
// get tcp coords:
robot->setJointValues(rns,path[i]);
Eigen::Matrix4f m;
result.push_back(r->getGlobalPose());
}
robot->setJointValues(rns,jv);
return result;
}
std::string Trajectory::toXML(int tabs) const
{
std::stringstream ss;
std::string tab = "";
for (int i=0;i<tabs;i++)
tab += "\t";
RobotPtr robot = rns->getRobot();
THROW_VR_EXCEPTION_IF(( !robot || !rns),"Need a valid robot and rns");
ss << tab << "<Trajectory Robot='" << robot->getType() << "' RobotNodeSet='" << rns->getName() << "' name='" << name << "' dim='" << dimension << "'>\n";
for (unsigned int i = 0; i < path.size(); i++)
{
ss << tab << "\t<Point id='" << i << "'>\n";
Eigen::VectorXf c = path[i];
for (unsigned int k = 0; k < dimension; k++)
{
ss << tab << "\t\t<c value='" << c[k] << "'/>\n";
}
ss << tab << "\t</Point>\n";
}
ss << tab << "</Trajectory>\n";
return ss.str();
}
int main(int argc, char* argv[])
{
// Virtual Worldの作成
WorldPtr world = createWorld();
RobotPtr robot = createRobot(world);
// Agentの作成
ActionPtr action = createAction(robot);
DistSensorPtr sensor = createDistSensor(robot);
EnvironmentPtr env = createEnvironment(sensor);
ThreadedAgentPtr agent = createAgent(sensor, action);
// 初期化
env->init();
agent->init();
robot->init();
QApplication app(argc, argv);
MainWindow window;
window.setWorldModel(world);
window.setRobotModel(robot);
window.setEnvironment(env);
window.setAgent(agent);
window.show();
return app.exec();
}
RobotNodeSet::RobotNodeSet(const std::string &name,
RobotWeakPtr robot,
const std::vector< RobotNodePtr > &robotNodes,
const RobotNodePtr kinematicRoot /*= RobotNodePtr()*/,
const RobotNodePtr tcp /*= RobotNodePtr()*/)
:SceneObjectSet(name,robotNodes.size()>0?robotNodes[0]->getCollisionChecker():CollisionCheckerPtr())
{
this->robotNodes = robotNodes;
this->kinematicRoot = kinematicRoot;
this->tcp = tcp;
RobotPtr rob = robot.lock();
if (!kinematicRoot && robotNodes.size()>0)
this->kinematicRoot = rob->getRootNode();
if (!tcp && robotNodes.size()>0)
this->tcp = robotNodes[robotNodes.size()-1];
this->robot = robot;
// now, the objects are stored in the parent's (SceneObjectSet) data structure, so that the methods of SceneObjectSet do work
for(size_t i = 0; i < robotNodes.size(); i++)
{
SceneObjectPtr cm = boost::dynamic_pointer_cast<SceneObject>(robotNodes[i]);
if (cm)
{
if (colChecker != cm->getCollisionChecker())
{
VR_WARNING << "col model of " << robotNodes[i]->getName() << " belongs to different instance of collision checker, in: " << getName().c_str() << endl;
} else
{
sceneObjects.push_back(cm);
}
}
}
}
bool Environment::performAction(oomdp_msgs::PerformAction::Request& req, oomdp_msgs::PerformAction::Response& res)
{
std_srvs::Empty empty;
// TODO: If working, delete this dead code
// if (entities_.empty())
// {
// updateState();
// }
RobotPtr robot = findRobot(getEntityList());
if (robot == NULL)
{
ROS_ERROR("THERE IS NO ROBOT!");
return false;
}
if (req.action == "drop")
{
robot->drop();
}
else if (boost::starts_with(req.action, "pick_up"))
{
std::vector<std::string> strs;
boost::split(strs, req.action, boost::is_space());
std::string item_name = strs[1];
robot->pickUp(item_name, entity_list_);
}
else
{
ros::service::call("/gazebo/unpause_physics", empty);
move_base_client_->waitForServer();
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.frame_id = "/map";
goal.target_pose.pose = robot->computeTargetPose(req.action);
move_base_client_->sendGoal(goal); // Remember that the exact true position is at the mercy of the nav system
bool finished_before_timeout = move_base_client_->waitForResult(ros::Duration(30.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = move_base_client_->getState();
ROS_INFO("Action finished, client state: %s", state.toString().c_str());
}
else
{
ROS_INFO("Action did not finish before the time out.");
}
ros::service::call("/gazebo/pause_physics", empty);
}
res.new_state = updateState();
return true;
}
bool CoMIK::computeSteps(float stepSize, float minumChange, int maxNStep)
{
std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
RobotPtr robot = rns->getRobot();
std::vector<float> jv(rns->getSize(), 0.0f);
int step = 0;
checkTolerances();
float lastDist = FLT_MAX;
while (step < maxNStep)
{
Eigen::VectorXf dTheta = this->computeStep(stepSize);
// Check for singularities
if (!isValid(dTheta))
{
VR_INFO << "Singular Jacobian" << endl;
return false;
}
for (unsigned int i = 0; i < rn.size(); i++)
{
jv[i] = (rn[i]->getJointValue() + dTheta[i]);
}
//rn[i]->setJointValue(rn[i]->getJointValue() + dTheta[i]);
robot->setJointValues(rns, jv);
// check tolerances
if (checkTolerances())
{
VR_INFO << "Tolerances ok, loop:" << step << endl;
return true;
}
float d = dTheta.norm();
if (dTheta.norm() < minumChange)
{
VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << endl;
return false;
}
if (checkImprovement && d > lastDist)
{
VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << ", last loop's norm:" << lastDist << "), loop:" << step << endl;
return false;
}
lastDist = d;
step++;
}
#ifndef NO_FAILURE_OUTPUT
//VR_INFO << "IK failed, loop:" << step << endl;
//VR_INFO << "error:" << (target - rns->getCoM().head(target.rows())).norm() << endl;
#endif
return false;
}
void RobotNode::setJointValue(float q)
{
RobotPtr r = getRobot();
VR_ASSERT(r);
WriteLockPtr lock = r->getWriteLock();
setJointValueNoUpdate(q);
updatePose();
}
int main(int argc, char* argv[])
{
boost::shared_ptr<Robot> robot = RobotFactory::createRobot("Robbi");
std::vector< boost::shared_ptr<RobotNode> > robotNodes;
VirtualRobot::RobotNodeRevoluteFactory revoluteNodeFactory;
DHParameter dhParameter(0, 0, 0, 0, true);
boost::shared_ptr<RobotNode> node1 = revoluteNodeFactory.createRobotNodeDH(robot, "RootNode", VisualizationNodePtr(), CollisionModelPtr(), (float) - M_PI, (float)M_PI, 0.0f, dhParameter);
robotNodes.push_back(node1);
std::map<RobotNodePtr, std::vector<std::string> > childrenMap;
bool resInit = RobotFactory::initializeRobot(robot, robotNodes, childrenMap, node1);
cout << "resInit:" << resInit << endl;
cout << "First robot:" << endl;
robot->print();
VirtualRobot::RuntimeEnvironment::considerKey("robot");
VirtualRobot::RuntimeEnvironment::processCommandLine(argc, argv);
VirtualRobot::RuntimeEnvironment::print();
std::string filename("robots/examples/loadRobot/RobotExample.xml");
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
if (VirtualRobot::RuntimeEnvironment::hasValue("robot"))
{
std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot");
if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile))
{
filename = robFile;
}
}
cout << "Using robot at " << filename << endl;
RobotPtr rob;
try
{
rob = RobotIO::loadRobot(filename, RobotIO::eStructure);
}
catch (VirtualRobotException& e)
{
cout << "Error: " << e.what() << endl;
return -1;
}
cout << "Second robot (XML):" << endl;
if (rob)
{
rob->print();
}
else
{
cout << " ERROR while creating robot" << endl;
}
}
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< std::string > &robotNodeNames,
const std::string &kinematicRootName,
const std::string &tcpName,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
std::vector< RobotNodePtr > robotNodes;
if (robotNodeNames.empty())
{
VR_WARNING << " No robot nodes in set..." << endl;
}
else
{
for (unsigned int i=0;i<robotNodeNames.size();i++)
{
RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]);
THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found...");
robotNodes.push_back(node);
}
}
RobotNodePtr kinematicRoot;
if (!kinematicRootName.empty())
{
RobotNodePtr node = robot->getRobotNode(kinematicRootName);
THROW_VR_EXCEPTION_IF(!node, "No root node with name " << kinematicRootName << " found...");
kinematicRoot = node;
}
else
{
if (!robotNodes.empty())
kinematicRoot = robotNodes[0];
}
RobotNodePtr tcp;
if (!tcpName.empty())
{
RobotNodePtr node = robot->getRobotNode(tcpName);
THROW_VR_EXCEPTION_IF(!node, "No tcp node with name " << tcpName << " found...");
tcp = node;
}
else
{
if (!robotNodes.empty())
tcp = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, name, robotNodes, kinematicRoot, tcp, registerToRobot);
return rns;
}
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< RobotNodePtr > &robotNodes,
const RobotNodePtr kinematicRoot,
const RobotNodePtr tcp,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
if (robotNodes.empty() || !robotNodes[0])
{
VR_WARNING << " No robot nodes in set..." << endl;
} else
{
CollisionCheckerPtr cc = robotNodes[0]->getCollisionChecker();
for (unsigned int i=1;i<robotNodes.size();i++)
{
if (robotNodes[0]->getRobot() != robotNodes[i]->getRobot())
{
THROW_VR_EXCEPTION("Robot nodes belong to different robots");
}
if (cc != robotNodes[i]->getCollisionChecker())
{
THROW_VR_EXCEPTION("Robot nodes belong to different collision checkers");
}
}
}
RobotNodePtr kinematicRootNode = kinematicRoot;
if (!kinematicRootNode)
{
kinematicRootNode = robot->getRootNode();
}
RobotNodePtr tcpNode = tcp;
if (!tcpNode)
{
THROW_VR_EXCEPTION_IF(robotNodes.empty(), "can not determine the tcp node need for creating a RobotNodeSet");
tcpNode = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns(new RobotNodeSet(name, robot, robotNodes, kinematicRootNode, tcpNode));
if (registerToRobot)
{
THROW_VR_EXCEPTION_IF(robot->hasRobotNodeSet(rns), "RobotNodeSet with name " << rns->getName() << " already present in the robot");
robot->registerRobotNodeSet(rns);
}
return rns;
}
void RobotNode::updateTransformationMatrices()
{
if (this->getParent())
updateTransformationMatrices(this->getParent()->getGlobalPose());
else
{
// check for root
RobotPtr r = getRobot();
if (r && r->getRootNode() == shared_from_this())
updateTransformationMatrices(r->getGlobalPose());
else
updateTransformationMatrices(Eigen::Matrix4f::Identity());
}
}
void RobotNodeSet::setJointValues(const Eigen::VectorXf &jointValues)
{
THROW_VR_EXCEPTION_IF(jointValues.rows() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
RobotPtr rob = robot.lock();
VR_ASSERT(rob);
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i=0;i<robotNodes.size();i++)
{
robotNodes[i]->setJointValueNoUpdate(jointValues[i]);
}
if (kinematicRoot)
kinematicRoot->updatePose();
else
rob->applyJointValues();
}
bool RobotConfig::setConfig( RobotNodePtr node, float value )
{
THROW_VR_EXCEPTION_IF(!node,"Null data");
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted, skipping update..." << endl;
return false;
}
THROW_VR_EXCEPTION_IF(!r->hasRobotNode(node),"RobotNode with name " << r->getName() << " does not belong to robot " << r->getName());
configs[node] = value;
return true;
}
bool RobotConfig::setJointValues()
{
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted, skipping update..." << endl;
return false;
}
WriteLockPtr lock = r->getWriteLock();
for (std::map< RobotNodePtr, float >::const_iterator i=configs.begin(); i!=configs.end(); i++ )
{
i->first->setJointValueNoUpdate(i->second);
}
r->applyJointValues();
return true;
}
bool RobotConfig::setConfig( const std::string &node, float value )
{
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted, skipping update..." << endl;
return false;
}
RobotNodePtr rn = r->getRobotNode(node);
if (!rn)
{
VR_WARNING << "Did not find robot node with name " << node << endl;
return false;
}
configs[rn] = value;
return true;
}
Eigen::Matrix4f LinkedCoordinate::getCoordinateTransformation(const RobotNodePtr &origin,
const RobotNodePtr &destination, const RobotPtr &robot){
if (!destination)
THROW_VR_EXCEPTION(format("Destination RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!origin)
THROW_VR_EXCEPTION(format("Origin RobotNodePtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!robot)
THROW_VR_EXCEPTION(format("RobotPtr not assigned (LinkedCoordinate::%1%)") % BOOST_CURRENT_FUNCTION);
if (!robot->hasRobotNode( origin) )
THROW_VR_EXCEPTION(format("Origin robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % origin->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
if (!robot->hasRobotNode( destination) )
THROW_VR_EXCEPTION(format("Destination robot node\"%1%\" not a member of robot \"%2%\" (LinkedCoordinate::%3%)") % destination->getName() % robot->getName() % BOOST_CURRENT_FUNCTION);
// std::cout << "Destination: " << destination->getName() <<std::endl << "Origin: " << origin->getName() << std::endl;
// std::cout << "Destination:\n" << destination->getGlobalPose() <<std::endl << "Origin:\n" << origin->getGlobalPose() << std::endl;
return destination->getGlobalPose().inverse() * origin->getGlobalPose();
}
void GenericIKSolver::setJointsRandom()
{
std::vector<float> jv;
float rn = 1.0f / (float)RAND_MAX;
for (unsigned int i=0; i<rns->getSize(); i++)
{
RobotNodePtr ro = rns->getNode(i);
float r = (float)rand() * rn;
float v = ro->getJointLimitLo() + (ro->getJointLimitHi() - ro->getJointLimitLo()) * r;
jv.push_back(v);
}
RobotPtr rob = rns->getRobot();
rob->setJointValues(rns,jv);
if (translationalJoint)
{
translationalJoint->setJointValue(initialTranslationalJointValue);
}
}
vector<EntityPtr> Environment::makeEntityList(vector<MDPObjectState> states)
{
vector<EntityPtr> entities;
for (vector<MDPObjectState>::const_iterator obj_it = states.begin(); obj_it != states.end(); ++obj_it)
{
entities.push_back(wubble_mdp::makeEntity(*obj_it));
}
for (vector<EntityPtr>::iterator ent_it = entities.begin(); ent_it != entities.end(); ++ent_it)
{
if ((*ent_it)->getClassString() == "Robot")
{
RobotPtr robot = boost::dynamic_pointer_cast<Robot>(*ent_it);
robot->init(entities); // TODO: Does this need to be somewhere else too?
}
}
return entities;
}
void RobotNodeSet::setJointValues( const RobotConfigPtr jointValues )
{
VR_ASSERT(jointValues);
RobotPtr rob = robot.lock();
VR_ASSERT(rob);
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i=0;i<robotNodes.size();i++)
{
if (jointValues->hasConfig(robotNodes[i]->getName()))
robotNodes[i]->setJointValueNoUpdate(jointValues->getConfig(robotNodes[i]->getName()));
}
if (kinematicRoot)
kinematicRoot->updatePose();
else
rob->applyJointValues();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "parser_test");
ros::NodeHandle nh;
try
{
std::string name = "red_arm";
RobotPtr robot = std::make_shared<Robot>(name);
ParserPtr parser = std::make_shared<Parser>();
std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/red_arm.yaml";
parser->load(path, robot);
ros::MultiThreadedSpinner spinner;
TfPublisherPtr tf_publisher = std::make_shared<TfPublisher>();
const std::string mnp_name = "mnp";
unsigned long cnt = 0;
ros::Rate r(10.0);
while(ros::ok())
{
Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF(mnp_name));
ManipulatorPtr mnp = robot->getManipulator(mnp_name);
double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1);
++cnt;
for(uint32_t i = 0; i < q.rows(); ++i)
{
q.coeffRef(6) = M_PI / 4.0 * goal;
}
std::cout << M_PI / 4.0 * goal << std::endl;
//q = Eigen::VectorXd::Constant(q.rows(), 0.0 * M_PI / 4.0);
robot->update(q);
robot->computeJacobian(mnp_name);
robot->computeMassMatrix(mnp_name);
M = robot->getMassMatrix(mnp_name);
J = robot->getJacobian(mnp_name, "gripper");
calc();
tf_publisher->publish(robot, false);
r.sleep();
}
}
catch(ahl_utils::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
return 0;
}
float RobotConfig::getConfig( const std::string & name ) const
{
if (!hasConfig(name))
return 0.0f;
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted..." << endl;
return 0.0f;
}
RobotNodePtr rn = r->getRobotNode(name);
THROW_VR_EXCEPTION_IF(!rn,"Did not find robot node with name " << name);
std::map< RobotNodePtr, float >::const_iterator i = configs.find(rn);
if (i==configs.end())
{
VR_ERROR << "Internal error..." << endl;
return 0.0f;
}
return i->second;
}
void RobotNode::updateVisualizationPose( const Eigen::Matrix4f &globalPose, bool updateChildren )
{
// check if we are a root node
SceneObjectPtr parent = getParent();
RobotPtr rob = getRobot();
if (!parent || parent == rob)
{
if (rob && rob->getRootNode() == static_pointer_cast<RobotNode>(shared_from_this()))
{
Eigen::Matrix4f gpPre = getLocalTransformation().inverse() * globalPose;
rob->setGlobalPose(gpPre, false);
} else
VR_WARNING << "INTERNAL ERROR: getParent==robot but getRoot!=this ?! " << endl;
}
this->globalPose = globalPose;
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "lwr_server");
ros::NodeHandle nh;
ros::Timer timer_update_model = nh.createTimer(ros::Duration(0.01), updateModel);
ros::Timer timer_control = nh.createTimer(ros::Duration(0.001), control);
robot = RobotPtr(new Robot("lwr"));
ParserPtr parser = ParserPtr(new Parser());
std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/lwr.yaml";
parser->load(path, robot);
controller = RobotControllerPtr(new RobotController());
controller->init(robot);
using namespace ahl_gazebo_if;
gazebo_interface = GazeboInterfacePtr(new GazeboInterface());
gazebo_interface->setDuration(0.010);
gazebo_interface->addJoint("lwr::joint1");
gazebo_interface->addJoint("lwr::joint2");
gazebo_interface->addJoint("lwr::joint3");
gazebo_interface->addJoint("lwr::joint4");
gazebo_interface->addJoint("lwr::joint5");
gazebo_interface->addJoint("lwr::joint6");
gazebo_interface->addJoint("lwr::joint7");
gazebo_interface->connect();
tf_pub = TfPublisherPtr(new TfPublisher());
ManipulatorPtr mnp = robot->getManipulator("mnp");
gravity_compensation = TaskPtr(new GravityCompensation(robot));
damping = TaskPtr(new Damping(mnp));
joint_control = TaskPtr(new JointControl(mnp));
joint_limit = TaskPtr(new JointLimit(mnp, 0.087));
position_control = TaskPtr(new PositionControl(mnp, "gripper", 0.001));
orientation_control = TaskPtr(new OrientationControl(mnp, "gripper", 0.001));
controller->addTask(gravity_compensation, 0);
controller->addTask(damping, 0);
controller->addTask(joint_control, 0);
controller->addTask(joint_limit, 100);
ros::MultiThreadedSpinner spinner;
spinner.spin();
return 0;
}
void updateModel(const ros::TimerEvent&)
{
try
{
boost::mutex::scoped_lock lock(mutex);
if(joint_updated)
{
robot->computeBasicJacobian("mnp");
robot->computeMassMatrix("mnp");
controller->updateModel();
updated = true;
tf_pub->publish(robot);
}
}
catch(ahl_ctrl::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
catch(ahl_gazebo_if::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
}
void RobotWorld::deleteRobot( RobotPtr aRobot,
bool aNotifyObservers /*= true*/)
{
auto i = std::find_if( robots.begin(), robots.end(), [aRobot](RobotPtr r)
{
return aRobot->getName() == r->getName();
});
if (i != robots.end())
{
robots.erase( i);
if (aNotifyObservers == true)
{
notifyObservers();
}
}
}
