virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/pa10grasp2.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
RobotBasePtr probot = vrobots.at(0);
// find a manipulator chain to move
for(size_t i = 0; i < probot->GetManipulators().size(); ++i) {
if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) {
probot->SetActiveManipulator(probot->GetManipulators()[i]);
break;
}
}
RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
// load inverse kinematics using ikfast
ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
vector<dReal> vsolution;
ssin << "LoadIKFastSolver " << probot->GetName() << " " << (int)IKP_Transform6D;
if( !pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
}
if( !pmanip->GetIkSolver()) {
throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert);
}
ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
penv->Add(pbasemanip,true,probot->GetName()); // load the module
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
// find a new manipulator position and feed that into the planner. If valid, robot will move to it safely.
Transform t = pmanip->GetEndEffectorTransform();
t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f);
t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f));
ssin.str("");
ssin.clear();
ssin << "MoveToHandPosition pose " << t;
// start the planner and run the robot
RAVELOG_INFO("%s\n",ssin.str().c_str());
if( !pbasemanip->SendCommand(ssout,ssin) ) {
continue;
}
}
// unlock the environment and wait for the robot to finish
while(!probot->GetController()->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
static bool isClosed(RobotBase::ManipulatorPtr manip, float closedThreshold) {
vector<int> gripperInds = manip->GetGripperIndices();
manip->GetRobot()->SetActiveDOFs(gripperInds);
vector<double> dof_values;
manip->GetRobot()->GetActiveDOFValues(dof_values);
bool out =dof_values[0] < closedThreshold;
cout << "isclosed: " << out << endl;
return out;
}
/** Get pointers to the robot and manipulator in the viewer_env, and
* clone the environment.
*/
void
JacoOpenraveThread::_post_init()
{
#ifdef HAVE_OPENRAVE
while( !__robot ) {
__robot = __viewer_env.robot->get_robot_ptr();
usleep(100);
}
while( !__manip ) {
EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
__manip = __robot->SetActiveManipulator(__cfg_manipname);
usleep(100);
}
// create cloned environment for planning
logger->log_debug(name(), "Clone environment for planning");
openrave->clone(__planner_env.env, __planner_env.robot, __planner_env.manip);
if( !__planner_env.env || !__planner_env.robot || !__planner_env.manip) {
throw fawkes::Exception("Could not clone properly, received a NULL pointer");
}
// set name of environment
switch( __arm->config ) {
case CONFIG_SINGLE:
__planner_env.env->set_name("Planner");
break;
case CONFIG_LEFT:
__planner_env.env->set_name("Planner_Left");
break;
case CONFIG_RIGHT:
__planner_env.env->set_name("Planner_Right");
break;
}
// set active manipulator in planning environment
{
EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname);
__planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
}
// Get chain of links from arm base to manipulator in viewer_env. Used for plotting joints
__robot->GetChain( __manip->GetBase()->GetIndex(), __manip->GetEndEffector()->GetIndex(), __links);
#endif //HAVE_OPENRAVE
}
bool ProblemBuilder::ReadBasicInfo(const Value& v) {
FAIL_IF_FALSE(v.isMember("n_steps"));
FAIL_IF_FALSE(v.isMember("manip"));
vector<RobotBasePtr> robots;
m_env->GetRobots(robots);
RobotBasePtr robot = robots[0];
IPI_LOG_WARNING("using first robot in environment");
RobotBase::ManipulatorPtr manip = GetManipulatorByName(*robot, v["manip"].asString());
// todo: support various other dof collections that aren't manipulators
IntVec joint_inds = manip->GetArmIndices();
m_rad = RobotAndDOFPtr(new RobotAndDOF(robot, joint_inds));
int n_dof = joint_inds.size();
int n_steps = v["n_steps"].asInt();
DblVec lower, upper;
m_rad->GetDOFLimits(lower, upper);
vector<double> vlower, vupper;
vector<string> names;
for (int i=0; i < n_steps; ++i) {
vlower.insert(vlower.end(), lower.data(), lower.data()+lower.size());
vupper.insert(vupper.end(), upper.data(), upper.data()+upper.size());
for (unsigned j=0; j < n_dof; ++j) {
names.push_back( (boost::format("j_%i_%i")%i%j).str() );
}
}
m_prob->createVariables(names, vlower, vupper);
m_prob->m_traj_vars = VarArray(n_steps, n_dof, m_prob->vars_.data());
DblVec cur_dofvals = m_rad->GetDOFValues();
// fix first timestep vars
for (int j=0; j < n_dof; ++j) {
m_prob->addLinearConstr(exprSub(AffExpr(m_prob->m_traj_vars(0,j)), cur_dofvals[j]), EQ);
}
return true;
}
int main(int argc, char ** argv)
{
if( argc < 3 ) {
RAVELOG_INFO("ikloader robot iktype\n");
return 1;
}
string robotname = argv[1];
string iktype = argv[2];
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
{
// lock the environment to prevent changes
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
// load the scene
RobotBasePtr probot = penv->ReadRobotXMLFile(robotname);
if( !probot ) {
penv->Destroy();
return 2;
}
penv->Add(probot);
ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype;
// if necessary, add free inc for degrees of freedom
//ssin << " " << 0.04f;
// get the active manipulator
RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
if( !pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
penv->Destroy();
return 1;
}
RAVELOG_INFO("testing random ik\n");
while(1) {
Transform trans;
trans.rot = quatFromAxisAngle(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5));
trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2;
vector<dReal> vsolution;
if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) {
stringstream ss; ss << "solution is: ";
for(size_t i = 0; i < vsolution.size(); ++i) {
ss << vsolution[i] << " ";
}
ss << endl;
RAVELOG_INFO(ss.str());
}
else {
// could fail due to collisions, etc
}
}
}
RaveDestroy(); // destroy
return 0;
}
void
JacoOpenraveThread::_plan_path(RefPtr<jaco_target_t> &from, RefPtr<jaco_target_t> &to)
{
#ifdef HAVE_OPENRAVE
// update state of the trajectory
__arm->target_mutex->lock();
to->trajec_state = TRAJEC_PLANNING;
__arm->target_mutex->unlock();
// Update bodies in planner-environment
// clone robot state, ignoring grabbed bodies
{
EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
EnvironmentMutex::scoped_lock plan_lock(__planner_env.env->get_env_ptr()->GetMutex());
__planner_env.robot->get_robot_ptr()->ReleaseAllGrabbed();
__planner_env.env->delete_all_objects();
/*
// Old method. Somehow we encountered problems. OpenRAVE internal bug?
RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(),
0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF);
saver.Restore( __planner_env.robot->get_robot_ptr() );
*/
// New method. Simply set the DOF values as they are in __viewer_env
vector<dReal> dofs;
__viewer_env.robot->get_robot_ptr()->GetDOFValues(dofs);
__planner_env.robot->get_robot_ptr()->SetDOFValues(dofs);
}
// then clone all objects
__planner_env.env->clone_objects( __viewer_env.env );
// restore robot state
{
EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
// Set active manipulator and active DOFs (need for planner and IK solver!)
RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname);
__planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
// update robot state with attached objects
{
EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
/*
// Old method. Somehow we encountered problems. OpenRAVE internal bug?
RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(),
KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies);
saver.Restore( __planner_env.robot->get_robot_ptr() );
*/
// New method. Grab all bodies in __planner_env that are grabbed in __viewer_env by this manipulator
vector<RobotBase::GrabbedInfoPtr> grabbed;
__viewer_env.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
for( vector<RobotBase::GrabbedInfoPtr>::iterator it=grabbed.begin(); it!=grabbed.end(); ++it ) {
logger->log_debug(name(), "compare _robotlinkname '%s' with our manip link '%s'",
(*it)->_robotlinkname.c_str(), manip->GetEndEffector()->GetName().c_str());
if( (*it)->_robotlinkname == manip->GetEndEffector()->GetName() ) {
logger->log_debug(name(), "attach '%s'!", (*it)->_grabbedname.c_str());
__planner_env.robot->attach_object((*it)->_grabbedname.c_str(), __planner_env.env, __cfg_manipname.c_str());
}
}
}
}
// Set target point for planner. Check again for IK, avoiding collisions with the environment
//logger->log_debug(name(), "setting target %f %f %f %f %f %f",
// to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5));
__planner_env.robot->enable_ik_comparison(true);
if( to->type == TARGET_CARTESIAN ) {
if( !__planner_env.robot->set_target_euler(EULER_ZXZ, to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5)) ) {
logger->log_warn(name(), "Planning failed, second IK check failed");
__arm->target_mutex->lock();
to->trajec_state = TRAJEC_PLANNING_ERROR;
__arm->target_mutex->unlock();
return;
} else {
// set target angles. This changes the internal target type to ANGLES (see openrave/robot.*)
// and will use BaseManipulation's MoveActiveJoints. Otherwise it will use MoveToHandPosition,
// which does not have the filtering of IK solutions for the closest one as we have.
vector<float> target;
__planner_env.robot->get_target().manip->get_angles(target);
__planner_env.robot->set_target_angles(target);
}
} else {
vector<float> target;
//TODO: need some kind cheking for env-collision, i.e. if the target is colllision-free.
// For now expect the user to know what he does, when he sets joint angles directly
__planner_env.robot->get_target().manip->set_angles_device(to->pos);
__planner_env.robot->get_target().manip->get_angles(target);
__planner_env.robot->set_target_angles(target);
}
// Set starting point for planner
//logger->log_debug(name(), "setting start %f %f %f %f %f %f",
// from->pos.at(0), from->pos.at(1), from->pos.at(2), from->pos.at(3), from->pos.at(4), from->pos.at(5));
__planner_env.manip->set_angles_device(from->pos);
// Set planning parameters
__planner_env.robot->set_target_plannerparams(__plannerparams);
// Run planner
try {
__planner_env.env->run_planner(__planner_env.robot, __cfg_OR_sampling);
} catch (fawkes::Exception &e) {
logger->log_warn(name(), "Planning failed: %s", e.what_no_backtrace());
// TODO: better handling!
// for now just skip planning, so the target_queue can be processed
__arm->target_mutex->lock();
//to->type = TARGET_ANGULAR;
to->trajec_state = TRAJEC_PLANNING_ERROR;
__arm->target_mutex->unlock();
return;
}
// add trajectory to queue
//logger->log_debug(name(), "plan successful, adding to queue");
__arm->trajec_mutex->lock();
// we can do the following becaouse get_trajectory_device() returns a new object, thus
// can be safely deleted by RefPtr auto-deletion
to->trajec = RefPtr<jaco_trajec_t>( __planner_env.robot->get_trajectory_device() );
__arm->trajec_mutex->unlock();
// update target.
__arm->target_mutex->lock();
//change target type to ANGULAR and set target->pos accordingly. This makes final-checking
// in goto_thread much easier
to->type = TARGET_ANGULAR;
to->pos = to->trajec->back();
// update trajectory state
to->trajec_state = TRAJEC_READY;
__arm->target_mutex->unlock();
#endif
}
