bool TaskSpaceRegion::Initialize(EnvironmentBasePtr penv_in)
{
_volume = 0;
_sumbounds = 0;
_dimensionality = 0;
for(int i = 0; i < 6; i++)
{
//compute volume in whatever dimensionality this defines
//when Bw values are backwards, it signifies an axis flip (not an error)
if(Bw[i][1] != Bw[i][0])
{
_volume = _volume * fabs(Bw[i][1] - Bw[i][0]);
_sumbounds = _sumbounds + fabs(Bw[i][1] - Bw[i][0]);
_dimensionality++;
}
}
if( stricmp(relativebodyname.c_str(), "NULL") == 0 )
{
prelativetolink.reset();
}
else
{
KinBodyPtr pobject;
pobject = penv_in->GetKinBody(relativebodyname.c_str());
if(pobject.get() == NULL)
{
RAVELOG_INFO("Error: could not find the specified object to attach frame\n");
return false;
}
//find the link
vector<KinBody::LinkPtr> vlinks = pobject->GetLinks();
bool bGotLink = false;
for(int j =0; j < vlinks.size(); j++)
{
if(strcmp(relativelinkname.c_str(), vlinks[j]->GetName().c_str()) == 0 )
{
RAVELOG_INFO("frame link: %s:%s\n",vlinks[j]->GetParent()->GetName().c_str(),vlinks[j]->GetName().c_str() );
prelativetolink = vlinks[j];
bGotLink = true;
break;
}
}
if(!bGotLink)
{
RAVELOG_INFO("Error: could not find the specified link of the object to attach frame\n");
return false;
}
}
//Print();
return true;
}
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));
}
}
}
void TaskSpaceRegion::Print()
{
stringstream O;
O << endl;
O << "Manipulator Pointer: " << manipind << " ";
O << endl;
O << "Link Pointer: ";
if(prelativetolink.get() == NULL)
O << "NULL";
else
O << prelativetolink->GetParent()->GetName() << ":" << prelativetolink->GetName() << " ";
O << endl;
O << "T0_w:"<<endl;
O << T0_w.rot.x << " " << T0_w.rot.y << " " << T0_w.rot.z << " " << T0_w.rot.w << " ";
O << T0_w.trans.x << " " << T0_w.trans.y << " " << T0_w.trans.z << " ";
O << endl;
O << "Tw_e:"<<endl;
O << Tw_e.rot.x << " " << Tw_e.rot.y << " " << Tw_e.rot.z << " " << Tw_e.rot.w << " ";
O << Tw_e.trans.x << " " << Tw_e.trans.y << " " << Tw_e.trans.z << " ";
O << endl;
O << "Bw:"<<endl;
O << Bw[0][0] << " ";
O << Bw[0][1] << " ";
O << endl;
O << Bw[1][0] << " ";
O << Bw[1][1] << " ";
O << endl;
O << Bw[2][0] << " ";
O << Bw[2][1] << " ";
O << endl;
O << Bw[3][0] << " ";
O << Bw[3][1] << " ";
O << endl;
O << Bw[4][0] << " ";
O << Bw[4][1] << " ";
O << endl;
O << Bw[5][0] << " ";
O << Bw[5][1] << " ";
O << endl;
RAVELOG_INFO(O.str().c_str());
}
int CBirrtProblem::main(const std::string& cmd)
{
RAVELOG_DEBUG("env: %s\n", cmd.c_str());
const char* delim = " \r\n\t";
string mycmd = cmd;
char* p = strtok(&mycmd[0], delim);
if( p != NULL )
_strRobotName = p;
std::vector<RobotBasePtr> robots;
GetEnv()->GetRobots(robots);
SetActiveRobots(robots);
_pIkSolver = RaveCreateIkSolver(GetEnv(),"GeneralIK");
_pIkSolver->Init(robot->GetActiveManipulator());
RAVELOG_INFO("IKsolver initialized\n");
return 0;
}
bool teo::TeoSimRateThread::threadInit() {
printf("[TeoSimRateThread] begin: threadInit()\n");
jmcMs = this->getRate();
yarp::os::ConstString externObj = DEFAULT_EXTERN_OBJ;
if(externObj=="redCan") {
OpenRAVE::RaveLoadPlugin("ExternObj");
OpenRAVE::ModuleBasePtr pExternObj = RaveCreateModule(environmentPtr,"ExternObj"); // create the module
environmentPtr->Add(pExternObj,true); // load the module, calls main and also enables good destroy.
std::stringstream cmdin,cmdout;
cmdin << "Open"; // default maxiter:4000
RAVELOG_INFO("%s\n",cmdin.str().c_str());
if( !pExternObj->SendCommand(cmdout,cmdin) ) {
fprintf(stderr,"Bad send Open command.\n");
}
printf("Sent Open command.\n");
}
lastTime = yarp::os::Time::now();
printf("[TeoSimRateThread] end: threadInit()\n");
return true;
}
RAVE_PLUGIN_API void DestroyPlugin()
{
RAVELOG_INFO("destroying plugin\n");
}
virtual void demothread(int argc, char ** argv) {
string scenefilename = "/home/yo/asibot/main/app/ravebot/models/asibot_kitchen_wheelchair.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
KinBodyPtr objPtr = penv->GetKinBody("asibot");
if(objPtr) {
if(vrobots.at(1)->Grab(objPtr)) printf("[success] object asibot exists and grabbed.\n");
else printf("[warning] object asibot exists but not grabbed.\n");
} else printf("[fail] object asibot does not exist.\n");
printf("begin wait\n");
sleep(5);
printf("end wait\n");
RobotBasePtr probot = vrobots.at(1);
vector<dReal> v(2);
/* vector<int> vindices(probot->GetDOF());
for(size_t i = 0; i < vindices.size(); ++i) {
vindices[i] = i;
}
probot->SetActiveDOFs(vindices);*/
vector<int> vindices; // send it empty instead
//probot->SetActiveDOFs(vindices,DOF_X|DOF_Y|DOF_RotationAxis,Vector(0,0,1));
probot->SetActiveDOFs(vindices,DOF_X|DOF_Y,Vector(0,0,1));
ModuleBasePtr pbasemanip = RaveCreateModule(penv,"basemanipulation"); // create the module
penv->Add(pbasemanip,true,probot->GetName()); // load the module
int iteracion = 0;
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
// find a set of free joint values for the robot
{
RobotBase::RobotStateSaver saver(probot); // save the state
while(1) {
Transform T = probot->GetTransform();
if(iteracion%2) {
v[0] = T.trans.x;
v[1] = T.trans.y+1;
//v[2] = T.trans.z;
iteracion++;
} else {
v[0] = T.trans.x;
v[1] = T.trans.y-0.5;
//v[2] = T.trans.z;
iteracion++;
}
if(iteracion==5) vrobots.at(1)->Release(objPtr);
probot->SetActiveDOFValues(v);
if( !penv->CheckCollision(probot) && !probot->CheckSelfCollision() ) {
break;
}
}
// robot state is restored
}
stringstream cmdin,cmdout;
cmdin << "MoveActiveJoints maxiter 2000 maxtries 1 goal ";
for(size_t i = 0; i < v.size(); ++i) {
cmdin << v[i] << " ";
}
// start the planner and run the robot
RAVELOG_INFO("%s\n",cmdin.str().c_str());
if( !pbasemanip->SendCommand(cmdout,cmdin) ) {
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));
}
}
}
bool GeneralIK::_SolveTransThenRot(std::vector<dReal>& q_s)
{
if(bPRINT)
RAVELOG_INFO("Solving...\n");
//clear from previous run
_numitr = 0;
badjointinds.resize(0);
prev_error = 1000000.0;
//initialize stepsize and epsilon
maxstep = 0.1*_targtms.size();
stepsize = maxstep;
epsilon = 0.001;
bClearBadJoints = true; //setting this to true will make the algorithm attempt to move joints that are at joint limits at every iteration
if(bDRAW)
{
for(int i = 0; i < _targmanips.size();i++)
{
GetEnv()->plot3( &(DoubleVectorToFloatVector(_targtms[i].trans)[0]), 1, 0, 5, Vector(0,1,0) );
}
}
bBalanceGradient = false;
Vector perpvec;
//while(1)
for(int kk = 0; kk < 3000; kk++)
{
_numitr++;
bLimit = false;
_pRobot->SetActiveDOFValues(q_s);
if(bWRITETRAJ)
{
for(int i = 0; i < _pRobot->GetActiveDOF(); i++)
{
_trajpoint.q[i] = q_s[i];
}
ptraj->AddPoint(_trajpoint);
}
for(int i = 0; i < _targmanips.size();i++)
{
_pRobot->SetActiveManipulator(_targmanips[i]);
_curtms[i] = _pRobot->GetActiveManipulator()->GetEndEffectorTransform();
//since doing only translation, trick the solver into thinking it has achieved correct rotation
_curtms[i].rot = _targtms[i].rot;
if(bDRAW)
GetEnv()->plot3( &(DoubleVectorToFloatVector(_curtms[i].trans)[0]), 1, 0, 5, Vector(1,0,0) );
}
x_error = TransformDifferenceVectorized(dx.Store(),_targtms,_curtms);
if(bPRINT)
PrintMatrix(dx.Store(),1,dx.Nrows(),"dx: ");
if(bPRINT)
RAVELOG_INFO("x error: %f\n",x_error);
//if balance stuff is on, error will go up sometimes, so don't do this check
if(!bBalance && (x_error >= prev_error || (prev_error - x_error < epsilon/10))&& x_error > epsilon)
{
if(bPRINT)
{
RAVELOG_INFO("no progress\n");
RAVELOG_INFO("prev: %f x_err: %f limit %d\n",prev_error,x_error,(int)bLimit);
}
stepsize = stepsize/2;
x_error = prev_error;
q_s = q_s_old;
}
else
stepsize = maxstep;
if(bPRINT)
RAVELOG_INFO("stepsize: %f\n",stepsize);
//don't let step size get too small
if(stepsize < epsilon)
{
//RAVELOG_INFO("Projection stalled _numitr: %d\n",_numitr);
return false;
}
if(bClearBadJoints)
{
badjointinds.resize(0);
}
do{
q_s_old = q_s;
if(bLimit == false)
prev_error = x_error;
if(bBalance)
{
GetCOGJacobian(Transform(),Jtemp2,curcog);
}
if(x_error < epsilon && (!bBalance || CheckSupport(curcog)))
{
if(bPRINT)
RAVELOG_INFO("Translation component projection successfull _numitr: %d\n",_numitr);
return _SolveStopAtLimits(q_s);
}
//only need to compute the jacobian once if there are joint limit problems
if(bLimit == false)
{
for(int i = 0; i < _targmanips.size();i++)
{
_pRobot->SetActiveManipulator(_targmanips[i]);
GetFullJacobian(_curtms[i],_targtms[i],Jtemp);
Jtrans.Rows(i*3 +1,(i+1)*3) = Jtemp.Rows(1,3);
dx_trans.Rows(i*3 +1,(i+1)*3) = dx.Rows(1,3);
}
if(bBalance)
{
//the cog jacobian should only be 2 dimensional, b/c we don't care about z
GetCOGJacobian(Transform(),Jtemp2,curcog);
if(!CheckSupport(curcog))
{
bBalanceGradient = true;
balancedx[0] = (curcog.x - cogtarg.x);
balancedx[1] = (curcog.y - cogtarg.y);
}
else
{
balancedx[0] = 0;
balancedx[1] = 0;
bBalanceGradient = false;
}
}
}
//eliminate bad joint columns from the Jacobian
for(int j = 0; j < badjointinds.size(); j++)
for(int k = 0; k < _numtransdims; k++)
Jtrans[k][badjointinds[j]] = 0;
if(bBalance)
{
for(int j = 0; j < badjointinds.size(); j++)
for(int k = 0; k <2; k++)
Jtemp2[k][badjointinds[j]] = 0;
}
if(x_error > stepsize)
magnitude = stepsize/x_error;
else
magnitude = 1;
//NEWMAT::DiagonalMatrix Weights;
//Weights.ReSize(_numdofs);
//Weights = 1;
//J = J * Weights;
//PrintMatrix(J.Store(),_numtargdims,_numdofs,"J: ");
NEWMAT::DiagonalMatrix Reg(_numtransdims);
Reg = 0.0001;
M << (Jtrans*Jtrans.t()) + Reg;
//PrintMatrix(J.Store(),_numtargdims,_numtargdims,"M: ");
invConditioningBound(100,M,Minv);
//Minv = M.i();
//PrintMatrix(Minv.Store(),_numtargdims,_numtargdims,"Minv: ");
Jplus = Jtrans.t()*Minv;
if(bBalanceGradient)
{
Mbal << (Jtemp2*Jtemp2.t());
invConditioningBound(100,Mbal,Mbalinv);
//do ik, then move toward balance in null space
step = magnitude*Jplus*(dx_trans) + (NEWMAT::IdentityMatrix(_numdofs) - Jplus*Jtrans)*(Jtemp2.t()*Mbalinv)*(1*balancedx);
}
else
{
step = magnitude*Jplus*(dx_trans);
}
if(bPRINT)
PrintMatrix(step.Store(),1,step.Nrows(),"step: ");
//add step and check for joint limits
bLimit = false;
for(int i = 0; i < _numdofs; i++)
{
q_s[i] = q_s_old[i] - step[i];
if(q_s[i] < _lowerLimit[i] || q_s[i] > _upperLimit[i])
{
if(bPRINT)
RAVELOG_INFO("Jacobian going past joint limit. J%d: %f outside %f to %f\n",i,q_s[i],_lowerLimit[i],_upperLimit[i]);
if(q_s[i] < _lowerLimit[i])
q_s[i] = _lowerLimit[i];
if(q_s[i] > _upperLimit[i])
q_s[i] = _upperLimit[i];
badjointinds.push_back(i); //note this will never add the same joint twice, even if bClearBadJoints = false
bLimit = true;
}
}
//move back to previous point if any joint limits
if(bLimit)
{
q_s = q_s_old;
}
}while(bLimit);
if(bPRINT)
RAVELOG_INFO("after limits\n");
}
RAVELOG_INFO("Iteration limit reached\n");
return false;
}
