Exemplo n.º 1
0
	void initialize()
	{
		init_ = true;

		// Enable stiffness
		AL::ALValue stiffness_name("Body");
		AL::ALValue stiffness(1.0f);
		AL::ALValue stiffness_time(1.0f);
		motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time);

		// Stand
		//robotPosture->goToPosture("Stand",0.5f);

		// Init moving
		motion_proxy_ptr->moveInit();

		// Initial Odometry
		x_0 = motion_proxy_ptr->getRobotPosition(true).at(0);
		y_0 = motion_proxy_ptr->getRobotPosition(true).at(1);
		z_0 = motion_proxy_ptr->getRobotPosition(true).at(2);
	}
Exemplo n.º 2
0
	int executeCB(ros::Duration dt)
	{
		std::cout << "**Walk -%- Executing Main Task, elapsed_time: "
		          << dt.toSec() << std::endl;
		std::cout << "**Walk -%- execute_time: "
		          << execute_time_.toSec() << std::endl;
		execute_time_ += dt;

		if(!init_)
		{
			set_feedback(RUNNING);
			initialize();
		}

		double x = motion_proxy_ptr->getRobotPosition(true).at(0);
		double y = motion_proxy_ptr->getRobotPosition(true).at(1);
		double z = motion_proxy_ptr->getRobotPosition(true).at(2);

		// Walk forward
		double angular = 0.1*modulo2Pi(z_0-z);
		//ROS_INFO("theta = %f",angular);
		if(angular > 1) {angular = 1;}
		if(angular < -1) {angular = -1;}
		geometry_msgs::Twist cmd;
		cmd.linear.x = 0.3; //0.5
		cmd.angular.z = angular;
		cmd_pub.publish(cmd);

		if(sqrt((x-x_0)*(x-x_0) + (y-y_0)*(y-y_0)) > dist)
		{
			set_feedback(SUCCESS);
			finalize();
			return 1;
		}

		return 0;
	}