Ejemplo n.º 1
0
int main( int, char* [] )
{
    HuboRT::LogRelay relay;
    HuboRT::Daemonizer rt;

    rt.redirect_signals();

    std::string log_name;
    std::string log_contents;
    while( rt.good() )
    {
        if(relay.receive(log_name, log_contents))
        {
            std::cout << log_name << " | " << log_contents << std::endl;
        }
    }

    return 0;
}
Ejemplo n.º 2
0
/* ********************************************************************************************* */
int main(int argc, char* argv[]) {

	// Load the scene
	dart::utils::DartLoader dl;
	dart::simulation::World* world = dl.parseWorld("../data/dart/scenes/01-World-Robot.urdf");
	hubo = world->getSkeleton("Hubo");
	std::cout << "Loaded world. Ready to start?" << std::endl;
	getchar();

	// Create the commander and redirect the kill signal
	HuboCmd::Commander cmd;
	HuboRT::Daemonizer rt;
	rt.redirect_signals();
	if(!cmd.initialized()) {
		std::cout << "Commander was not initialized successfully!" << std::endl;
		return 1;
	}

	// Get the joint indices
	std::string joint_names [NUM_JOINTS] = {"LHY", "LHR", "LHP", "LKP", "LAP", "LAR", 
																					"RHY", "RHR", "RHP", "RKP", "RAP", "RAR",
																					"LEP", "REP"};
	size_t joint_indices [NUM_JOINTS];
	for(size_t i = 0; i < NUM_JOINTS; i++) joint_indices[i] = cmd.get_index(joint_names[i]);
	cmd.update();

	// Claim the leg joints
	std::cout << "Claiming joint" << std::endl;
	for(size_t i = 0; i < NUM_JOINTS; i++) cmd.claim_joint(joint_indices[i]);
	cmd.send_commands();
	cmd.update();

	// Set the command mode and reference positions
	for(size_t i = 0; i < NUM_JOINTS; i++) {
		cmd.set_mode(joint_indices[i], HUBO_CMD_RIGID);
		cmd.set_position(joint_indices[i], cmd.joints[joint_indices[i]].position);
	}
	cmd.send_commands();
	cmd.update();

	// Set the right arm ids
	std::vector <int> rightLegIds;
	for(size_t i = 12; i < 18; i++) rightLegIds.push_back(i);

	// Update the commands
	int c_ = 0;
	Vector14d lastNext;
	size_t goal_index = 0;
	while(rt.good()) {

		// Update the state
		dbg = ((c_++ % 50) == 0);
		if(dbg) std::cout << "=========================================================" << std::endl;
		cmd.update();
		Vector14d state;
		for(size_t i = 0; i < NUM_JOINTS; i++) 
			state(i) = cmd.joints[joint_indices[i]].position;
		if(c_ == 1) lastNext = state;
		Vector6d rightLegState = state.block<6,1>(6,0);
		hubo->setConfig(rightLegIds, rightLegState);

		// Set the workspace velocity
		Eigen::Vector6d dx;
		if(goal_index == 0) 
			dx << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
		else if(goal_index == 1) 
			dx << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;

		// Get the jointspace velocity for the right leg
		Eigen::Vector6d dqRightLeg;
		moveFoot(dx, false, dqRightLeg);
		Vector14d dq = Vector14d::Zero();
		dq.block<6,1>(6,0) = dqRightLeg;
		if(dbg) cout << "dq: " << dq.transpose() << endl;

		// Compute the next joint command using the current goal, state and step size
		Vector14d next = lastNext + dq;
		lastNext = next;

		// Set command
		for(size_t i = 0; i < NUM_JOINTS; i++) 
			cmd.set_position(joint_indices[i], next(i));
		cmd.send_commands();

		// Update the command
		HuboCan::error_result_t result = cmd.update();
		if(result != HuboCan::OKAY) {   
			std::cout << "Update threw an error: " << result << std::endl;
			return 1;
		}   

		// Print stuff
		if(dbg) {
			std::cout << "state: " << state.transpose() << std::endl;
			std::cout << "next : " << next.transpose() << std::endl;
		}

		// Print the current values
		if(dbg) {
			for(size_t i = 0; i < NUM_JOINTS; i++) 
				std::cout << cmd.joints[joint_indices[i]] << std::endl;
		}
	}

	return 0;
}