Example #1
0
//computes new configuration for urTobj
Q computenewQ(Q qin, Transform3D<> Tin, Transform3D<> Tdes, Device::Ptr device, State state,Frame* marker,Frame* bottleframe){
	//set device state
	//Q home(6, 1.6765, -2.0368, 1.5597, -1.6505, -1.6474, -1.6449);
	device->setQ(qin,state);
	//compute the small iteration, show error
	VelocityScrew6D<> deltau=computeG(Tin,Tdes);
	//do while error larger than threshold
	while (deltau.norm2()>epsilon && flag==0){
		//get Jacobian
		Jacobian J=device->baseJframe(marker,state);
		//calcultate dq
//		Q dq (LinearAlgebra::inverse(J.e())*deltau.e());
		Q dq (J.e().inverse()*deltau.e());
		//add dq to q
//		Q dq(6,1,2,3,4,5,6);
		qin=qin+dq;
		//qin=0;
		//set new q
		device->setQ(qin,state);
		//get new transform
		Transform3D<> bThan = device->baseTframe(marker,state);
		Tin=bThan;
		//compute and show new deltau, error
		deltau=computeG(Tin,Tdes);
		T2=clock();
		Ttotal=T2-T1;
		sec += (Ttotal/CLOCKS_PER_SEC);
		if(sec>30)
		{
			flag=1;
		}
	}
	//return final Q
	return(qin);
}
void printDevice(Device::Ptr device)
{
    std::cout << "device - [id: " << device->deviceId()
        << "][device type: " << device->deviceType()
        << "][ip address: " << device->ipAddress()
        << "][binding id: " << device->bindingId()
        << "]" << std::endl;
}
Example #3
0
bool receiveXYZRPY(kinematics::KinXYZRPY::Request &req, kinematics::KinXYZRPY::Response &res){
	//ROS_INFO("xyz (%0.2f, %0.2f, %0.2f), RPY (%0.2f, %0.2f, %0.2f)", req.x, req.y, req.z, req.R, req.P, req.Y);
	if (req.from_current){
		device->setQ(positions, state);
	}
	Transform3D<double> Tcurrent, Tdesired;
	Tcurrent = device->baseTend(state);
	Tdesired.P() = Vector3D<double>(req.x, req.y, req.z);
	Tdesired.R() = RPY<double>(req.R*Pi/180, req.P*Pi/180, req.Y*Pi/180).toRotation3D();
	res.success = sendConfigurations(Tcurrent, Tdesired, state, req.interpolate, req.force_angle, req.angle);
	return true;
}
Example #4
0
void calculatePose(Q q){
	State tmpState = wc->getDefaultState();
	device->setQ(q, tmpState);
	Transform3D<double> cPose = device->baseTend(tmpState);
	RPY<double> rpy(cPose.R());
	pose.x = cPose.P()[0];
	pose.y = cPose.P()[1];
	pose.z = cPose.P()[2];
	pose.R = rpy(0)*180/Pi;
	pose.P = rpy(1)*180/Pi;
	pose.Y = rpy(2)*180/Pi;

	//cout << "POOOOOSE: " << cPose.P() << "\n";
	kinematics::Pose msg;
	msg.x = cPose.P()[0];
	msg.y = cPose.P()[1];
	msg.z = cPose.P()[2];
	msg.R = rpy(0)*180/Pi;
	msg.P = rpy(1)*180/Pi;
	msg.Y = rpy(2)*180/Pi;
	pose_pub.publish(msg);
}
Example #5
0
void
Cucs_processor::On_register_vehicle(Request::Ptr request, Device::Ptr vehicle)
{
    auto device_id = vehicle->Get_session_id();
    if (vehicles.find(device_id) != vehicles.end()) {
        VSM_EXCEPTION(Exception, "Vehicle %d already registered", device_id);
    }

    auto res = vehicles.emplace(device_id, Vehicle_context());
    auto &ctx = res.first->second;
    ctx.vehicle = vehicle;

    ctx.registration_message.set_device_id(device_id);

    vehicle->Register(ctx.registration_message);

    // LOG("Vehicle registered %s",ctx.registration_message.DebugString().c_str());

    request->Complete();

    Broadcast_message_to_ucs(ctx.registration_message);
}
Example #6
0
//compute urTcam transform
Transform3D<> find_urTcam(Device::Ptr device, State state,Frame* camera){
	tf::TransformListener listen_ucam;
	tf::StampedTransform ucam_transform[5];
	Transform3D<> urTcam;
	bool ucam=true;
	int i=0;
	try{
		  listen_ucam.waitForTransform("/arm_base_link", "/head_cam3d_link",ros::Time(0),ros::Duration(5.0));
		  for(i=0;i<5;i++)
		  {
			  listen_ucam.lookupTransform("/arm_base_link", "/head_cam3d_link",ros::Time(0), ucam_transform[i]);
		  }

	    }
	    catch (tf::TransformException ex){
		      ROS_ERROR("%s",ex.what());
		      cout<<"Using RW values for urTcam.\n";
		      ucam=false;
	    }
	    if(ucam)
	    {
	    	float vec_X=0;
	    	float vec_Y=0;
	    	float vec_Z=0;
	    	float rot_X=0;
	    	float rot_Y=0;
	    	float rot_Z=0;
	    	float rot_W=0;
	    	for(i=0;i<5;i++)
	    	{
	    		vec_X+=ucam_transform[i].getOrigin().getX();
	    		vec_Y+=ucam_transform[i].getOrigin().getY();
	    		vec_Z+=ucam_transform[i].getOrigin().getZ();
	    		rot_X+=ucam_transform[i].getRotation().getX();
	    		rot_Y+=ucam_transform[i].getRotation().getY();
	    		rot_Z+=ucam_transform[i].getRotation().getZ();
	    		rot_W+=ucam_transform[i].getRotation().getW();
	    	}

	    	Vector3D<> ucam_vec(vec_X/5,vec_Y/5,vec_Z/5);
	    	Quaternion<>ucam_q (rot_X/5,rot_Y/5,rot_Z/5,rot_W/5);
	    	Rotation3D<> ucam_rot(ucam_q.toRotation3D());
	    	Transform3D<> urcam(ucam_vec,ucam_rot);
	    	urTcam=urcam;
	    }
	    else
	    {
	    	urTcam=device->baseTframe(camera,state);
	    }
	 return urTcam;
}
Example #7
0
bool receiveJog(kinematics::KinXYZRPY::Request &req, kinematics::KinXYZRPY::Response &res){
	device->setQ(positions, state);
	cout << "POSITIONS: " << positions << "\n";
	Transform3D<double> Tcurrent = device->baseTend(state);
	Transform3D<double> Tdesired = device->baseTend(state);
	Tdesired.P() += Vector3D<double>(req.x, req.y, req.z);
	//Rotation3D<double> rot = RPY<double>(req.R, req.P, req.Y).toRotation3D();
	/*
	Tdesired.R() = Rotation3D<double>(
		Tcurrent.R(0,0)+rot(0,0),
		Tcurrent.R(0,1)+rot(0,1),
		Tcurrent.R(0,2)+rot(0,2),
		Tcurrent.R(1,0)+rot(1,0),
		Tcurrent.R(1,1)+rot(1,1),
		Tcurrent.R(1,2)+rot(1,2),
		Tcurrent.R(2,0)+rot(2,0),
		Tcurrent.R(2,1)+rot(2,1),
		Tcurrent.R(2,1)+rot(2,2),
		);
		*/
	res.success = sendConfigurations(Tcurrent, Tdesired, state, req.interpolate);
	return true;
}
Example #8
0
void posCallback(const pa10_controller::Positions::ConstPtr& msg){
   //ROS_INFO("I HEARD: %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f", msg->positions[0],msg->positions[1],msg->positions[2],msg->positions[3],msg->positions[4],msg->positions[5],msg->positions[6]);
	if (positions.size() == 0){
		positions = Q(7,0,0,0,0,0,0,0);
		for (int i = 0; i < (int) msg->positions.size(); i ++){
			positions[i] = msg->positions[i]*Pi/180;
		}
		device->setQ(positions, state);
	}else{
		for (int i = 0; i < (int) msg->positions.size(); i ++){
			positions[i] = msg->positions[i]*Pi/180;
		}
	}
	calculatePose(positions);
}
Example #9
0
bool checkCollisions(Device::Ptr device, const State &state, const CollisionDetector &detector, const Q &q) {
	State testState;
	CollisionDetector::QueryResult data;
	bool colFrom;

	testState = state;
	device->setQ(q,testState);
	colFrom = detector.inCollision(testState,&data);
	if (colFrom) {
		cerr << "Configuration in collision: " << q << endl;
		cerr << "Colliding frames: " << endl;
		FramePairSet fps = data.collidingFrames;
		for (FramePairSet::iterator it = fps.begin(); it != fps.end(); it++) {
			cerr << (*it).first->getName() << " " << (*it).second->getName() << endl;
		}
		return false;
	}
	return true;
}
Example #10
0
int main(int argc, char **argv){
	ros::init(argc, argv, "kinematics_node");
	ROS_INFO("kinematics_node started");
	ros::NodeHandle n, nh("~");

	//string wcFile = "/home/ege/ros/sandbox/kinematics/Scene/Scene.wc.xml";
	//string deviceName = "PA10";
	string wcFile, deviceName, xyzrpyService, qService, posTopic, jogService, poseTopic;

	nh.param<string>("workcell", wcFile, "/home/ege/ros/sandbox/kinematics/Scene/Scene.wc.xml");
	nh.param<string>("device", deviceName, "PA10");
	nh.param<string>("xyzrpy_service", xyzrpyService, "/pa10_xyzRPY");
	nh.param<string>("jog_service", jogService, "/pa10_jog");
	nh.param<string>("q_service", qService, "/pa10_Qs");
	nh.param<string>("pos_topic", posTopic, "/pa10_positions");
	nh.param<string>("pose_topic", poseTopic, "/pa10_pose");
	nh.param<double>("interpolate_stepsize", interpolateStepSize, 0.1);

	ROS_INFO("Trying to use workcell: %s and device %s" , wcFile.c_str(), deviceName.c_str());
	wc=WorkCellLoader::load(wcFile);
	device=wc->findDevice(deviceName);
	state = wc->getDefaultState();
	Q qStart(7,0,0,0,0,0,0,0);
	device->setQ(qStart, state);

	ros::Subscriber sub = n.subscribe(posTopic, 1, posCallback);
	pose_pub = n.advertise<kinematics::Pose>(poseTopic, 10);
	ros::service::waitForService(qService, ros::Duration(1.0));
	serviceClient = n.serviceClient<pa10_controller::PA10Qs>(qService);
	ros::ServiceServer service = n.advertiseService(xyzrpyService, receiveXYZRPY);
	ros::ServiceServer jService = n.advertiseService(jogService, receiveJog);

	ROS_INFO("kinematics_node ready");
	ros::spin();
	return 0;
}
Example #11
0
int main(int argc, char** argv) {
	//get wcFile and device name
	string wcFile = "/home/sdsuvei/robwork/RobWorkStudio/scenes/COBLab/KitchenScene.wc.xml";
	string robotName = "UR";
	string gripperName = "SDH";
	std::string connectorName = "URConnector";
	cout << "Trying to use workcell: " << wcFile << " and device " << robotName << endl;

	//load work cell
	WorkCell::Ptr wc = WorkCellFactory::load(wcFile);
	Device::Ptr UR = wc->findDevice(robotName);
	Device::Ptr gripper = wc->findDevice(gripperName);
	Device::Ptr connector = wc->findDevice(connectorName);
	if (UR == NULL) {
		cerr << "Device: " << robotName << " not found!" << endl;
		return 0;
	}

	//default state of the RobWork Workcell
	State state = wc->getDefaultState();

	//load the collision detector
	CollisionDetector cd(wc,ProximityStrategyFactory::makeDefaultCollisionStrategy());

	//ROS initialization
	ros::init(argc, argv, "listener");
	ros::NodeHandle nh;
	ros::Rate loop_rate(5);
	sub = nh.subscribe ("/joint_states", 1, jointCallback);

	while(ros::ok())
	{
		if(itHappened)
		{

	//RobWork robot model initialization
	//starting configuration of the UR5 arm
//	Q pos(6,0.0785, -1.7667, 2.037, -2.1101, -2.1724, -1.8505);
//	Q pos(6,-0.3946, -1.8944, 1.9378, -1.3807, -1.1023, -2.4439);
	Q pos(6, joint_pos[0], joint_pos[1], joint_pos[2], joint_pos[3], joint_pos[4], joint_pos[5]);
	cout<<"Initial arm configuration:\n"<<pos<<"\n";
	//set the initial position of the UR5 arm
	UR->setQ(pos,state);

	//starting configuration of the SDH gripper
	Q grip(7,-1.047,0.000,0.000,-0.765,-0.282,-0.846,0);
	//set the initial position of the gripper
	gripper->setQ(grip,state);

	//half-away configuration of the URconnector
	Q conn(1,-1.597);
	//set the half-away configuration of the URconnector
	connector->setQ(conn,state);

	//RobWork Workcell frame definitions
//	Frame* bottle = wc->findFrame("BottleTray"); //bootle frame
	MovableFrame* bottle = (MovableFrame*)wc->findFrame("BottleTray");
	Frame* marker=wc->findFrame("SDH.Marker"); //marker frame
	Frame* camera=wc->findFrame("Camera3D"); //camera frame
	Frame* pregrasp=wc->findFrame("Pregrasp"); // pregrasping frame

	if(cd.inCollision(state))
	{
		cout<<"Before change: In collision!\n";
	}

	//update the bottle frame with the value from the tracker
	Transform3D<> camTobj_rw=rw_camTobj(UR,state,bottle,camera);
	//change the object frame in the RobWork scene
	bottle->setTransform(camTobj_rw,state);

	if(cd.inCollision(state))
		{
			cout<<"After change: In collision!\n";
		}
	//store the kinematic values, from the RobWork workcell
	Transform3D<> kinematic_camTobj=Kinematics::frameTframe(camera,bottle,state);
	Transform3D<> kinematic_camTmrk=Kinematics::frameTframe(camera,marker,state);

    //compute the intermediate transforms
	// 1)URbase -> MARKER transform
	Transform3D<> urTmrk  = UR->baseTframe(marker,state);

    // 2)CAMERA -> OBJECT transform
	Transform3D<> camTobj=find_camTobj(UR,state,bottle,camera,kinematic_camTobj);

    // 3)CAMERA -> MARKER transform
	Transform3D<> camTmrk=find_camTmrk(UR,state,marker,camera,kinematic_camTmrk);

    // 4)MARKER -> OBJECT transform
	Transform3D<> mrkTobj=inverse(camTmrk)*camTobj;

	// 5)OBJECT -> PREGRASP transform
//	Transform3D<> objTpg=Kinematics::frameTframe(bottle,pregrasp,state);

	// 6)URbase -> PreGrasping frame
	Transform3D<> objTpg=Kinematics::frameTframe(bottle,pregrasp,state);
	Transform3D<> urTcam=find_urTcam(UR,state,camera);
	Transform3D<> urTpg=urTcam*camTobj_rw*objTpg; //take initial rotation of object as pregrasping rotation
//	Transform3D<> urTpg=urTcam*camTobj_rw;

	// 7)URbase -> OBJECT transform
	Transform3D<> urTobj((urTmrk*mrkTobj*objTpg).P(),urTpg.R());
//	Transform3D<> urTobj((urTmrk*mrkTobj).P(),urTpg.R());

	//determine initial distance between the gripper-marker and the object
	Vector3D<> dist=(urTobj.P()-urTmrk.P());
	cout<<"Initial distance is: "<<dist.norm2()<<endl;
	cout<<endl;
	//set the admissible threshold - distance between gripper-marker and object
	double tresh=0.01;

	//store the current value
	kinematic_camTmrk=camTmrk;
	kinematic_camTobj=camTobj;

	//start servoing method
	while (!cd.inCollision(state)){
		//compute current distance between gripper-marker and object
		Vector3D<> cur_dist=(urTobj.P()-urTmrk.P());
		cout<<"Current distance: "<<cur_dist.norm2()<<"\n";
		//stop program if threshold limit is exceeded
		if(cur_dist.norm2()<tresh)
		{
			cout<<"Threshold limit exceeded!"<<endl;
			ros::shutdown();
			break;
		}

		//compute the new urTmrk transform by performing interpolation between urTmrk & urTobj
		Transform3D<> urTmrk_new=computeinterpolation(urTmrk,urTobj,deltatess);

		//compute the new UR5 configuration which moves the gripper-marker closer to the object
		T1=clock();
		Q new_pos=computenewQ(pos,urTmrk,urTmrk_new,UR,state,marker,bottle); //new configuration
		//Bring robot in new configuration
		UR->setQ(new_pos,state); //set device
		cout<<"new_pos:"<<new_pos;
		cout<<endl;

		//store the new urTmrk transform
		urTmrk=urTmrk_new;
		//store the new UR5 arm configuration
		pos=new_pos;

		//recompute the intermediate transforms
	    // 1)CAMERA -> OBJECT transform
		Transform3D<> camTobj=find_camTobj(UR,state,bottle,camera,kinematic_camTobj);

	    // 2)CAMERA -> MARKER transform
		Transform3D<> camTmrk=find_camTmrk(UR,state,marker,camera,kinematic_camTmrk);

	    // 3)MARKER -> OBJECT transform
		Transform3D<> mrkTobj=inverse(camTmrk)*camTobj;

		// 4)URbase -> OBJECT transform
//		Transform3D<> urTobj((urTmrk*mrkTobj*objTpg).P(),urTpg.R()); //use the pregrasping rotation
		Transform3D<> urTobj((urTmrk*mrkTobj).P(),urTpg.R());
		//store kinematic values for camTobj & camTmrk
		kinematic_camTobj=camTobj;
		kinematic_camTmrk=camTmrk;

		//execute robot movement
		//open file to write the script
		std::ofstream myfile;
		cout << "Generating script ... ";
		myfile.open ("/home/sdsuvei/workspace/PythonScripts/armQ.py");
		if (myfile.is_open()){
			myfile << 	"#!/usr/bin/python \n \n" <<
						"import roslib \n" <<
						"roslib.load_manifest('cob_script_server') \n" <<
						"import rospy \n \n" <<
						"from simple_script_server import * \n" <<
						"sss = simple_script_server() \n \n" <<
						"if __name__ == \"__main__\": \n" <<
						"	rospy.init_node(\"asd\") \n" <<
						"	sss.move(\"arm\", [["<<pos[0]<<","<<pos[1]<<","<<pos[2]<<","<<pos[3]<<","<<pos[4]<<","<<pos[5]<<"]]) \n";
			myfile.close();
			cout << "Done!" << endl;
		}
		else{
			cout << "myfile not open!" << endl;
		}

		int val1;
		cout<<"\n Press key to allow movement: ";
		cin>>val1;
		cout<<"\n";

		cout << "Executing movement..." << endl;
		system("python /home/sdsuvei/workspace/PythonScripts/armQ.py");


		cout << "Movement done!" << endl;

		int val;
		cout<<"\n Press any key to continue ";
		cin>>val;
		cout<<"\n";
	}
	itHappened=false;
	}
	ros::spinOnce();
	loop_rate.sleep();
	}
	cout<<"After While";
			cout<<endl;

	if(flag)
		{
			cout<<"\nOrientation impossible! Try a different grasping approach!\n"<< endl;
		}
	else
		{
			cout<<"\nDone!\n\n";
		}
	return 0;
}
Example #12
0
int main(int argc, char** argv) {
	const string wcFile = "Kr16WallWorkCell/Scene.wc.xml";
	const string deviceName = "KukaKr16";
	cout << "Trying to use workcell " << wcFile << " and device " << deviceName << endl;

	WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile);
	Device::Ptr device = wc->findDevice(deviceName);
	if (device == NULL) {
		cerr << "Device: " << deviceName << " not found!" << endl;
		return 0;
	}
	
	const char* stats_str;
	double extend;
	
	if (argc > 1) {
		stats_str = argv[1];
		extend = atof(argv[2]);
	}else {
		stats_str = "stat.txt";
		extend = 0.1;
	}
	cout << extend << endl << stats_str <<endl;
	
	
	Math::seed();
	
	State state = wc->getDefaultState();
	
	Q start(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573);
	device->setQ(start,state);
	
	MovableFrame* bottle = wc->findFrame<MovableFrame>("Bottle");
	Kinematics::gripFrame(bottle,device->getEnd(),state);

	CollisionDetector detector(wc, ProximityStrategyFactory::makeDefaultCollisionStrategy());
	PlannerConstraint constraint = PlannerConstraint::make(&detector,device,state);

	/** Most easy way: uses default parameters based on given device
		sampler: QSampler::makeUniform(device)
		metric: PlannerUtil::normalizingInfinityMetric(device->getBounds())
		extend: 0.05 */
	//QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, device, RRTPlanner::RRTConnect);

	/** More complex way: allows more detailed definition of parameters and methods */
	QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr());
	QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>();
	//double extend = 0.1;

	Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573);
	//Q to(6,1.7,0.6,-0.8,0.3,0.7,-0.5); // Very difficult for planner
	Q to(6,1.571,0.006,0.030,0.153,0.762,4.490);

	if (!checkCollisions(device, state, detector, from))
		return 0;
	if (!checkCollisions(device, state, detector, to))
		return 0;

	cout << "Planning from " << from << " to " << to << endl;
	ofstream stat;
	stat.open(stats_str);
	stat << "Path length,Time" << endl;
	QPath path;
	Timer t;
	
	while (extend <= 0.5) {
		QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect);
	
		t.resetAndResume();
		planner->query(from,to,path,MAXTIME);
		t.pause();
		stat << path.size() << "," << t.getTime() << "," << extend << endl;
	
		for (int x=1; x<100; x++) {
			QPath path2;
			t.resetAndResume();
			planner->query(from,to,path2,MAXTIME);
			t.pause();
			stat << path2.size() << "," << t.getTime() << "," << extend << endl;
			if (path2.size() < path.size()) {
				path = path2;	
			}
		}
		extend += 0.01;
	}
	
	if (t.getTime() >= MAXTIME) {
		cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl;
	}

	ofstream LUA; //create file object
	LUA.open("LUA.txt"); //open the file
	//write the LUA "header"
	LUA << "wc = rws.getRobWorkStudio():getWorkCell()\n";
	LUA << "state = wc:getDefaultState() \ndevice = wc:findDevice(\"KukaKr16\")\n";
	LUA << "bottle = wc:findFrame(\"Bottle\")\n";
	LUA << "function setQ(q)\n";
	LUA << "	qq = rw.Q(#q,q[1],q[2],q[3],q[4],q[5],q[6])\n";
	LUA << "	device:setQ(qq,state)\n";
	LUA << "	rws.getRobWorkStudio():setState(state)\n";
	LUA << "	rw.sleep(0.1)\n";
	LUA << "end\n";
	LUA << "setQ({-3.142, -0.827, -3.002, -3.143, 0.099, -1.573})\n";
	LUA << "rw.gripFrame(bottle,device:getEnd(),state)\n"; 
	
	for (QPath::iterator it = path.begin(); it < path.end(); it++) {
		//cout << *it << endl; //output path to Terminal
		ostringstream out; 
		out << *it; //write path to ostring
		string out2 = out.str(); //convert to string
		LUA << "setQ(" << out2.substr(4,out2.length()) << ")" << endl; // write path to LUA file.
	}
	
	LUA.close(); //close file

	cout << "Program done." << endl;
	return 0;
}
Example #13
0
bool sendConfigurations(Transform3D<double> Tcurrent, Transform3D<double> Tdesired, State& curState, bool interpolate=false, bool force_angle=false, double angle=0.0){
	ROS_INFO("MAKING XYZ RPY");
	transformToText("From", Tcurrent);
	transformToText("To", Tdesired);
	//cout << "From " << Tcurrent.P() << "\n";
	//cout << "To " << Tdesired.P() << "\n";

	rw::invkin::JacobianIKSolver solver(device, curState);
	solver.setCheckJointLimits(true);
	solver.setClampToBounds(true);

	Q qDesired;
	vector<Q> path;
	double length = (Tdesired.P()-Tcurrent.P()).norm2();
	if (interpolate && length > interpolateStepSize){
		solver.setEnableInterpolation(true);
		double stepSize = interpolateStepSize;
		cout << "Norm2: " << length << endl;
		rw::trajectory::LinearInterpolator<Transform3D<double> > interpolator(Tcurrent,Tdesired,length);
		double while_t = 0;
		while(while_t <= length){
			double t = while_t;
		//for (double t=stepSize; t <= length; t+=stepSize){
			cout << "T at: " << t << " : " << interpolator.x(t).P() << "\n";
			vector<Q> newQs = solver.solve(interpolator.x(t), curState);
			if (newQs.size()>0){
				qDesired = newQs[0] *180 / Pi;
				path.push_back(qDesired);
				device->setQ(newQs[0], curState);
				cout << "Q at: " << t << " : " << qDesired << "\n";
			}else{
				cout << "Couldn't interpolate " << Tdesired << "\n";
				break;
			}
			while_t += stepSize;
		}

	
	}

	vector<Q> newQs = solver.solve(Tdesired, curState);
	cout << "Last T: " << Tdesired.P() << "\n";
	if (newQs.size()>0){
		qDesired = newQs[0] * 180 / Pi;
		//cout << "Q: " << qDesired << " - " << newQs[0] << "\n";
		path.push_back(qDesired);
		device->setQ(newQs[0], curState);
		cout << "last Q: " << qDesired << "\n";
		//cout << "AFTER: " << endl;
		//transformToText("After: ", device->baseTend(curState));
		//cout << "AFTER : " << device->baseTend(curState).P() << "\n";
		//cout << "AFTER : " << device->baseTend(state).P() << "\n";
	}

	if (path.size() > 0){
		for (int i = 0; i < (int) path.size(); i++){
			if (force_angle){
				path[i][6] += angle;
			}
			sendQs(path[i]);
		}
		return true;
	}else{
		return false;
	}
}
Example #14
0
tuple<double, double, double> pathPlannerFunc(double extend){

    const string wcFile = "/home/student/ROVI/Mand2/Kr16WallWorkCell/Scene.wc.xml";
    const string deviceName = "KukaKr16";
    const string bottle = "Bottle";

    Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573);
    Q to(6,1.571,0.006,0.030,0.153,0.762,4.490);

    WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile);
    Device::Ptr device = wc->findDevice(deviceName);

    rw::kinematics::Frame *deviceB = wc->findFrame(bottle);

    if (device == NULL) {
        cerr << "Device: " << deviceName << " not found!" << endl;
        exit(-1);
    }
    if (deviceB == NULL) {
        cerr << "Device: " << bottle << " not found!" << endl;
        exit(-1);
    }

    rw::kinematics::State state = wc->getDefaultState();

    device->setQ(from, state);
    Kinematics::gripFrame(deviceB,device->getEnd(),state);

    CollisionDetector detector(wc, ProximityStrategyFactory::makeDefaultCollisionStrategy());
    PlannerConstraint constraint = PlannerConstraint::make(&detector,device,state);

    QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr());
    QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>();
    QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect);

    if (!checkCollisions(device, state, detector, from))
        exit(-1);
    if (!checkCollisions(device, state, detector, to))
        exit(-1);

    PathAnalyzer::CartesianAnalysis distance_traveled;
    double path_length = 0;

    PathAnalyzer path_analyse(device,state);

    QPath path;

    Timer t;
    t.resetAndResume();
    planner->query(from,to,path,MAXTIME);
    t.pause();
    distance_traveled = path_analyse.analyzeCartesian(path,deviceB);
    path_length += distance_traveled.length;

    cout << extend<< "\tPath of length " << path.size() << " found in " << t.getTime() << " seconds." << endl;
    if (t.getTime() >= MAXTIME) {
        cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl;
    }

    for (QPath::iterator it = path.begin(); it < path.end(); it++) {
        cout << "set" << *it << endl;//"setQ" << bins.substr(3,bins.length()) << endl;
    }

    auto tmp = make_tuple(extend, path_length,t.getTime());

    return tmp;
}