how can i implement a pid controller to convert pixels into degrees on Yarp iCubSim?

[bionicsgirl]

Hi there!
I have to code a pid controller that controls the position of the eyes and of the head of the robot, following a red ball moving on iCub sim.
First the robot follow the ball with only the eyes (joint 3 and 4) and then if the eye position is more than 10 degrees, the robot moves the head/ neck (joint 0 and 2).
How can i convert the pixels i get from the image-port to a position for the eyes to take?
Thank you in advance!
Here's the code I've written so far:

#include <iostream>
#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/dev/all.h>

using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace std;

class controller_thread: public PeriodicThread {

    public: 

        controller_thread(double p):PeriodicThread(p){
        
	    bool ok = imagePort.open("/image-port");  // give the port a name
	    if (!ok) {
	          std::cerr << "Failed to open port" << std::endl;
		  return;
	    }
	   // Connect the client to the RPC Server of iCub_SIM
	    yarp.connect("/icubSim/cam/left", "/image-port");
            options.put("device", "remote_controlboard");
            options.put("local", "/local/test");     // local port name
            options.put("remote", "/icubSim/head");  // where we connect to
            
            pd.open(options);
            if (!pd.isValid()) {
            	cerr<<"PolyDriver not instatiated"<<endl;
            	}
            pd.view(pos);  // makes the PolyDriver view the position control
            pd.view(ie);   // makes the PolyDriver view the encoders
            encoders.resize(6);
            command_position.resize(6);
	}

    
    protected:
    
    	bool threadInit(){
   	    	return true;
   		}
   	
   		void run() {
			ImageOf<PixelRgb> *image = imagePort.read();  // read an image
			if (image!=NULL) { // check we actually got something
				cout<< "We got an image of size " << image->width() << image->height() <<endl;
				double xMean = 0;
				double yMean = 0;
				int ct = 0;
				for (int x=0; x<image->width(); x++) {
					for (int y=0; y<image->height(); y++) {
						PixelRgb& pixel = image->pixel(x,y);
							// verify the red level exceeds blue and green by a factor of 2 //
						if (pixel.r>pixel.b*1.2+10 && pixel.r>pixel.g*1.2+10) {
							// find the average location of the red pixels //
							xMean += x;
							yMean += y;
							ct++;
						}
					}
				}
				if (ct>0) {
					xMean /= ct;
					yMean /= ct;
				}
				x_pixel = xMean;
				y_pixel = yMean;
				cout << "X pixel: " << x_pixel << " Y pixel: " << y_pixel << endl;
				pos->getAxes(&jnts);
				double eye_pos_x = (x_pixel - 160);
				double eye_pos_y = (y_pixel - 120);
				
				for (int ii= 0; ii< jnts; ii++) {
					command_position[ii] = 0;
				}
				command_position[4] = eye_pos_x;
				command_position[3] = eye_pos_y;
				pos->positionMove(command_position.data()); // update joint value	
				
				ie->getEncoders(encoders.data());
				for (int ii=0; ii<6; ii++) 
					cout<<encoders[ii]<< " ";              // print encoders value
					cout<<endl;
			}
		}
	
		void threadRelease() {
			yarp.disconnect("/icubSim/cam/left", "/image-port");
			imagePort.close(); 
			pd.close();
			cout << "Done, goodbye from myThread." << endl;
		}
	
    private:

		Network yarp;                                //  initialize the network
		BufferedPort<ImageOf<PixelRgb> > imagePort;  //  port for reading images
    	Property options;	   
   		PolyDriver pd;           // PolyDriver
		IPositionControl *pos;   // position control pointer
		IEncoders *ie;           // encoders pointer   
		
        Vector command_position; // vector for position control
        Vector encoders;         // vector for storing encoders values
		int jnts = 0;
        double x_pixel = 0;
		double y_pixel = 0;
			
		
};

[bionicsgirl]

I searched on internet and found that to convert pixels into degrees, you need information about the field of view (FOV) of the camera or sensor. The FOV represents the angular extent of the observable world that the camera can capture. Once you have the FOV information, you can use it to convert pixel coordinates to degrees.

the Fov on the x-axis is equal to the Fov in the y-axis for iCub_sim: fx = fy = 257.34
the formula is the following:
angle = error*atan(1/Fov)

I used this in my code, the robot follow the sphere correctly when it's on his left but then kind of lose it, maybe I'm wrong in some if cycle.
How can i correct this motion of the robot to move as I've written above?
here's my updated code:

