- Author
- Rosen Diankov
Robot moving in random configurations.
Shows how to send a cubicaly interpolated trajectory to the robot controller. The actual trajectory consists of two points: the current configuration and the target configuration. The viewer is loaded in the main thread while the computation happens in a new thread.
traj->Init(probot->GetActiveConfigurationSpecification());
probot->GetActiveDOFValues(q);
traj->Insert(0,q);
q[0] = 0.5;
traj->Insert(1,q);
The demo also adds a collision check at the target point to make sure robot is going to a collision free configuration.
{
RobotBase::RobotStateSaver saver(probot);
probot->SetDOFValues(q);
continue;
}
}
In order for the path itself to be collision free, we would have to use planners.
Full Example Code:
#include <vector>
#include <cstring>
#include <sstream>
using namespace OpenRAVE;
using namespace std;
namespace cppexamples {
class TrajectoryExample : public OpenRAVEExample
{
public:
virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/lab1.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
std::vector<dReal> q;
while(IsOk()) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
traj->Init(probot->GetActiveConfigurationSpecification());
probot->GetActiveDOFValues(q);
traj->Insert(0,q);
{
probot->SetDOFValues(q);
continue;
}
}
traj->Insert(1,q);
probot->GetController()->SetPath(traj);
}
while(!probot->GetController()->IsDone() && IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
}
};
}
int main(
int argc,
char ** argv)
{
return example.
main(argc,argv);
}