/** 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;
}
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
}