double error_x = (x_pixel - 160);
   			double error_y = (y_pixel - 120);
                               double f = 257.34;
   			double angle_x = error_x*atan(1/f)*180/M_PI;
   			double angle_y = error_y*atan(1/f)*180/M_PI;

   			cout<< "angle x: " << angle_x << "angle y: " << angle_y << endl;
   			if (angle_x>10 || angle_x< -10){
   				for (int ii= 0; ii< jnts; ii++) {
   					command_position[ii] = 0;
   				}
   				command_position[2] = 10 - angle_x;
   				pos->positionMove(command_position.data()); // update joint value
   			}
   			if (angle_y>10 || angle_y< -10){
   				for (int ii= 0; ii< jnts; ii++) {
   					command_position[ii] = 0;
   				}
   				command_position[0] = 10 - angle_y;
   				pos->positionMove(command_position.data()); // update joint
   			}
   			else{
   				for (int ii= 0; ii< jnts; ii++) {
   					command_position[ii] = 0;
   				}
   				command_position[4] = angle_x;
   				command_position[3] = angle_y;
   				pos->positionMove(command_position.data()); // update joint value
   			}
   			ie->getEncoders(encoders.data());
   			for (int ii=0; ii<6; ii++) 
   				cout<<encoders[ii]<< " ";              // print encoders value
   				cout<<endl;
   		}

[pattacini]

The key is controlling the robot head in velocity and not in position 💡

This way, you won't need to know any conversion factor pixels → angles, but rather you'll have to focus on mapping the sign of pixel error into the correct direction of motion for all the axes at stake.

In this setting, the PID will be responsible for driving the pixel error to zero by yielding velocity commands.

Why are you coding a PID?

• Is this a sort of exercise? If so, interestingly, we do have a similar assignment, although it works in Gazebo as iCub_SIM is deprecated.
• If not, are you aware that there's already a fairly sophisticated gaze controller available for the iCub?

[bionicsgirl]

I have this assignment for a course I'm following, so I can't use neither the iGazeController nor the iVelocityPosition as I have to implement a PI controller that works on the error in pixels between the image of the sphere and the centre of the image and on the error between the encoder's position and the actual sphere position, so that's why I need to convert pixels into degrees...
Also because I want to move the head only if the angle is > 10 degrees
Now I updated my code and tried to implement a PI controller, the only problem I have is that when the sphere starts to move on the screen, the head kind of jump, so I have to find a way to better control the robot's movement at the start.
Here's my updated code:

#include <iostream>
#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/dev/all.h>

using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace std;


class controller_thread: public PeriodicThread {

    public: 

        controller_thread(double p):PeriodicThread(p){
        
			bool ok = imagePort.open("/image-port");  // give the port a name
			if (!ok) {
				std::cerr << "Failed to open port" << std::endl;
				return;
				}
			// Connect the client to the RPC Server of iCub_SIM
			yarp.connect("/icubSim/cam/left", "/image-port");
            options.put("device", "remote_controlboard");
            options.put("local", "/local/test");     // local port name
            options.put("remote", "/icubSim/head");  // where we connect to
            
            pd.open(options);
            if (!pd.isValid()) {
            	cerr<<"PolyDriver not instatiated"<<endl;
            	}
            pd.view(pos);  // makes the PolyDriver view the position control
            pd.view(ie);   // makes the PolyDriver view the encoders
            encoders.resize(6);
            command_position.resize(6);
	}

    
    protected:
    
    	bool threadInit(){
   	    	return true;
   		}
   	
   		void run() {
			ImageOf<PixelRgb> *image = imagePort.read();  // read an image
			if (image!=NULL) { // check we actually got somethi
				double xMean = 0;
				double yMean = 0;
				int ct = 0;
				for (int x=0; x<image->width(); x++) {
					for (int y=0; y<image->height(); y++) {
						PixelRgb& pixel = image->pixel(x,y);
							// verify the red level exceeds blue and green by a factor of 2 //
						if (pixel.r>pixel.b*4+10 && pixel.r>pixel.g*4+10) {
							// find the average location of the red pixels //
							xMean += x;
							yMean += y;
							ct++;
						}
					}
				}
				if (ct>0) {
					xMean /= ct;
					yMean /= ct;
				}
				x_pixel = xMean;
				y_pixel = yMean;
				
				x_pixel = (x_pixel - 160);
				y_pixel = (y_pixel - 120);
				cout << encoders[4] << "altro y "<< encoders[3] << endl;
				double error_x = x_pixel - encoders[4];
				double error_y = y_pixel - encoders[3];

				if (abs(error_x) > 10) {
					error_x = x_pixel - encoders[2];
				}
				if (abs(error_y) > 10) {
					error_y = y_pixel - encoders[0];
				}

				cout<< "error x: " << error_x << "error y: " << error_y << endl;

				pid_controller(error_x,  error_y);


				

				
			}
		}
	
		void threadRelease() {
			yarp.disconnect("/icubSim/cam/left", "/image-port");
			imagePort.close(); 
			pd.close();
			cout << "Done, goodbye from myThread." << endl;
		}
	
    private:

		Network yarp;                                //  initialize the network
		BufferedPort<ImageOf<PixelRgb> > imagePort;  //  port for reading images
    	Property options;	   
   		PolyDriver pd;           // PolyDriver
		IPositionControl *pos;   // position control pointer
		IEncoders *ie;           // encoders pointer   
		
        Vector command_position; // vector for position control
        Vector encoders;         // vector for storing encoders values
		int jnts = 0;
        double x_pixel = 0;
		double y_pixel = 0;
		double integral_x = 0;
		double integral_y = 0;
		double last_time = 0.0;

		void pid_controller(double error_x, double error_y){
			
			double f = 257.34; 
			double kp = atan(1/f)*180/M_PI;
			double ki = 2.5*exp10(-11);
			double time_now = Time::now();
			double dt = time_now - last_time;
			integral_x += error_x * dt;
			integral_y += error_y * dt;
			cout<< "P x: " << kp*error_x << "I x: " << ki*integral_y << endl;
			double PI_x = kp*error_x + ki*integral_x;
			double PI_y = kp*error_y + ki*integral_y;
			last_time = Time::now();

			cout<< "PI x: " << PI_x << "PI y: " << PI_y << endl;


			if (PI_x>10 || PI_x<-10){
					for (int ii= 0; ii< jnts; ii++) {
						command_position[ii] = 0;
					}
					command_position[2] = 10 - PI_x;
					pos->positionMove(command_position.data()); // update joint value
			}
			if (PI_y>10 || PI_y< -10){
				for (int ii= 0; ii< jnts; ii++) {
					command_position[ii] = 0;
				}
				command_position[0] = 10 - PI_y;
				pos->positionMove(command_position.data()); // update joint
			}
			else{
				for (int ii= 0; ii< jnts; ii++) {
					command_position[ii] = 0;
					}
				command_position[4] = PI_x;
				command_position[3] = -PI_y;
				pos->positionMove(command_position.data()); // update joint value
			}
			ie->getEncoders(encoders.data());
			for (int ii=0; ii<6; ii++) 
				cout<<encoders[ii]<< " ";              // print encoders value
				cout<<endl;

		}
		
};

I have also implemented a thread that makes the sphere move, so if you need it I can write it here.
Thank you in advance for your suggestions!

[pattacini]

Hi @bionicsgirl

have to implement a PI controller that works on the error in pixels between the image of the sphere and the centre of the image and on the error between the encoder's position and the actual sphere position

This sounds weird to me. A PID has just one input, not both.
Indeed, "mixing pears and apples" as in the following is conceptually wrong:

double error_x = x_pixel - encoders[4];
double error_y = y_pixel - encoders[3];

Also, what does the excerpt below mean?

error between the encoder's position and the actual sphere position

Perhaps, it could be helpful to have a pointer to your specific assignment.

Tip

You can still use a velocity-based PID and then convert its outputs into position commands by integration.
This final integral stage needs to be initialized with the actual joint positions and then you would be required to apply anti-windup to account for joints bounds.

Caution

Whatever control scheme you come up with, position commands are to be sent to the robot using the IPositionDirect interface and not IPositionControl. The difference is important as the latter is used to issue a point-to-point movement, whereas in your case you'll need to apply correction constantly.

Note

We have a tutorial illustrating the peculiarities of the three main joint interfaces:

• https://github.com/vvv-school/tutorial_joint-interface

Finally, a much better way to invite others to review your code is to rely on Gist as pointed out by our guidelines. In this way, code can be referenced more easily.

Even better, you may consider making available a sandbox on a repo to ease reproducibility. You could leverage the assignment I posted above, for example.

[bionicsgirl]

Hi @pattacini
you're right, I don't have to consider the encoder's position.
What I have to do is create a thread that:

• Control the eyes of the robot to keep the sphere at the
  center of the image;
• Control the head so that it moves in the direction of the
  ball if abs (eye position)>10 degrees.
  So I should implement a closed loop controller with the main goal to keep the position of the sphere at the center of the image.

I wish I could use your tips, but, as we didn't use them in the exercises during our course, I guess I can't use IPositionDirect and IVelocityControl in this assignment.

What I don't understand is how I can avoid that the robot at some point jumps and how i could correct this behavior.
The sphere thread I created is the following:

#include <iostream>
#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/dev/all.h>

using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace std;

class SphereThread: public PeriodicThread {

	public: 

        SphereThread(double p):PeriodicThread(p){
			
			bool ok  = sphere_port.open("/sphere-client");
				if (!ok) {
					std::cerr << "Failed to open port" << std::endl;
					return;
				}
			// Connect the client to the RPC Server of iCub_SIM
			yarp.connect("/sphere-client", "/icubSim/world");
			}
		
		

	protected:
    
    	bool threadInit(){
			
			
			req.addString("world");
			req.addString("mk");
			req.addString("ssph");
			req.addFloat64(0.04);
			req.addFloat64(xPos);
			req.addFloat64(yPos);
			req.addFloat64(0.8);
			req.addFloat64(1);
			req.addFloat64(0);
			req.addFloat64(0);  
			sphere_port.write(req,reply);
			cout << "Reply from iCubSim: " << reply.toString() << endl;
   	    	return true;
			
			
   	    }

		void run() {
			double start = Time::now();
			double end = start + 2.0;
			// move right along radius
			while (Time::now() < end ) {
				double t = Time::now() - start;	
				xPos = 0.02 + 0.4*(t/2.0);
				yPos = 0.9;
				//cout << "x position: "<< xPos << endl;
				//cout << "y position: "<< yPos << endl;
				setPosition(xPos, yPos);
			}

			// move along the circle
			start = Time::now();
			end = start + (1/0.2);
			while (Time::now() < end){
				double t = Time::now() - start;
				xPos = 0.02 + 0.4*cos(2*M_PI*0.2*t);
				yPos = 0.9 + 0.4*sin(2*M_PI*0.2*t);
				setPosition(xPos, yPos);
			}

			Time::delay(3.0);

			// infinity simbol
			start = Time::now();
			end = start + 5.0;
			while (Time::now() < end){
				double t = Time::now() - start;
				double theta = 2*M_PI*0.2;
				xPos = 0.02 + 0.2*sin(theta*t);
				yPos = 0.9 + 0.1*sin(2*theta*t);
				setPosition(xPos, yPos);
			}

			Time::delay(5.0);

			

		}
			
		
		void threadRelease() {
			yarp.disconnect("/sphere-client", "/icubSim/world");
    		sphere_port.close();
			cout << "Done, goodbye from myThread." << endl;
		}
	
    private:

		Network yarp;  //initialize the network
    	RpcClient sphere_port;  // create rpc client for the sphere
		Bottle req, reply;
		float xPos = 0.02;
		float yPos = 0.9;
		
		
		void setPosition(float x, float y){
			req.clear();
			req.addString("world");
			req.addString("set");
			req.addString("ssph");
			req.addInt32(1);
			req.addFloat64(x);
			req.addFloat64(y);
			req.addFloat64(0.8);
			sphere_port.write(req, reply);
			
			
		}
        

};


int main(int argc, char *argv[]) {

    Network yarp;
    srand(time(NULL));
    SphereThread mt(0.1/0.2);
    mt.start();
    
	Time::delay(20);
    
    mt.stop();

    return 0;

};

The sphere moves with 2 iterations lasting 10 seconds.
So the joints' position of the robot has to be set to zero at the end of each iteration.

[pattacini]

Hi @bionicsgirl

I'm sorry, but IPositionControl does NOT get along with a high-level PID. This interface provides the user with a control mode characterized by commands of the type send-and-forget. In other words, these are the steps the user is required to accomplish in the code, once the interface gets correctly configured:

1. Establish the average speed of the point-to-point movement by calling setRefSpeed. This is something you're not doing yet, apparently.
2. Issue the movement by calling positionMove.
3. Wait until the movement is complete by polling via checkMotionDone.
4. Only then, repeat from point 1 with a subsequent command.

A send-and-forget control mode can be seen from the user's perspective as an open loop, not a closed loop.

I can feel your frustration from here. But wait, here are a couple of comments:

• I wish I could use your tips, but, as we didn't use them in the exercises during our course, I guess I can't use IPositionDirect and IVelocityControl in this assignment.

  Well, if it's not explicitly forbidden, you can, instead! You can motivate that by saying that you made a search in this respect¹, finding relevant resources². I'm putting this forward from a teacher standpoint as I ran several academic and crash courses in the past.

• If you really want to continue with IPositionControl, simply ditch the high-level PID and implement an iterative approach by leveraging the FOV formula you found:

  1. Compute the pixel error using the image for the dominant eye only. You'll have two errors in the two directions.
  2. Acquire the eyes' angular position using getEncoders.
  3. Compute the angular errors for the dominant eye using the FOV formula in the two directions.
  4. The new angular setpoints are equal to the sum of the values obtained in ii and iii.
  5. Follow the steps above to issue two point-to-point movements at your preferred velocity for the tilt and the pan of the eyes, respectively.
  6. Wait until the motion is complete.
  7. Repeat from point i.

Hopefully, the system will converge.

When you're happy about the eyes' motion, deal with the neck in the same way. You'll only need to get rid of the initial steps relative to the pixels and consider the actual positions of the eyes' tilt and pan as the source of the error to work out.

Footnotes

1. After all, you had to search for the FOV formula as well. ↩

2. You can even cite this conversation. No problem with me. ↩