答:
Mapping With Coordinates Systems

Suppose you have an robotic arm with 3 links:

Using generalized coordinates:

$\hspace{2.5em}$ $\vec{q}$ = $[q_{1}\hspace{1em}q_{2}]^{T}$ $\hspace{1.5em}$ [Generalized coordinate]

We can evaluate the system as following:

The kinematic equation that maps the origin to point A is:

$\hspace{5.em}$ $_{o}\vec{r}_{OA}$ = $_{o}\vec{r}_{O1}$ + $_{o}\vec{r}_{12}$ + $_{o}\vec{r}_{2A}$ $\hspace{1.5em}$ [kinematic equation]

We have two rotations over joint 1 and 2. Substituting in the equation above:

$\hspace{5.em}$ $_{o}\vec{r}_{OA}$ = $_{0}\vec{r}_{O1}$ + $R{(q_{1})_{01}}$ $ _{1}\vec{r}_{12}$ + $R{(q_{1}+q_{2})_{12}}$ $ _{2}\vec{r}_{2A}$

Where $R(\bullet)$ is the rotation matrix.

If you want to map between point 1 to A:

$\hspace{5.em}$ $_{1}\vec{r}_{1A}$ = $_{1}\vec{r}_{12}$ + $_{1}\vec{r}_{2A}$ $\hspace{1.5em}$ [New kinematic equation]

$\hspace{5.em}$ $_{1}\vec{r}_{1A}$ = $R{(q_{1})_{01}}$ $ _{1}\vec{r}_{12}$ + $R{(q_{1}+q_{2})_{12}}$ $ _{2}\vec{r}_{2A}$

It's pretty straightforward! For more on direct kinematics here.

The term $R{(q_{1})_{01}}$ $ _{1}\vec{r}_{12}$ should be something like:

$\hspace{5.em}$ $R{(q_{1})_{01}}$ $ _{1}\vec{r}_{12}$ = $\begin{bmatrix} cos(q_{1}) & -sin(q_{1}) & 0 \\ sin(q_{1}) & cos(q_{1}) &  0 \\ 0 & 0 & 1\end{bmatrix}$  $\begin{bmatrix} l_{1} \\ 0 \\ 0 \end{bmatrix}$

With a little bit of algebra, you can achieve the answer:

$\hspace{5.em}$ $ _{0}\vec{r}_{OA}$ = $\begin{bmatrix} l_{0} + l_{1}cos(q_{1}) + l_{2}cos(q_{1}+q_{2}) \\ 0 + l_{1}sin(q_{1}) + l_{2}sin(q_{1}+q_{2}) \\ 0 \end{bmatrix}$

That equation make possible to map all your system at any time relative to the origin.


问:
Inverse kinematics after calibration
I am working on a 6DOF robot arm project and I have one big question. When I first derived the inverse kinematics (IK) algorithm after decoupling (spherical wrist), I could easily get the equations based on nominal DH values, where alpha are either 0 or 90 degrees and there are many zeros in $a_i$ and $d_i$. However, after kinematics calibration, the identified DH parameters are no longer ideal ones with a certain small, but non-zero, bias added to the nominal values.

So my question is, can the IK algorithm still be used with the actual DH parameters? If yes, definitely there will be end-effector errors in actual operation. If not, how should I change the IK algorithm?

P.S. I am working on a modular robot arm which means the DH bias could be bigger than those of traditional robot arms.

答:
The analytic inverse kinematics solutions you found do depend on those $0$ terms in your transformation matrices.   Those values are, as you've implied, based on the $0$ and $90$ degree values for the axes relationships.   The kinematic mapping process works for other $a_i$ and $d_i$ values, too, but it becomes difficult to find closed-form inverse solutions when those nice zeroes become other values.

The solution most often implemented is to use numerical solutions for the inverse kinematics after calibration.   The closed-form, analytic, inverse kinematics are a good starting estimate for the numerical solver, but you must then account for the as-built errors with some (often iterative) approach to find the actual joint angles given the EE pose.  There are many published approaches to the numerical solution methods.

EDIT (Industrial robots):  For general-purpose industrial robots the above is rarely done.   Instead of calibrating to eliminate as-built errors in the kinematics, the robots are usually calibrated to align the task space coordinate system with the robot's base coordinate system (resulting in a single additional transformation matrix of constant values).  The end effector destination locations are then taught (in either task, end-effector, or joint coordinates).  Because they use a "teach and repeat" method of programming, the small errors in the calibration are absorbed by the teaching process.

Additional calibrations of the robot may be accomplished by the robot manufacturer.  In my experience those calibrations generally set precise values for the link lengths and encoder home, or zero, values - but not joint axis offset angles.  It's possible that some robot manufacturers calibrate more precisely, then use numerical methods to find inverse solutions, but I have not seen this absolute precision needed in an industrial setting.

ContentLicense: CC BY-SA 3.0

Title: Inverse kinematics after calibration


问:
How to import catia assembly to Matlab. Simmechanics
In catia .stl format is available only for part file not for assembly file. Please help how to import asembly in simmechanics

.CATProduct to .stl
Or Is there any other way to do?

答:
An STL file converts a solid model to an approximation that is represented by a bunch of triangular faces. There is no way for an STL file to represent joints or any other motion constraints because it's simply not part of the file protocol.

This means that, if your assembly has motion constraints or movable parts that you don't actually want to export that assembly to one STL file. Instead, you should export each part individually and then re-build the assembly in SimMechanics using the frame transforms and motion constraint blocks provided there.

I understand that this can be a (very) painful and tedious process, but unfortunately SimMechanics only supports SolidWorks, Inventor, and Pro/E  (stated in the tip at the bottom of the page).

That said, if you're using SimMechanics, I'd say there's a fair bet you're a student (because SimMechanics is EXPENSIVE given the perquisites) and Autodesk Inventor is free to students.

If your model is only a few parts, it will probably be easiest to just reassemble it with the joint constraints. If it's especially detailed, you might be better off fusing the non-moving parts for a "simulation model" such that you do wind up with only a few parts. If you absolutely need to import an assembly that has a lot (probably over 10 would be a lot to me) of parts or you intend on importing models frequently then I would pursue acquiring Inventor or SolidWorks. SolidWorks is better but not free to students, but they have an education version that is less expensive than the commercial version.


问:
Are power and torque required related in some way?
I am designing a new platform for outdoor robotics and I need to calculate the power and/or torque that is needed to move the platform. I have calculated that I need about 720 W of total power to move it (360W per motor), but I don't know how to calculate the torque that I need.

Is it really just about having the required power and ignoring the torque or is there a way to calculate it easily?

Already known parameters of the platform are:

Weight of the whole platform: 75 kg.

Number of wheels: 4.

Number of powered wheels: 4.

Diameter of wheels: 30 cm.

Number of motors: 2.

Wanted speed: 180 RPM (3 m/s).

Wanted acceleration: > 0.2 m/s^2

答:
While the answer by freeman01in references a useful presentation (alternative source) on the practical application aspect of your question, it is probably worth answering the specific question in the title too, in terms of first principles.

From the Wikipedia Power page:

In physics, power is the rate at which energy is transferred, used, or transformed. The unit of power is the joule per second (J/s), known as the watt ...

Energy transfer can be used to do work, so power is also the rate at which this work is performed. The same amount of work is done when carrying a load up a flight of stairs whether the person carrying it walks or runs, but more power is expended during the running because the work is done in a shorter amount of time. The output power of an electric motor is the product of the torque the motor generates and the angular velocity of its output shaft. The power expended to move a vehicle is the product of the traction force of the wheels and the velocity of the vehicle.

The integral of power over time defines the work done. Because this integral depends on the trajectory of the point of application of the force and torque, this calculation of work is said to be "path dependent."

Meanwhile, from the Wikipedia Torque page:

Torque, moment or moment of force (see the terminology below), is the tendency of a force to rotate an object about an axis,1 fulcrum, or pivot. Just as a force is a push or a pull, a torque can be thought of as a twist to an object. Mathematically, torque is defined as the cross product of the lever-arm distance and force, which tends to produce rotation.

Loosely speaking, torque is a measure of the turning force on an object such as a bolt or a flywheel. For example, pushing or pulling the handle of a wrench connected to a nut or bolt produces a torque (turning force) that loosens or tightens the nut or bolt.

The symbol for torque is typically $\boldsymbol{\tau}$, the Greek letter tau. When it is called moment, it is commonly denoted $M$.

The magnitude of torque depends on three quantities: the force applied, the length of the lever arm connecting the axis to the point of force application, and the angle between the force vector and the lever arm. ...

The SI unit for torque is the newton metre (N·m).

And from Relationship between torque, power, and energy you get the formula:

Power is the work per unit time, given by $$P = \boldsymbol{\tau} \cdot \boldsymbol{\omega}$$ where $P$ is power, $\boldsymbol{\tau}$ is torque, $\boldsymbol{\omega}$ is the angular velocity, and $\cdot$ represents the scalar product.


问:
Multi-Rate Sensor Fusion using EKF
Context: I have an IMU(a/g/m) + Wheel Odometry measurement data that I'm trying to fuse in order to localize a 2D (ackermann drive) robot.

The state vector X = [x y yaw].
I'm using the odometry data to propagate the state through time (no control input).
The update step includes the measurement vector Z = [x_odo y_odo yaw_imu].

I have two questions:

1.Does it make sense to use the odometry data(v_linear, omega) in both the prediction as well as update steps?

2.How do I account for the frequency difference between the odometry data(10Hz) and the imu data(40Hz)? Do I run the filter at the lower frequency, do I dynamically change the matrix sizes or is there any other way?

Thanks!

答:
Part 1.  Use one or the other. Often odometery is used instead of kinematics or dynamics for prediction, at least in my work.

Part 2.
This is handled by the construction of the measurement equation jacobian.

Every time a measurement comes in, construct a Jacobian for the whole state. You'll notice that some of the state elements are independent of the current measurement (vel sensor cannot measure position, e.g.). So, no need to resize the matrix, just run the ekf with some zeros in the jacobian.

Specifically, for every incoming measurement

Predict state from last update to the current

Construct jacobian for current measurement type / equation

Update state

Title: Multi-Rate Sensor Fusion using EKF


答:
I've performed 2D localization with just odometry and a gyroscope before, and to be honest, depending on (i) how good your encoders are; (ii) what type of environment you're in (is there a chance your wheels will slip a lot); (iii) how good your IMU is, there's a good chance that you don't lose much by just using odometry for translation, and only calculating heading using the gyroscope. In other words, just update the position when you get an odometry measurement, and just update the heading when you get a gyroscope measurement (i.e., two prediction steps, no correction step). Practically speaking, you won't get much better results using a correction step when you aren't measuring the environment.

That being said, I noticed that you didn't mention anything about using the magnetometer of your IMU to estimate the heading of the robot. If you are in a somewhat magnetically clean environment (i.e., you can somewhat reliably measure the Earth's magnetic field), then this measurement would be extremely useful.
The reason why is because it is an exteroceptive sensor (i.e., it's measuring the environment) and your other sensors (wheel encoders, gyroscope) are interoceptive sensors (i.e., they measure the robot internally). With only interoceptive sensors measuring your heading, it is subject to dead reckoning and its uncertainty will increase without bounds. And as you know, if your heading becomes increasingly uncertain, so does your position. With an exteroceptive sensor measurement of your heading, its uncertainty becomes bounded.

Here's one way to set up your filter:

Each time you receive an odometry measurement (10 Hz), perform the prediction step to propagate your state forward.

Each time you receive an IMU measurement (40 Hz), perform a (different) prediction step to propagate your heading estimate forward by integrating the gyroscope, then perform a correction step to correct your heading using the magnetometer.

To use the magnetometer in this way, you need to know the direction of the Earth's magnetic field in your local area (you can look this up online). Let's call this $m\in[-\pi,\pi)$. Now you get a measurement of the magnetic field $z_k\in[-\pi,\pi)$ from your magnetometer.

Your measurement model is:

$$
\hat{z}_k = m - \hat{\theta}_k
$$

where $\hat{\theta}_k$ is your current heading estimate. Note: you need to account for wrapping of the angles here! One way to do this is:

$$
\hat{z}_k = \text{atan2}(\sin(m - \hat{\theta}_k),\cos(m - \hat{\theta}_k))
$$

but there are other ways as well. Then just proceed with the normal KF correction step.

Title: Multi-Rate Sensor Fusion using EKF

Body:
Context: I have an IMU(a/g/m) + Wheel Odometry measurement data that I'm trying to fuse in order to localize a 2D (ackermann drive) robot.

The state vector X = [x y yaw].
I'm using the odometry data to propagate the state through time (no control input).
The update step includes the measurement vector Z = [x_odo y_odo yaw_imu].

I have two questions:

1.Does it make sense to use the odometry data(v_linear, omega) in both the prediction as well as update steps?

2.How do I account for the frequency difference between the odometry data(10Hz) and the imu data(40Hz)? Do I run the filter at the lower frequency, do I dynamically change the matrix sizes or is there any other way?

Thanks!


答:
You might want to try IKFast in OpenRave.  It can supposedly handle "robots with arbitrary joint complexity like non-intersecting axes".

However, I had the same problem as you.  After kinematic calibration, my joint axes were no longer at right angles.  But when I tried my new robot kinematics in IKFast, it would crash and I was never able to get valid IK.  I had a 7 DOF arm on a 1 DOF torso though.  You might have better luck with a 6 DOF robot.

Title: Inverse kinematics after calibration

Body:
I am working on a 6DOF robot arm project and I have one big question. When I first derived the inverse kinematics (IK) algorithm after decoupling (spherical wrist), I could easily get the equations based on nominal DH values, where alpha are either 0 or 90 degrees and there are many zeros in $a_i$ and $d_i$. However, after kinematics calibration, the identified DH parameters are no longer ideal ones with a certain small, but non-zero, bias added to the nominal values.

So my question is, can the IK algorithm still be used with the actual DH parameters? If yes, definitely there will be end-effector errors in actual operation. If not, how should I change the IK algorithm?

P.S. I am working on a modular robot arm which means the DH bias could be bigger than those of traditional robot arms.


问:
Finding Center of Mass for Humanoid Robot
I've been working on Humanoid Robot, and I face the problem of finding the Center of Mass of the Robot which will help in balancing the biped. Although COM has a very simple definition, I'm unable to find a simple solution to my problem.

My view: I have already solved the Forward and Inverse Kinematics of the Robot with Torso as the base frame. So, if I can find the position(and orientation) of each joint in the base frame, I can average all of them to get the COM. Is this approach reasonable? Will it produce the correct COM?

Can anyone offer any series of steps that I can follow to find the COM of the biped? Any help would be appreciated.

Cheers!

答:
Yes.  As @hauptmech mentioned, you can use your forward kinematics to get the center of mass of each link in the base frame.  Then you can simply compute the weighted average of the masses and positions to get the overall center of mass.

In other words:

$$
M = \sum_{i=0}^n m_i
$$
$$
\mathbf{P}_i^0 = pos( \mathbf{T}_i^0(\mathbf{q}) \mathbf{T}_{i_m}^i)
$$
$$
\mathbf{COM}^0 = \frac{1}{M} \sum_{i=0}^n m_i \mathbf{P}_i^0
$$

Where:

$m_i$ = mass of link $i$

$M$ = total mass

$\mathbf{q}$ = vector of joint angles

$\mathbf{T}_i^0$ = Transform from base frame $0$ to link $i$.  (The forward kinematics which is a function of $\mathbf{q}$).

$\mathbf{T}_{i_m}^i$ = Transform from the link $i$ frame to it's center of mass.

$pos()$ = function to extract position vector from full transform.

$\mathbf{P}_i^0$ = position of mass of link $i$ in base frame $0$.

$\mathbf{COM}^0$ = center of mass of system in base frame $0$.


问:
High voltage motor control with arduino
I'm trying to control a higher voltage motor than an arduino can source with a pin, with an arduino. I am trying to hook it up to a transistor. The battery pack is not supposed to be 4.8V, it's 6V, 4 D batteries.

Here is the setup:

Here is the arduino code I'm trying to run to it:

```<code>int motorpin = 2;

void setup()
{
    pinMode(motorpin, OUTPUT);
}

void loop()
{
    digitalWrite(motorpin, HIGH);
    delay(500);
    digitalWrite(motorpin, LOW);
    delay(500);
}
</code>```

Code gives me no errors, but no motor movement happens. What would make this work? Thanks.

答:
It's possible that this isn't working because you have blown your transistor. There are a couple of things which could have blown it:

No resistor to limit the base current

No diode to protect against back EMF from the motor

So you should add a base resistor, and a flyback diode.

I always like to add an LED to the Arduino's output too, so that I can see exactly when the output is high. And if you're doing PWM on that pin, then the brightness of the LED tells you the duty cycle.

Added:

The reason for putting the transistor below the motor is that it's easier to make it work. To switch on the transistor, I just have to push a little current through the base by bringing its voltage up higher than the emitter. But if the transistor was above the motor, and the motor was running at 12v, then it's going to be impossible for the Arduino to do this, because it can't generate a voltage high enough.

When you put the transistor above the motor, you need to add another transistor to help you deal with the large voltage required by the base.

In this circuit they're using a FET, rather than an NPN transistor, so the base is called a gate, but the same thing applies.

Title: High voltage motor control with arduino


问:
PID Control: Integral error does not converge to zero
Good day,

I had been recently reading up more on PID controllers and stumbled upon something called integral wind up. I am currently working on an autonomous quadcopter concentrating at the moment on PID tuning. I noticed that even with the setpoint of zero degrees reached in this video, the quadcopter would still occasionally overshoot a bit:  https://youtu.be/XD8WgVFfEsM

Here is the corresponding data testing the roll axis:

I noticed that the I-error does not converge to zero and continues to increase:

Is this the integral wind-up?

What is the most effective way to resolve this?

I have seen many implementations mainly focusing on limiting the output of the system by means of saturation. However I do not see this bringing the integral error eventually back to zero once the system is stable.

Here is my current code implementation with the setpoint of 0 degrees:

```<code>cout << "Starting Quadcopter" << endl;

float baseThrottle = 155; //1510ms
float maxThrottle = 180; //This is the current set max throttle for the PITCH YAW and ROLL PID to give allowance to the altitude PWM. 205 is the maximum which is equivalent to 2000ms time high PWM
float baseCompensation = 0; //For the Altitude PID to be implemented later

delay(3000);

float startTime=(float)getTickCount();
deltaTimeInit=(float)getTickCount(); //Starting value for first pass

while(1){
//Read Sensor Data
readGyro(&gyroAngleArray);
readAccelMag(&accelmagAngleArray);

//Time Stamp
//The while loop is used to get a consistent dt for the proper integration to obtain the correct gyroscope angles. I found that with a variable dt, it is impossible to obtain correct angles from the gyroscope.

while( ( ((float)getTickCount()-deltaTimeInit) / ( ((float)getTickFrequency()) ) ) < 0.005){ //0.00209715|0.00419
       deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //Get Time Elapsed
       cout << " DT endx = " << deltaTime2 << endl;
}
//deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //Get Time Elapsed
deltaTimeInit=(float)getTickCount(); //Start counting time elapsed
cout << " DT end = " << deltaTime2 << endl;

//Complementary Filter
float pitchAngleCF=(alpha)*(pitchAngleCF+gyroAngleArray.Pitch*deltaTime2)+(1-alpha)*(accelmagAngleArray.Pitch);
float rollAngleCF=(alpha)*(rollAngleCF+gyroAngleArray.Roll*deltaTime2)+(1-alpha)*(accelmagAngleArray.Roll);
float yawAngleCF=(alpha)*(yawAngleCF+gyroAngleArray.Yaw*deltaTime2)+(1-alpha)*(accelmagAngleArray.Yaw);

//Calculate Orientation Error (current - target)
float pitchError = pitchAngleCF - pitchTarget;
pitchErrorSum += (pitchError*deltaTime2);
float pitchErrorDiff = pitchError - pitchPrevError;
pitchPrevError = pitchError;

float rollError = rollAngleCF - rollTarget;
rollErrorSum += (rollError*deltaTime2);
float rollErrorDiff = rollError - rollPrevError;
rollPrevError = rollError;

float yawError = yawAngleCF - yawTarget;
yawErrorSum += (yawError*deltaTime2);
float yawErrorDiff = yawError - yawPrevError;
yawPrevError = yawError;

//PID controller list
float pitchPID = pitchKp*pitchError + pitchKi*pitchErrorSum + pitchKd*pitchErrorDiff/deltaTime2;
float rollPID = rollKp*rollError + rollKi*rollErrorSum + rollKd*rollErrorDiff/deltaTime2;
float yawPID = yawKp*yawError + yawKi*yawErrorSum + yawKd*yawErrorDiff/deltaTime2;

//Motor Control - Mixing
//Motor Front Left (1)
float motorPwm1 =  -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;

//Motor Front Right (2)
float motorPwm2 =  -pitchPID - rollPID + yawPID + baseThrottle + baseCompensation;

//Motor Back Left (3)
float motorPwm3 = pitchPID + rollPID + yawPID + baseThrottle + baseCompensation;

//Motor Back Right (4)
float motorPwm4 = pitchPID - rollPID - yawPID + baseThrottle + baseCompensation;

 //Check if PWM is Saturating - This method is used to fill then trim the outputs of the pwm that gets fed into the gpioPWM() function to avoid exceeding the earlier set maximum throttle while maintaining the ratios of the 4 motor throttles.
    float motorPWM[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
    float minPWM = motorPWM[0];
    int i;
    for(i=0; i<4; i++){ // Get minimum PWM for filling
        if(motorPWM[i]<minPWM){
            minPWM=motorPWM[i];
        }
    }

    cout << " MinPWM = " << minPWM << endl;

    if(minPWM<baseThrottle){
        float fillPwm=baseThrottle-minPWM; //Get deficiency and use this to fill all 4 motors
        cout << " Fill = " << fillPwm << endl;
        motorPwm1=motorPwm1+fillPwm;
        motorPwm2=motorPwm2+fillPwm;
        motorPwm3=motorPwm3+fillPwm;
        motorPwm4=motorPwm4+fillPwm;
    }

    float motorPWM2[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
    float maxPWM = motorPWM2[0];
    for(i=0; i<4; i++){ // Get max PWM for trimming
        if(motorPWM2[i]>maxPWM){
            maxPWM=motorPWM2[i];
        }
    }

    cout << " MaxPWM = " << maxPWM << endl;

    if(maxPWM>maxThrottle){
        float trimPwm=maxPWM-maxThrottle; //Get excess and use this to trim all 4 motors
        cout << " Trim = " << trimPwm << endl;
        motorPwm1=motorPwm1-trimPwm;
        motorPwm2=motorPwm2-trimPwm;
        motorPwm3=motorPwm3-trimPwm;
        motorPwm4=motorPwm4-trimPwm;
    }

//PWM Output
    gpioPWM(24,motorPwm1);  //1
    gpioPWM(17,motorPwm2);  //2
    gpioPWM(22,motorPwm3);  //3
    gpioPWM(18,motorPwm4);  //4
</code>```

答:
When I look at your graphs of position error and integral error, I don't see any unexpected behaviors for a system which is not quite tuned well enough.   You are not showing integral windup nor saturation.   Make sure to use the integral loop of your controller only when holding at a steady-state position and you will avoid windup.

It looks like your value for $k_p$ needs to be increased, and possibly also $k_i$.   Disturbances are causing you to leave the steady-state position, and your system is not able to remain close enough to its steady-state position (or else you wouldn't be trying to improve the performance).   The $k_i$ and $k_p$ terms are your options for locking that position in tighter around the steady-state value.  How low do you want the steady state error band to go before you consider it acceptable behavior?

The integral response will be more sluggish than the position-loop response, so I would begin by increasing $k_p$ a bit, and contine to do so until you see the fist signs of unstable behavior, then back it off by 25 - 50%.    Then do the same for $k_i$, and see if those two gains squash your steady state error sufficiently.


答:
I don't think this is related to integral windup at all.

I noticed that the I-error does not converge to zero

That's a good thing, because it means your integral term is not useless.

The integral term is there to compensate for steady-state errors. If you set the integral gain to 0, you should see that your system never reaches the setpoint.

The I-error that builds up will fix that.

and continues to increase

I'm not sure if that's really the case. From the image that you posted it might as well converge to some constant value. Hence my request to let the system run for a longer period to see how the I-error behaves in the long run.

Maybe there actually is something in your system that increases the steady-state error over time. I imagine that the increasing temperature of an electrical resistance (in the driver or the motor) might cause that.

It's somewhat hard to tell what the reason is because it's one PID for everything. If you had several (cascaded) controllers, you could locate the reason by looking at the individual controllers. In case of a rising electrical resistance due to rising temperature, that should be visible in the current controller.

tl:dr; There's a steady-state error in your system and the integral part of your controller compensates it. If that keeps growing, it means that the steady-state error is growing over time, which I think suggests that there's some kind of drift.

Title: PID Control: Integral error does not converge to zero

Body:
Good day,

I had been recently reading up more on PID controllers and stumbled upon something called integral wind up. I am currently working on an autonomous quadcopter concentrating at the moment on PID tuning. I noticed that even with the setpoint of zero degrees reached in this video, the quadcopter would still occasionally overshoot a bit:  https://youtu.be/XD8WgVFfEsM

Here is the corresponding data testing the roll axis:

I noticed that the I-error does not converge to zero and continues to increase:

Is this the integral wind-up?

What is the most effective way to resolve this?

I have seen many implementations mainly focusing on limiting the output of the system by means of saturation. However I do not see this bringing the integral error eventually back to zero once the system is stable.

Here is my current code implementation with the setpoint of 0 degrees:

```<code>cout << "Starting Quadcopter" << endl;

float baseThrottle = 155; //1510ms
float maxThrottle = 180; //This is the current set max throttle for the PITCH YAW and ROLL PID to give allowance to the altitude PWM. 205 is the maximum which is equivalent to 2000ms time high PWM
float baseCompensation = 0; //For the Altitude PID to be implemented later

delay(3000);

float startTime=(float)getTickCount();
deltaTimeInit=(float)getTickCount(); //Starting value for first pass

while(1){
//Read Sensor Data
readGyro(&gyroAngleArray);
readAccelMag(&accelmagAngleArray);

//Time Stamp
//The while loop is used to get a consistent dt for the proper integration to obtain the correct gyroscope angles. I found that with a variable dt, it is impossible to obtain correct angles from the gyroscope.

while( ( ((float)getTickCount()-deltaTimeInit) / ( ((float)getTickFrequency()) ) ) < 0.005){ //0.00209715|0.00419
       deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //Get Time Elapsed
       cout << " DT endx = " << deltaTime2 << endl;
}
//deltaTime2=((float)getTickCount()-deltaTimeInit)/(((float)getTickFrequency())); //Get Time Elapsed
deltaTimeInit=(float)getTickCount(); //Start counting time elapsed
cout << " DT end = " << deltaTime2 << endl;

//Complementary Filter
float pitchAngleCF=(alpha)*(pitchAngleCF+gyroAngleArray.Pitch*deltaTime2)+(1-alpha)*(accelmagAngleArray.Pitch);
float rollAngleCF=(alpha)*(rollAngleCF+gyroAngleArray.Roll*deltaTime2)+(1-alpha)*(accelmagAngleArray.Roll);
float yawAngleCF=(alpha)*(yawAngleCF+gyroAngleArray.Yaw*deltaTime2)+(1-alpha)*(accelmagAngleArray.Yaw);

//Calculate Orientation Error (current - target)
float pitchError = pitchAngleCF - pitchTarget;
pitchErrorSum += (pitchError*deltaTime2);
float pitchErrorDiff = pitchError - pitchPrevError;
pitchPrevError = pitchError;

float rollError = rollAngleCF - rollTarget;
rollErrorSum += (rollError*deltaTime2);
float rollErrorDiff = rollError - rollPrevError;
rollPrevError = rollError;

float yawError = yawAngleCF - yawTarget;
yawErrorSum += (yawError*deltaTime2);
float yawErrorDiff = yawError - yawPrevError;
yawPrevError = yawError;

//PID controller list
float pitchPID = pitchKp*pitchError + pitchKi*pitchErrorSum + pitchKd*pitchErrorDiff/deltaTime2;
float rollPID = rollKp*rollError + rollKi*rollErrorSum + rollKd*rollErrorDiff/deltaTime2;
float yawPID = yawKp*yawError + yawKi*yawErrorSum + yawKd*yawErrorDiff/deltaTime2;

//Motor Control - Mixing
//Motor Front Left (1)
float motorPwm1 =  -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;

//Motor Front Right (2)
float motorPwm2 =  -pitchPID - rollPID + yawPID + baseThrottle + baseCompensation;

//Motor Back Left (3)
float motorPwm3 = pitchPID + rollPID + yawPID + baseThrottle + baseCompensation;

//Motor Back Right (4)
float motorPwm4 = pitchPID - rollPID - yawPID + baseThrottle + baseCompensation;

 //Check if PWM is Saturating - This method is used to fill then trim the outputs of the pwm that gets fed into the gpioPWM() function to avoid exceeding the earlier set maximum throttle while maintaining the ratios of the 4 motor throttles.
    float motorPWM[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
    float minPWM = motorPWM[0];
    int i;
    for(i=0; i<4; i++){ // Get minimum PWM for filling
        if(motorPWM[i]<minPWM){
            minPWM=motorPWM[i];
        }
    }

    cout << " MinPWM = " << minPWM << endl;

    if(minPWM<baseThrottle){
        float fillPwm=baseThrottle-minPWM; //Get deficiency and use this to fill all 4 motors
        cout << " Fill = " << fillPwm << endl;
        motorPwm1=motorPwm1+fillPwm;
        motorPwm2=motorPwm2+fillPwm;
        motorPwm3=motorPwm3+fillPwm;
        motorPwm4=motorPwm4+fillPwm;
    }

    float motorPWM2[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
    float maxPWM = motorPWM2[0];
    for(i=0; i<4; i++){ // Get max PWM for trimming
        if(motorPWM2[i]>maxPWM){
            maxPWM=motorPWM2[i];
        }
    }

    cout << " MaxPWM = " << maxPWM << endl;

    if(maxPWM>maxThrottle){
        float trimPwm=maxPWM-maxThrottle; //Get excess and use this to trim all 4 motors
        cout << " Trim = " << trimPwm << endl;
        motorPwm1=motorPwm1-trimPwm;
        motorPwm2=motorPwm2-trimPwm;
        motorPwm3=motorPwm3-trimPwm;
        motorPwm4=motorPwm4-trimPwm;
    }

//PWM Output
    gpioPWM(24,motorPwm1);  //1
    gpioPWM(17,motorPwm2);  //2
    gpioPWM(22,motorPwm3);  //3
    gpioPWM(18,motorPwm4);  //4
</code>```


问:
Plotting location using wheel encoder data
Context: I am working with the SFU Mountain Dataset [http://autonomylab.org/sfu-mountain-dataset/]

The UGV image - via the SFU Mountain Dataset website:

I have used the following state update equations (Husky A200 - differential drive)

State Update - from Prob. Robotics, Thrun et. al [x' y' theta'] represent the state at the next time step

After plotting the x and y positions based on just the wheel encoder data (v_fwd and w -> the dataset provides these directly, instead on the vr and vl), the curve seems to be quite weird and unexpected.
Wheel Odometry Data - http://autolab.cmpt.sfu.ca/files/datasets/sfu-mountain-workshop-version/sfu-mountain-torrent/encoder-dry-a.tgz

Blue - Wheel Odom  |  Red - GPS

Actual path!

Question: Is the above curve expected (considering the inaccuracy of wheel odometry) or is there something I'm missing? If the wheel encoder data is that bad, will an EKF (odom + imu) even work?

PS: I'm not worried about the EKF (update step) just as yet. What concerns me more is the horrible wheel odometry data.

答: Yes, I have experienced this. Wheel encoders are great for a second to second hint for an ekf predict step, but are generally awful for long term, long distance prediction. Odometery and imu can do better, but both are integrators and will accumulate error quickly. Add GPS, terrain features, or other global estimates for a real solution.</p>


答:
Thanks for the update. Now it looks like $x_c$ and $y_c$ denote the origin/starting position, and $\theta$ is positive, measured CCW from the positive x-axis. Now I am even more concerned about the equations you're using. Consider just $x$. You have:

$$
x_k = x_{k-1} - \frac{v}{\omega} \sin{(\theta)} + \frac{v}{\omega} \sin{(\theta + \omega \Delta t)}
$$

So,

Consider the term $\frac{v}{\omega}$. If there is no yawing motion, $\omega = 0$, then what happens? That term is divide-by-zero. If $\omega$ happens to be near zero, the term explodes to infinity.

Consider #1 to not be broken, and imagine no yaw at a heading of $\theta = 0$, which means the vehicle is pointing in-line with the +x axis. However, assuming #1 is a non-issue, x values update directly proportional to $\sin{\theta}$, which evaluates to zero. That is, per the diagram and code provided, when the vehicle is in line with the x-axis it is not possible to change the x-coordinate of the vehicle.

Ignoring #1 and #2 above, there is no time-dependency on the non-x terms. This means that, whether the sample frequency is 10Hz or 1000Hz, each sample will accumulate with the same weight. This is akin to me saying I've traveled another 60 miles every time I look at the speedometer in my car, regardless of how frequently I check it. I (you) don't include how long the vehicle has been at that speed, so any check of vehicle speed is improperly weighted.

Currently you only show (a picture of!) the algorithm you're using to generate positions. The more you can post the easier it is to help - the actual code would help the most.

Title: Plotting location using wheel encoder data

Body:
Context: I am working with the SFU Mountain Dataset [http://autonomylab.org/sfu-mountain-dataset/]

The UGV image - via the SFU Mountain Dataset website:

I have used the following state update equations (Husky A200 - differential drive)

State Update - from Prob. Robotics, Thrun et. al [x' y' theta'] represent the state at the next time step

After plotting the x and y positions based on just the wheel encoder data (v_fwd and w -> the dataset provides these directly, instead on the vr and vl), the curve seems to be quite weird and unexpected.
Wheel Odometry Data - http://autolab.cmpt.sfu.ca/files/datasets/sfu-mountain-workshop-version/sfu-mountain-torrent/encoder-dry-a.tgz

Blue - Wheel Odom  |  Red - GPS

Actual path!

Question: Is the above curve expected (considering the inaccuracy of wheel odometry) or is there something I'm missing? If the wheel encoder data is that bad, will an EKF (odom + imu) even work?

PS: I'm not worried about the EKF (update step) just as yet. What concerns me more is the horrible wheel odometry data.


问:
What is the best way to attach a 3D printed part to a servo for robotics use?
I am trying to make custom parts that fit directly onto a servo. Doing this has proved more difficult than I've expected so far.

I was hoping to avoid incorporating the provided servo horns into the 3D printed part, so I've been trying this method out. Below are images of my current test - a 3D printed attachment to the servo, with an indentation for an M3 nut (the servo accepts an M3 bolt) for attachment to the servo. The plastic ring doesn't have the spline (I can't print that level of detail I think) but is tight around it. The top piece attaches to a 3/8" nut for use with the 3/8" threaded rod I had laying around.

So far, I'm having difficulty of this setup working with any level for torque and not just spinning in place.

So... is this the correct approach? Am I going to have to design a piece with the servo horn inside of it to get the servo to connect? Are there better approaches I haven't considered?

答:
Sounds more like a material/mechanics of materials problem to me. I'm assuming this is an FDM part, and the M3 nut is metal? The nut will have no trouble stripping this. FDM parts are far from 'full strength' parts, nearly all 3D printing is. There are too many 'spaces' inside of the print itself. This gets compounded by your design itself, since this part leaves quite a bit of room for leverage. See how the holes that allow the two piece of mate together are almost the same distance as the diameter of the nut? That is too far for a plastic part interfacing with a metal part. You need to picture an imaginary lever from the center of each of those six pins to the center of the nut, then another six levers from the center of the nut to each vertex of the nut. The plastic part is going to be able to exert far greater force than the nut, but the plastic will deform and the nut will not. The plastic is set up to destroy itself right now, and the nut will make itself a nice hole to spin inside of. Either figure out a way to get it made from metal, or decrease the size of the printed part. I would recommend metal if you can afford the weight and plan on applying any kind of torque to this system.  Get quotes from some rapid prototyping services or local machine shops to get a few of these made from steel, it will probably be cheaper than you think. Just be willing to work with them, and take their advice if they suggest a design change to aid in manufacturability.

All that said, you would be better off interfacing with the servo horn if you can fit it into your design IMO. It would be better able to handle any torque in the system, regardless of material.


答:
A few things:

I took a look at your data set. Did you make sure you used the time column correctly? The first entry is "1429481388546050050" without the decimal. To make it in seconds, it should be 1429481388.546050050.

Your motion model is fine (I've used it before, for people who want to see it derived, it is very similar to this one). However, to avoid dividing by zero, you should be checking the value for angular velocity, and if it below a certain threshold, you need to change it to

$$
\begin{bmatrix}
x_{k+1} \\ y_{k+1} \\ \theta_{k+1}
\end{bmatrix}
=
\begin{bmatrix}
x_k \\ y_k \\ \theta_k
\end{bmatrix}
+
\begin{bmatrix}
v_k\Delta t\cos(\theta_k) \\ v_k\Delta t\sin(\theta_k) \\ \omega_k\Delta t
\end{bmatrix}
$$

The wheel encoders on the Husky are actually pretty good. If you can get your hands on the raw data (i.e., the ticks), you might get a better result. However, this requires that it was somewhat well calibrated (i.e., how many ticks per metre of travel).

If this were an indoor test, I would tell you that yes, you should definitely get better results. But it looks like the Husky was travelling on pretty rough terrain. If you've compensated for the two things I mentioned, it might just be the tough environment!


问:
What kind of sensor I can use to identify which fruit it is (like mango or apple)?
What kind of sensor I can use to identify which fruit it is (like mango or apple). Moreover, is there any sensor to identify different varieties of apples or mangoes. </p>

答:
To answer your question I can think of at least: RGB camera, NIR camera, laser scanner.

What you do with the sensors is much more difficult, for example, you have an RGB image of a mango fruit on a tree, how do you find which pixels are mango and which are not (e.g. tree, sky, ground). This is an ongoing research topic in computer vision and agricultural robotics.  Some papers show results under controlled conditions (in a lab) or semi-controlled (using assumptions, shading and controlled lighting).

It really depends on the assumptions you can make, then you can match by colour, size and visual texture, amongst other properties.


问:
How does information gain based exploration differ from frontier based?
I've recently come across the concept of using information gain (or mutual information criteria) as a metric for minimizing entropy on a map to aid in robotic exploration. I have somewhat of a basic question about it.

A lot of papers that talk about minimizing entropy consider an example case of something like a laser scanner and try to compute the 'next best pose' so that the maximum entropy reduction is achieved. Usually this is mentioned like "information gain based approaches help finding the best spot to move the robot such that the most entropy is minimized using raycasting techniques, as opposed to frontier based exploration which is greedy" etc. But I don't understand what the underlying reason is for information gain/entropy based exploration being better.

Let's say a robot in a room with three walls and open space in front. Because of range limitations, it can only see two walls: so in frontier based exploration, the robot has two choices; move towards the third wall and realize it's an obstacle, or move towards the open space and keep going. How does an information gain based method magically pick the open space frontier over the wall frontier? When we have no idea what's beyond our frontiers, how can raycasting even help?

答:
Frontier based exploration is concerned primarily with exploring the physical space in order to produce an occupancy grid (or cost map) of the terrain traversibility. The control actions follow a set of rules which work empirically well (but not theoretically optimally) to achieve for the frontier-exploration goal.

Information-gain methods can be used to specify any objective that you can model mathematically. Because they are probabilistic the prior beliefs of the system can also be modelled, which gives the system a better idea than a set of simple rules.

Take the example of the Mars Rover Curiosity looking for stromatolites (fossilised bacteria) on an unexplored region of Mars:

A grid of 100x100 represents the area to be explored

For each point on the grid, the grid could be either a Ordinary rock, Interesting rock or Stromatolite (class labels $\{O, I, S\}$)

The roboticists work with the geologists to provide Curiosity with expert knowledge of geology concerning stromatolite discovery:

$P(x=S | x_n=I) = 0.05$, which means the probability of the grid at x being a stromatolite, given that any if x's neighbour is an Interesting rock, is 5%.

$P(x=S | x_n=S) = 0.35$, similarly this encodes the domain knowledge that stromatolites occur spatially close to one another

In such a system, the info-gain system would choose the action that would tell us the most about the population of rock classes in this map, taking in to account the domain knowledge, sensor uncertainty model and current state of the map.

This is contrasted to the frontier mapping method which can only find the shape of the map, generally using non-probabilistic methods (probabilistic frontier-exploration exist).


问:
Using genetic algorithm for tuning controllers
I've read some papers for controlling nonlinear systems (e.g. nonlinear pendulum). There are several approaches for targeting nonlinear systems. The most common ones are feedback linearizaing, backstepping, and sliding mode controllers.

In my case, I've done the theoretical and practical parts of controlling nonlinear model of a simple pendulum plus other manipulators problems in C++. For the pendulum, I've utilized a backstepping controller for solving the tracking task for the angular displacement and velocity. The results are

$$
\ddot{\theta} + (k/m) \dot{\theta} + (g/L) \sin\theta= u
$$

where $m=0.5, k=0.0001, L=.2$ and $g=9.81$.

The results are good. However, tuning the controller is time consuming. The majority of papers use genetic algorithms for tuning their controllers such as PD, PID, and backstepping controllers. I'm clueless in this field and I hope someone sheds some light on this concept, preferable if there is a MATLAB sample for at least controlling a simple pendulum.

So far I've designed a simple GUI in C++/Qt in order to tune the controller manually. In the below picture, the response of the controller for step function.

答: I am confused by what you are calling a genetic algorithm.  The PD, PID, etc., controllers are not heuristic nor stochastic methods, and they are not based on evolutionary algorithms.  They are, instead, deterministic algorithms intended to allow placement of poles and zeroes of a system response to provide desired performance (error minimization, speed of response, settling time, stability, and disturbance rejection).  A good learning reference, in my opinion, is Kuo's Automatic Control Systems book.  You can find a less rigorous treatment in Raven's Automatic Control Engineering book.


问:
Storing Kinect Data on a USB Drive
Does anybody know if Kinect Data can be stored directly onto a USB Drive??
I have a Kinect for Windows that i cannot use on Linux(ROS). However what i plan is to mount the Kinect on my robot, store the captured frames on a USB and then un mount the USB ,transfer to Linux and process them on ROS.

Is this possible?? Any suggestions.

答:
Can Kinect data be stored directly onto a USB drive?

Yes. In ROS that would be easily done using bag files. In Windows, however, you might have to look for something native in the Microsoft SDK/OpenNI or code it yourself. Edit: In OpenNI there is something called .oni files for that purpose.

The implicit question: Does Kinect for Windows work in Linux?

As far as I am aware, yes. From what I researched there were compatibility issues with OpenNI initially but they seem to be solved as of May 2012. There are reports of users successfully using K4W in both Windows and Linux there. See this previous question for more info.

I assume the changes have made their way upstream, but the only way to be sure is to test it. If they haven't, you can always use the patched version referred in the link.

Conclusion

My suggestion is get your sensor working in Linux and don't waste your time trying to figure out how to save Kinect data to a USB drive in Windows.

Title: Storing Kinect Data on a USB Drive


答:
Genetic algorithms are a machine learning technique to remove the need for a user to 'fine tune' a system, and instead allow a computer to figure out how to tune the system. The benefits of this approach are that it frees up the researchers/users time and often can lead to a system that is tuned better than what the researcher/user would have been able to achieve themselves.

The general idea of using a genetic algorithm would follow some basic control flow as follow:

```<code>Create_X_Number_Of_Random_Controllers()
while (Not_Yet_Satisfied_With_tuning)
    Score_Each_Controller_Based_On_Performance()
    Remove_Lowest_Performing_Controllers()
    Create_New_Controllers_By_Mixing_Best_Performers()
    Add_Slight_Randomness_For_Evolution()
Display_Top_Performer()
</code>```

The best example I can easily show of this principle in practice is this 'game' that uses genetic evolution to design vehicles that are capable of driving over a course. The same idea applies to any system you want to fine tune without having to actual do any tuning yourself.
Example: http://rednuht.org/genetic_cars_2/

Title: Using genetic algorithm for tuning controllers

Body:
I've read some papers for controlling nonlinear systems (e.g. nonlinear pendulum). There are several approaches for targeting nonlinear systems. The most common ones are feedback linearizaing, backstepping, and sliding mode controllers.

In my case, I've done the theoretical and practical parts of controlling nonlinear model of a simple pendulum plus other manipulators problems in C++. For the pendulum, I've utilized a backstepping controller for solving the tracking task for the angular displacement and velocity. The results are

$$
\ddot{\theta} + (k/m) \dot{\theta} + (g/L) \sin\theta= u
$$

where $m=0.5, k=0.0001, L=.2$ and $g=9.81$.

The results are good. However, tuning the controller is time consuming. The majority of papers use genetic algorithms for tuning their controllers such as PD, PID, and backstepping controllers. I'm clueless in this field and I hope someone sheds some light on this concept, preferable if there is a MATLAB sample for at least controlling a simple pendulum.

So far I've designed a simple GUI in C++/Qt in order to tune the controller manually. In the below picture, the response of the controller for step function.

Body:
Genetic algorithms are a machine learning technique to remove the need for a user to 'fine tune' a system, and instead allow a computer to figure out how to tune the system. The benefits of this approach are that it frees up the researchers/users time and often can lead to a system that is tuned better than what the researcher/user would have been able to achieve themselves.

The general idea of using a genetic algorithm would follow some basic control flow as follow:

Create_X_Number_Of_Random_Controllers()
while (Not_Yet_Satisfied_With_tuning)
    Score_Each_Controller_Based_On_Performance()
    Remove_Lowest_Performing_Controllers()
    Create_New_Controllers_By_Mixing_Best_Performers()
    Add_Slight_Randomness_For_Evolution()
Display_Top_Performer()

The best example I can easily show of this principle in practice is this 'game' that uses genetic evolution to design vehicles that are capable of driving over a course. The same idea applies to any system you want to fine tune without having to actual do any tuning yourself.
Example: http://rednuht.org/genetic_cars_2/


问:
EKF SLAM (prediction of new landmarks)
Prediction of new landmarks are commonly expressed as:

```<code>        Xm = Xr + r*cos(phi + theta_r),
        Ym = Yr + r*sin(phi + theta_r)
</code>```

However this is only true for point landmarks. What if I am extracting line feature?

答:
This is true because landmarks are often encapsulated by just their coordinates. It's a toy example. In real life, you'd have to use all the parameters necessary to encapsulate the information about the line feature. For example. Lines are often represented by a point and orientation.

here's an example

Title: EKF SLAM (prediction of new landmarks)


问:
Check collision between robot and environment in OpenRAVE
I have a robot arm in an environment. How can I check for collision between this robot arm and the environment?</p>

答:
It's really easy.

```<code>env = openravepy.Environment()
env.StopSimulation()
env.Load('_ArmFileName'_.xml') #Whatever .xml file you're using
robot = env.GetRobots()[0]
manip = robot.SetActiveManipulator('left_arm') #whatever arm you're using
robot.SetDOFValues(joint_values, manip.GetArmIndices())
env.CheckCollision(robot)
</code>```

where joint_values is the an array of the joint values that you want. For example, if I was working on the Baxter robot, then I'd make an array of length 7 (7 DOF arm) with each value being values within Baxter's min/max joint values. Also note that you need to specify in the xml that your robot has the appropriate arm.

Body:
It's really easy.

env = openravepy.Environment()
env.StopSimulation()
env.Load('_ArmFileName'_.xml') #Whatever .xml file you're using
robot = env.GetRobots()[0]
manip = robot.SetActiveManipulator('left_arm') #whatever arm you're using
robot.SetDOFValues(joint_values, manip.GetArmIndices())
env.CheckCollision(robot)

where joint_values is the an array of the joint values that you want. For example, if I was working on the Baxter robot, then I'd make an array of length 7 (7 DOF arm) with each value being values within Baxter's min/max joint values. Also note that you need to specify in the xml that your robot has the appropriate arm.


问:
How to align solidworks global origin with assembly origin while exporting in solidworks to urdf
I have created a robot model in solidworks and exported in solidworks to urdf plug-in. When exporting the co-ordinates of the model is misaligned which is causing problem while using in ROS.

As you could see in picture the Z-axis is horizontal in assembly whereas vertical in solidworks. How to align these co-ordinates. The generated co-ordinate system must be similar to solidworks' co-ordinates

PS: I have mated the assembly origin and base_link origin

答:
I did a little step-by-step tutorial with images, but if my other answer regarding aligning frames didn't work well for you, or the definition of "Front Plane" or "Top Plane" is confusing in Solidworks (spoiler: it is), then consider making your own axes.

From the assembly tab, go to reference geometry -> axis, then select the assembly planes to make an axis. There are three possible combinations of planes, giving you three unique axes (1-2, 1-3, 2-3).

Then, compare the axes to the global axes, and rename them to the same for clarity later.

Then, delete the old mates that are keeping the assembly grounded, then re-mate whatever lines you have that determine a particular frame to your newly-named axes you just made.

This should fix the problem for you.


问:
KUKA FRI program using JAVA
I am trying to establish the FRI connection for KUKA LBR iiwa. I know how to configure the FRI connection as there are example programs available in the Sunrise.Workbench.

A sample code is given below. My question is 'how to pass' the joint torque values (or joint position or wrench) to the controller using 'torqueOverlay' as mentioned in the code below. Since I could not find any documentation on this, it was quite difficult to figure out. Any sample code with explanation or any clues would be more than helpful.

JAVA code:

```<code>package com.kuka.connectivity.fri.example;

import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;

import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;

import com.kuka.connectivity.fri.ClientCommandMode;
import com.kuka.connectivity.fri.FRIConfiguration;
import com.kuka.connectivity.fri.FRIJointOverlay;
import com.kuka.connectivity.fri.FRISession;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.motionModel.PositionHold;
import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode;

/**
 * Moves the LBR in a start position, creates an FRI-Session and executes a
 * PositionHold motion with FRI overlay. During this motion joint angles and
 * joint torques can be additionally commanded via FRI.
 */
public class LBRTorqueSineOverlay extends RoboticsAPIApplication
{
    private Controller _lbrController;
    private LBR _lbr;
    private String _clientName;

    @Override
    public void initialize()
    {
        _lbrController = (Controller) getContext().getControllers().toArray()[0];
        _lbr = (LBR) _lbrController.getDevices().toArray()[0];
        // **********************************************************************
        // *** change next line to the FRIClient's IP address                 ***
        // **********************************************************************
        _clientName = "127.0.0.1";
    }

    @Override
    public void run()
    {
        // configure and start FRI session
        FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _clientName);
        // for torque mode, there has to be a command value at least all 5ms
        friConfiguration.setSendPeriodMilliSec(5);
        friConfiguration.setReceiveMultiplier(1);

        getLogger().info("Creating FRI connection to " + friConfiguration.getHostName());
        getLogger().info("SendPeriod: " + friConfiguration.getSendPeriodMilliSec() + "ms |"
                + " ReceiveMultiplier: " + friConfiguration.getReceiveMultiplier());

        FRISession friSession = new FRISession(friConfiguration);
        FRIJointOverlay torqueOverlay = new FRIJointOverlay(friSession, ClientCommandMode.TORQUE);

        // wait until FRI session is ready to switch to command mode
        try
        {
            friSession.await(10, TimeUnit.SECONDS);
        }
        catch (final TimeoutException e)
        {
            getLogger().error(e.getLocalizedMessage());
            friSession.close();
            return;
        }

        getLogger().info("FRI connection established.");

        // move to start pose
        _lbr.move(ptp(Math.toRadians(90), Math.toRadians(-60), .0, Math.toRadians(60), .0, Math.toRadians(-60), .0));

        // start PositionHold with overlay
        JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(200, 200, 200, 200, 200, 200, 200);
        PositionHold posHold = new PositionHold(ctrMode, 20, TimeUnit.SECONDS);

        _lbr.move(posHold.addMotionOverlay(torqueOverlay));

        // done
        friSession.close();
    }

    /**
     * main.
     *
     * @param args
     *            args
     */
    public static void main(final String[] args)
    {
        final LBRTorqueSineOverlay app = new LBRTorqueSineOverlay();
        app.runApplication();
    }

}
</code>```

答:
Currently FRI can only be controlled in C++.

Java is also necessary to initialize the FRI interface. The C++ code if found in a file called FRI-Client-SDK_Cpp.zip which is included in the kuka connectivity suite. You need to go in the menus to the software components and check the box that enables FRI (fast robot interface). Look in the docs for how to enable/disable software components. Enabling the FRI component will dump this zip file into a folder within your sunrise project for you.

You may just want to try using the grl library itself because it provides an easier to use FRI wrapper. It isn't 100% perfect but is has run successfully for the FRI position motion case. Please note that some manual patching of FRI-Client-SDK_Cpp.zip is necessary to build the grl kuka drivers because distribution of the zip file itself isn't yet allowed. Disclaimer: I'm the author of grl.

Here is grl Java code for starting FRI

Source that calls HoldPosition then drives via FRI exclusively:

```<code>package grl.friCommunication;

import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;

import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;

import com.kuka.connectivity.fri.FRIConfiguration;
import com.kuka.connectivity.fri.FRIJointOverlay;
import com.kuka.connectivity.fri.FRISession;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.deviceModel.LBR;

/**
 * Creates a FRI Session.
 */
public class FRI_Command extends RoboticsAPIApplication
{
    private Controller _lbrController;
    private LBR _lbr;
    private String _hostName;

    @Override
    public void initialize()
    {
        _lbrController = (Controller) getContext().getControllers().toArray()[0];
        _lbr = (LBR) _lbrController.getDevices().toArray()[0];
        // **********************************************************************
        // *** change next line to the FRIClient's IP address                 ***
        // **********************************************************************
        _hostName =  "192.170.10.100";
    }

    @Override
    public void run()
    {
        // configure and start FRI session
        FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _hostName);
        friConfiguration.setSendPeriodMilliSec(10);
        FRISession friSession = new FRISession(friConfiguration);
        FRIJointOverlay motionOverlay = new FRIJointOverlay(friSession);

        // wait until FRI session is ready to switch to command mode
        try
        {
            friSession.await(10, TimeUnit.SECONDS);
        }
        catch (final TimeoutException e)
        {
            return;
        }

        // move to start pose
        _lbr.move(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(90), .0));

         for(int i = 0; i < 100; i++) {
             // async move with overlay ...
             _lbr.moveAsync(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(90), .0)
                     //.setJointVelocityRel(0.0)
                     .addMotionOverlay(motionOverlay)
                     .setBlendingRel(0.0)
                     );

             // ... blending into sync move with overlay
             _lbr.moveAsync(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(-90), .0)
                     //.setJointVelocityRel(0.0)
                     .addMotionOverlay(motionOverlay)
                     .setBlendingRel(0.0)
                     );
         }

        _lbr.move(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(-90), .0)
                .addMotionOverlay(motionOverlay)
                );
        // done
        friSession.close();
    }

    /**
     * main.
     *
     * @param args
     *            args
     */
    public static void main(final String[] args)
    {
        final FRI_Command app = new FRI_Command();
        app.runApplication();
    }

}
</code>```

FRI code is available https://github.com/ahundt/grl. Disclosure: I'm the author of grl.

Title: KUKA FRI program using JAVA

Body:
Currently FRI can only be controlled in C++.

Java is also necessary to initialize the FRI interface. The C++ code if found in a file called FRI-Client-SDK_Cpp.zip which is included in the kuka connectivity suite. You need to go in the menus to the software components and check the box that enables FRI (fast robot interface). Look in the docs for how to enable/disable software components. Enabling the FRI component will dump this zip file into a folder within your sunrise project for you.

You may just want to try using the grl library itself because it provides an easier to use FRI wrapper. It isn't 100% perfect but is has run successfully for the FRI position motion case. Please note that some manual patching of FRI-Client-SDK_Cpp.zip is necessary to build the grl kuka drivers because distribution of the zip file itself isn't yet allowed. Disclaimer: I'm the author of grl.

Here is grl Java code for starting FRI

Source that calls HoldPosition then drives via FRI exclusively:

package grl.friCommunication;

import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;

import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;

import com.kuka.connectivity.fri.FRIConfiguration;
import com.kuka.connectivity.fri.FRIJointOverlay;
import com.kuka.connectivity.fri.FRISession;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.deviceModel.LBR;

/**
 * Creates a FRI Session.
 */
public class FRI_Command extends RoboticsAPIApplication
{
    private Controller _lbrController;
    private LBR _lbr;
    private String _hostName;

    @Override
    public void initialize()
    {
        _lbrController = (Controller) getContext().getControllers().toArray()[0];
        _lbr = (LBR) _lbrController.getDevices().toArray()[0];
        // **********************************************************************
        // *** change next line to the FRIClient's IP address                 ***
        // **********************************************************************
        _hostName =  "192.170.10.100";
    }

    @Override
    public void run()
    {
        // configure and start FRI session
        FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _hostName);
        friConfiguration.setSendPeriodMilliSec(10);
        FRISession friSession = new FRISession(friConfiguration);
        FRIJointOverlay motionOverlay = new FRIJointOverlay(friSession);

        // wait until FRI session is ready to switch to command mode
        try
        {
            friSession.await(10, TimeUnit.SECONDS);
        }
        catch (final TimeoutException e)
        {
            return;
        }

        // move to start pose
        _lbr.move(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(90), .0));

         for(int i = 0; i < 100; i++) {
             // async move with overlay ...
             _lbr.moveAsync(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(90), .0)
                     //.setJointVelocityRel(0.0)
                     .addMotionOverlay(motionOverlay)
                     .setBlendingRel(0.0)
                     );

             // ... blending into sync move with overlay
             _lbr.moveAsync(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(-90), .0)
                     //.setJointVelocityRel(0.0)
                     .addMotionOverlay(motionOverlay)
                     .setBlendingRel(0.0)
                     );
         }

        _lbr.move(ptp(Math.toRadians(90), .0, .0, Math.toRadians(90), .0, Math.toRadians(-90), .0)
                .addMotionOverlay(motionOverlay)
                );
        // done
        friSession.close();
    }

    /**
     * main.
     *
     * @param args
     *            args
     */
    public static void main(final String[] args)
    {
        final FRI_Command app = new FRI_Command();
        app.runApplication();
    }

}

FRI code is available https://github.com/ahundt/grl. Disclosure: I'm the author of grl.

ContentLicense: CC BY-SA 4.0


问:
Is it possible to both move and stabilize a two wheeled robot with no gyroscopes?
With two wheeled robot like this one, I have managed to stabilize it while keeping it stationary. This was done using a digital feedback control system by reading the position of the wheels to determine position, and the natural back electromotive force from the wheel motors was used in the feedback loop to determine velocity. It was kept stable with a PID controller, which was designed using a root locus algorithm to keep it stable and modulate the performance parameters (such as percent overshoot, settling time, etc.). I wanted to attempt to keep it stable while simultaneously propelling it forward, but I couldn't figure out how to go about designing a linear controller that could do that. Is it possible to both propel the robot forward and keep it stable using a feedback controller on the wheels, or is a gyroscope necessary?

答:
You need some sensors to detect the state of the system.

First linearize the system into a state space form, then consider what sensors you do have. Then check if it is observable. If it is observable, then you can feed the estimated states into your controller.

Currently, it sounds like you are using the wheel position and back EMF (for velocity) as direct measurements. Without checking the observability matrix, I am unsure if the system is observable.


答:
You need first to know how much current your motor draws at no load, and also at stall.
After that you need to choose a transistor that its collector or its drain can support the stall current.

You have to know that your current configuration is just for a one direction control, and to which direction that depends on the polarity.

You need an H-Bridge like L293-L298 for controlling in two directions.

Title: High voltage motor control with arduino

Body:
I'm trying to control a higher voltage motor than an arduino can source with a pin, with an arduino. I am trying to hook it up to a transistor. The battery pack is not supposed to be 4.8V, it's 6V, 4 D batteries.

Here is the setup:

Here is the arduino code I'm trying to run to it:

```<code>int motorpin = 2;

void setup()
{
    pinMode(motorpin, OUTPUT);
}

void loop()
{
    digitalWrite(motorpin, HIGH);
    delay(500);
    digitalWrite(motorpin, LOW);
    delay(500);
}
</code>```

Code gives me no errors, but no motor movement happens. What would make this work? Thanks.


问:
A Vector Field Histogram implementation in Python 2.7
I am trying to implement the Vector Field Histogram as described by Borenstein, Koren, 1991 in Python 2.7 using the SciPy stack.

I have already been able to calculate the polar histogram, as described in the paper, as well as the smoothing function to eliminate noise. This variable is stored in a numpy array, named self.Hist.

However, the function computeTheta, pasted below, which computes the steering direction, is only able to compute the proper direction if the valleys (i.e. consecutive sectors in the polar histogram whose obstacle density is below a certain threshold) do not contain the section where a full circle is completed, i.e. the sector corresponding to 360º.

To make things clearer, consider these two examples:

If the histogram contains a peak in the angles between, say, 330º and 30º, with the rest of the histogram being a valley, then the steering direction will be computed correctly.

If, however, the peak is contained between, say, 30º and 60º, then the valley will start at 60º, go all the way past 360º and end in 30º, and the steering direction will be computed incorrectly, since this single valley will be considered two valleys, one between 0º and 30º, and another between 60º and 360º.

def computeTheta(self, goal):

```<code>thrs = 2.
s_max = 18

#We start by calculating the sector corresponding to the direction of the target.
target_sector = int((180./np.pi)*np.arctan2(goal[1] - self.VCP[1], goal[0] - self.VCP[0]))
if target_sector < 0:
    target_sector += 360
target_sector /= 5

#Next, we assume there is no best sector.
best_sector = -1
dist_best_and_target = abs(target_sector - best_sector)

#Then,  we find the sector within a valley that is closest to the target sector.
for k in range(self.Hist.shape[0]):
    if self.Hist[k] < thrs and abs(target_sector - k) < dist_best_and_target:
        best_sector = k
        dist_best_and_target = abs(target_sector - k)

#If the sector is still -1, we return it as an error.
print (target_sector, best_sector)
if best_sector == -1:
    return -1

#If not, we can proceed...
elif best_sector > -1:
    #... by deciding whether the valley to which the best sector belongs is a "wide" or a "narrow" one.
    #Assume it's wide.
    type_of_valley = "Wide"

    #If we find a sector that contradicts our assumption, we change our minds.
    for sector in range(best_sector, best_sector + s_max + 1):
        if sector < self.Hist.shape[0]:
            if self.Hist[sector] > thrs:
                type_of_valley = "Narrow"

    #If it is indeed a wide valley, we return the angle corresponding to the sector (k_n + s_max)/2.
    if type_of_valley == "Wide":

        theta = 5*(best_sector + s_max)/2
        return theta

    #Otherwise, we find the far border of the valley and return the angle corresponding to the mean value between the best sector and the far border.
    elif type_of_valley == "Narrow":
        for sector in range(best_sector, best_sector + s_max):
            if self.Hist[sector] < thrs:
                far_border = sector

        theta = 5*(best_sector + far_border)/2
        return theta
</code>```

How can I address this issue? Is there a way to treat the histogram as circular? Is there maybe a better way to write this function?

Thank you for your time.

答:
Because you are using Python's arctan2 function (which returns values between $[-\pi, \pi]$), I don't see why you need to normalize the target sector angle using</p>

```<code>if target_sector < 0:
    target_sector += 360
</code>```

Body:
Because you are using Python's arctan2 function (which returns values between $[-\pi, \pi]$), I don't see why you need to normalize the target sector angle using

if target_sector < 0:
    target_sector += 360


答:
I'm having a hard time following your code, partly because I don't know Python but I think mostly because I'm not sure I understand your variables. That said, I think I do understand your problem.

An approach I would take to solve this would be to "warp" the data before evaluation, then make your decisions, then "de-warp" the output. For example, consider the following pseudo-code:

```<code>headingOffset = 0;
while(histogram[0] != peak)
    headingOffset = headingOffset + 1;
    histogram = CircularShift(histogram, -1);
end

<find heading>
heading = CircularShift(heading,+headingOffset);
</code>```

Hopefully I've made this clear; basically, re-orient your data such that you always start at a peak. This will ensure a valley always ends <= 360 degrees. Then, once you find your heading, shift it according to how far you shifted your data to start with.

Title: A Vector Field Histogram implementation in Python 2.7

Body:
I am trying to implement the Vector Field Histogram as described by Borenstein, Koren, 1991 in Python 2.7 using the SciPy stack.

I have already been able to calculate the polar histogram, as described in the paper, as well as the smoothing function to eliminate noise. This variable is stored in a numpy array, named self.Hist.

However, the function computeTheta, pasted below, which computes the steering direction, is only able to compute the proper direction if the valleys (i.e. consecutive sectors in the polar histogram whose obstacle density is below a certain threshold) do not contain the section where a full circle is completed, i.e. the sector corresponding to 360º.

To make things clearer, consider these two examples:

If the histogram contains a peak in the angles between, say, 330º and 30º, with the rest of the histogram being a valley, then the steering direction will be computed correctly.

If, however, the peak is contained between, say, 30º and 60º, then the valley will start at 60º, go all the way past 360º and end in 30º, and the steering direction will be computed incorrectly, since this single valley will be considered two valleys, one between 0º and 30º, and another between 60º and 360º.

def computeTheta(self, goal):

```<code>thrs = 2.
s_max = 18

#We start by calculating the sector corresponding to the direction of the target.
target_sector = int((180./np.pi)*np.arctan2(goal[1] - self.VCP[1], goal[0] - self.VCP[0]))
if target_sector < 0:
    target_sector += 360
target_sector /= 5

#Next, we assume there is no best sector.
best_sector = -1
dist_best_and_target = abs(target_sector - best_sector)

#Then,  we find the sector within a valley that is closest to the target sector.
for k in range(self.Hist.shape[0]):
    if self.Hist[k] < thrs and abs(target_sector - k) < dist_best_and_target:
        best_sector = k
        dist_best_and_target = abs(target_sector - k)

#If the sector is still -1, we return it as an error.
print (target_sector, best_sector)
if best_sector == -1:
    return -1

#If not, we can proceed...
elif best_sector > -1:
    #... by deciding whether the valley to which the best sector belongs is a "wide" or a "narrow" one.
    #Assume it's wide.
    type_of_valley = "Wide"

    #If we find a sector that contradicts our assumption, we change our minds.
    for sector in range(best_sector, best_sector + s_max + 1):
        if sector < self.Hist.shape[0]:
            if self.Hist[sector] > thrs:
                type_of_valley = "Narrow"

    #If it is indeed a wide valley, we return the angle corresponding to the sector (k_n + s_max)/2.
    if type_of_valley == "Wide":

        theta = 5*(best_sector + s_max)/2
        return theta

    #Otherwise, we find the far border of the valley and return the angle corresponding to the mean value between the best sector and the far border.
    elif type_of_valley == "Narrow":
        for sector in range(best_sector, best_sector + s_max):
            if self.Hist[sector] < thrs:
                far_border = sector

        theta = 5*(best_sector + far_border)/2
        return theta
</code>```

How can I address this issue? Is there a way to treat the histogram as circular? Is there maybe a better way to write this function?

Thank you for your time.

Body:
I'm having a hard time following your code, partly because I don't know Python but I think mostly because I'm not sure I understand your variables. That said, I think I do understand your problem.

An approach I would take to solve this would be to "warp" the data before evaluation, then make your decisions, then "de-warp" the output. For example, consider the following pseudo-code:

headingOffset = 0;
while(histogram[0] != peak)
    headingOffset = headingOffset + 1;
    histogram = CircularShift(histogram, -1);
end

<find heading>
heading = CircularShift(heading,+headingOffset);

Hopefully I've made this clear; basically, re-orient your data such that you always start at a peak. This will ensure a valley always ends <= 360 degrees. Then, once you find your heading, shift it according to how far you shifted your data to start with.


问:
Helicopter Stabilization Algorithm
I've hacked a rc helicopter, and I am able to control it by running a program on my computer. I am interested in writing algorithms that will stabilize the helicopter. For instance, the helicopter is hovering, and then if it is shoved off balance it can return to its previous position in a stable state. Any help on an algorithm would be awesome.</p>

答:
Stabilization of a helicopter and a quadrotor are similar tasks - have a reference signal, compare that to feedback, then act on the difference.

A quad rotor has four motors, and the helicopter arguably does as well: main rotor, tail rotor, swash plate fore/aft servo, swash plate port/stbd servo.

I would bet you can find a helicopter community that could point you to stabilization algorithms specific to helicopters, but it might be easier to build on the (freely available and widely published) resources available for quadrotors.

For instance, you'll need to communicate with a transmitter. Clearly you have that already. You'll need some form of an IMU; a microcontroller to communicate with that, the receiver, and the motors; electronic speed controllers if you find overriding the existing circuit is too difficult; etc.

Once you've got all that, you'll need the code to interface with all of the above.

The only difference really comes down to your model of how a particular control axis affects the motion of the vehicle. You could do modeling in state space, but I think most quadrotors just use PID control on each axis and tune the gains to achieve the desired response.

You can do the same; there will only be a small change. A quadrotor use PID controllers on each axis and then sums those outputs to give values for each motor, because the motions are coupled. Your roll, pitch, yaw, and thrust are all separate, so each controller runs its own motor/servo - no summing.

So, tl;dr - use widely available quadrotor framework, assign each axis to one motor, then do some PID tuning.

Title: Helicopter Stabilization Algorithm


问:
iRobot Create: Making Noise and Flashing Red Light While Charging
My iRobot Create is playing a tune about every 30 seconds and continuously flashing a red light when I attempt to charge it. What is the issue?</p>

答:
From the iRobot support site, List of Roomba 400/Discovery error codes:

As Roomba cleans, it communicates with songs, beeps, and indicator lights. It also uses sounds to let you know when there's a problem or it needs maintenance. Here are the most common error codes and what they mean. If the problem cannot be resolved, contact iRobot Customer Care.

Your particular issue, if the tune being played is a 4-note tune (in conjunction with the flashing Power Button red LED, is due to a low battery which requires charging:

As it says:

Connect Roomba to a battery charger (or Home Base) and recharge the battery.

As your iRobot is continuously making this tune, even whilst charging, this would suggest that either:

the battery is not taking on any charge (i.e. it is dead), or;

the charging circuitry (within the iRobot) is not detecting the charge current, and assumes that it is not plugged in. As a sanity check, verify that the charger is working, and that the connections are OK. Otherwise, maybe the charging circuit is defective, and needs maintenance, or replacing?

This is what should happen, during the charge cycle: Roomba 400 Series/Discovery Battery and Charging Overview.

Under normal conditions, Roomba should charge in this manner:

Your Battery Charger should have a solid green light on when it is plugged into a wall outlet charging Roomba.

Your Home Base, if you have one, should have two solid green lights (one for Power and one for Docked) when it is plugged into the Charger
  and Roomba is charging.

Your Roomba’s POWER light should pulse red/amber while charging. Charging time under normal conditions is between 3 – 6 hours depending
  on your Battery Charger. Charging could take up to 16 hours if your
  battery is deeply discharged at the beginning of the charge cycle.

When the charging cycle is complete, the POWER light on Roomba will turn solid green.

While Roomba is running, the POWER light will display the state of the
  battery as follows:

Green - fully charged

Amber - partially discharged

Red - nearly discharged

Flashing Red - discharged

Always store Roomba charging on the Home Base or with the Charger
  plugged into the robot. For long-term storage, iRobot recommends that
  the battery be fully charged and then removed from the robot and
  stored in a cool, dry place.


答:
Using evolutionary methods (GA is one of them) for parameter tuning is indeed a common method [1] in the control domain. Especially for non-linear systems, the analytic solutions for optimal parameters can be difficult to find. Evolutionary methods are one way to efficiently perform a search for near-optimal parameters.

A very successful and universal method that is widely used is CMA-ES. There are a large number of implementations out there, including for MATLAB. I know that pole balancing in its various forms is often used as a benchmark.

Applying the algorithm is usually not that difficult. Rating the performance of your result - this is called the fitness function in EA - is usually the most involved part.

[1] P.J Fleming, R.C Purshouse, Evolutionary algorithms in control systems engineering: a survey, Control Engineering Practice, Volume 10, Issue 11, November 2002, Pages 1223-1241, ISSN 0967-0661, http://dx.doi.org/10.1016/S0967-0661(02)00081-3.


问:
How to use quaternions to feed a PID quadcopter stabilization loop?
I'm making a quadcopter. I have set up a PID loop to stabilize it to a given Euler angle (pitch and roll). The problem arises when the roll approaches 90 degrees (45 degrees and up). The values don't make sense anymore, as it approaches the gimbal lock. I intend to make it do complex maneuvers like looping etc., which exceeds the 45 degree roll limit.

How can I use quaternions to overcome this problem? (I get quaternions from the MPU-9150.) I have read many articles on the matter of quaternions, but they all talk about rotations in 3D software, and tweening between two rotation points. This makes little sense as I do not know imaginary numbers and matrices.

答:
This paper, Full Quaternion Based Attitude Control for a Quadrotor by
Emil Fresk and George Nikolakopoulos, demonstrates what you are trying to achieve.

Abstract— The aim of this article is to present a novel quaternion based control scheme for the attitude control problem of a quadrotor.
  A quaternion is a hyper complex number of rank 4 that can be utilized
  to avoid the inherent geometrical singularity when representing rigid
  body dynamics with Euler angles or the complexity of having coupled
  differential equations with the Direction Cosine Matrix (DCM). In the
  presented approach both the quadrotor’s attitude model and the
  proposed non-linear Proportional squared (P2) control
  algorithm have been implemented in the quaternion space, without any
  transformations and calculations in the Euler’s angle space or DCM.
  Throughout the article, the merits of the proposed novel approach are
  being analyzed and discussed, while the efficacy of the suggested
  novel quaternion based controller are being evaluated by extended
  simulation results.


答: Much as stated above the power of a servo is limited. You may need to instead use a light interrupted motor as are found in printers combined with a series of reduction gears to allow the force needed to suspend your robot without adding much weight.</p>

Title: What is the best way to attach a 3D printed part to a servo for robotics use?

Body:
I am trying to make custom parts that fit directly onto a servo. Doing this has proved more difficult than I've expected so far.

I was hoping to avoid incorporating the provided servo horns into the 3D printed part, so I've been trying this method out. Below are images of my current test - a 3D printed attachment to the servo, with an indentation for an M3 nut (the servo accepts an M3 bolt) for attachment to the servo. The plastic ring doesn't have the spline (I can't print that level of detail I think) but is tight around it. The top piece attaches to a 3/8" nut for use with the 3/8" threaded rod I had laying around.

So far, I'm having difficulty of this setup working with any level for torque and not just spinning in place.

So... is this the correct approach? Am I going to have to design a piece with the servo horn inside of it to get the servo to connect? Are there better approaches I haven't considered?


问:
Programming 4-digit seven segment display using interrupts only
I have to program an autonomous bot (using an ATmega2560). It has a 4-digit seven segment display attached to it. I have to make the bot traverse through arena while continuously displaying the time in seconds on the seven segment display.

I can't use the code to display on seven segment display in my main() function.

Any help?

答:
While this question may seem to be better suited for SE Arduino, I shall, nevertheless, propose a solution. Employ a ready written Arduino library, as this would save a lot of groundwork. Granted, as you mention that you have a main(), then one can assume that you are not using the Arduino IDE. However, the library (with the .cpp file and .h file) can still be used.

Interestingly, I went through the pain of coding up a seven segment display last year, on the Arduino, albeit only a two digit display, an SL-1255 (see Dual Seven Segment display madness). I even came up with an all singing and dancing Arduino library, SevenSegmentSL1255.

However, I do not recommend you using it, as it does not use interrupts and, much to my chagrin, I found another, more fully featured, seven segment display Arduino library, SevenSeg, after having completed mine. The SevenSeg library will work with four digit displays and it also handles interrupt driven displays.

From section 4.4 Using Interrupt Timers of the PDF,

Running the printing functions in an endless loop to perform multiplexing is not always an ideal way to do things. Outputting information to a 7-segment display is not a computationally intensive task, but due to the delay used for multiplexing, the microcontroller just sits there and waits for most of the time. Sure, you can insert commands taking little time in a loop together with the printing functions, but if they are slightly time-consuming, the display will halt or flicker. For this purpose it is possible to use SevenSegalong with interrupt timers. Then, you can do whatever you want inside loop(){...}, and simply run a high-level printing function only when you want to change what's on the display. The microcontroller will automatically be interrupted just briefly to update the display as needed. You can still change the refresh rate and the duty cycle like normal, the SevenSeg-library will take care of configuring the timers for you.

The library code can be downloaded from here.

An example sketch (again taken from section 4.4 of the PDF) is as follows:

```<code>#include <SevenSeg.h>
SevenSeg disp(11,7,3,5,6,10,2);
const int numOfDigits =4;
int digitPins[numOfDigits]={12,9,8,13};

void setup () {
  disp.setDigitPins(numOfDigits ,digitPins);
  disp.setTimer(2); disp.startTimer();
}

void loop() {
  for(int i=1;i<=10;i++){
    disp.write(i);
    delay(1000); // Or other time-consuming tasks
  }
}

ISR(TIMER2_COMPA_vect) {
  disp.interruptAction();
}
</code>```

Obviously, you will need to modify the sketch for your particular purposes, notably either calling setup() and loop() from your main(), or incorporating the display updates from the loop() into your main().

However, please make particular note of the Interrupt Service Routine (ISR), as this is what you require: ISR(TIMER2_COMPA_vect) {disp.interruptAction();}. This will ensure that you have a flicker free display, while the processor is performing processor intensive tasks, when controlling your bot.

Body:
While this question may seem to be better suited for SE Arduino, I shall, nevertheless, propose a solution. Employ a ready written Arduino library, as this would save a lot of groundwork. Granted, as you mention that you have a main(), then one can assume that you are not using the Arduino IDE. However, the library (with the .cpp file and .h file) can still be used.

Interestingly, I went through the pain of coding up a seven segment display last year, on the Arduino, albeit only a two digit display, an SL-1255 (see Dual Seven Segment display madness). I even came up with an all singing and dancing Arduino library, SevenSegmentSL1255.

However, I do not recommend you using it, as it does not use interrupts and, much to my chagrin, I found another, more fully featured, seven segment display Arduino library, SevenSeg, after having completed mine. The SevenSeg library will work with four digit displays and it also handles interrupt driven displays.

From section 4.4 Using Interrupt Timers of the PDF,

Running the printing functions in an endless loop to perform multiplexing is not always an ideal way to do things. Outputting information to a 7-segment display is not a computationally intensive task, but due to the delay used for multiplexing, the microcontroller just sits there and waits for most of the time. Sure, you can insert commands taking little time in a loop together with the printing functions, but if they are slightly time-consuming, the display will halt or flicker. For this purpose it is possible to use SevenSegalong with interrupt timers. Then, you can do whatever you want inside loop(){...}, and simply run a high-level printing function only when you want to change what's on the display. The microcontroller will automatically be interrupted just briefly to update the display as needed. You can still change the refresh rate and the duty cycle like normal, the SevenSeg-library will take care of configuring the timers for you.

The library code can be downloaded from here.

An example sketch (again taken from section 4.4 of the PDF) is as follows:

#include <SevenSeg.h>
SevenSeg disp(11,7,3,5,6,10,2);
const int numOfDigits =4;
int digitPins[numOfDigits]={12,9,8,13};

void setup () {
  disp.setDigitPins(numOfDigits ,digitPins);
  disp.setTimer(2); disp.startTimer();
}

void loop() {
  for(int i=1;i<=10;i++){
    disp.write(i);
    delay(1000); // Or other time-consuming tasks
  }
}

ISR(TIMER2_COMPA_vect) {
  disp.interruptAction();
}

Obviously, you will need to modify the sketch for your particular purposes, notably either calling setup() and loop() from your main(), or incorporating the display updates from the loop() into your main().

However, please make particular note of the Interrupt Service Routine (ISR), as this is what you require: ISR(TIMER2_COMPA_vect) {disp.interruptAction();}. This will ensure that you have a flicker free display, while the processor is performing processor intensive tasks, when controlling your bot.


问:
Wearable Accelerometor
I've worked with wiimote accelerometer, but I think now I want to move past that. Mostly because I want to have a wider range of available gestures and I think that using only one accelerometer has too many limitations for what I want to do. I'm looking for something compatible with arduino or RPi. Does anyone have recommendations on how I should do this?</p>

答:
If I understood your question correctly, what you need to capture more gestures are just more accelerometers positioned at different locations on your device.

If you are talking complex finger gestures, then maybe you need something like a machine vision system to recognize all the complex hand and finger motions. Hope this helps a little.

Title: Wearable Accelerometor


答:
This is how I do it:

```<code># return True if in collision, False if no collision
# pass in joint angles q to test configuration other than current
def inCollision(q=None):
    with env:
        if q == None:
            return env.CheckCollision(robot) or robot.CheckSelfCollision()
        else:
            initq = robot.GetDOFValues()
            robot.SetDOFValues(q)
            ret = env.CheckCollision(robot) or robot.CheckSelfCollision()
            robot.SetDOFValues(initq)
            return ret
</code>```

NOTE: OpenRave uses ODE as its collision checker by default.  And for some reason, the default build of ODE for Ubuntu does not enable cylinder-cylinder collision checking.  You can use Bullet instead if you like, but I have found it to be slower than ODE.  Use these lines to change which collision checker OpenRave uses:

```<code>collisionChecker = RaveCreateCollisionChecker(env, 'bullet')
env.SetCollisionChecker(collisionChecker)
</code>```

Or you can just download the ODE source and recompile.  These are the steps I follow:

Download the latest ODE (https://bitbucket.org/odedevs/ode/downloads/ode-0.13.1.tar.gz)

./configure --enable-double-precision --enable-libccd --disable-demos --disable-asserts --enable-shared

make -j8

sudo make install

Title: Check collision between robot and environment in OpenRAVE

Body: I have a robot arm in an environment. How can I check for collision between this robot arm and the environment?</p>

Body:
This is how I do it:

# return True if in collision, False if no collision
# pass in joint angles q to test configuration other than current
def inCollision(q=None):
    with env:
        if q == None:
            return env.CheckCollision(robot) or robot.CheckSelfCollision()
        else:
            initq = robot.GetDOFValues()
            robot.SetDOFValues(q)
            ret = env.CheckCollision(robot) or robot.CheckSelfCollision()
            robot.SetDOFValues(initq)
            return ret

NOTE: OpenRave uses ODE as its collision checker by default.  And for some reason, the default build of ODE for Ubuntu does not enable cylinder-cylinder collision checking.  You can use Bullet instead if you like, but I have found it to be slower than ODE.  Use these lines to change which collision checker OpenRave uses:

collisionChecker = RaveCreateCollisionChecker(env, 'bullet')
env.SetCollisionChecker(collisionChecker)

Or you can just download the ODE source and recompile.  These are the steps I follow:

Download the latest ODE (https://bitbucket.org/odedevs/ode/downloads/ode-0.13.1.tar.gz)

./configure --enable-double-precision --enable-libccd --disable-demos --disable-asserts --enable-shared

make -j8

sudo make install


问:
Robot autonomous variable terrain with yaw sensor
I am programming a robot to drive over variable terrain obstacles autonomously. The variable terrain could potentially knock the robot off of its initial heading, but I would like to design an autonomous sequence to correct for any change in direction. I am using a very accurate sensor with compass and yaw. What is the best way to have it correct for any changes and maintain its heading? Side to side motion does not have to stay perfect, but the heading needs to stay the same.We are currently correcting it by overpowering one side of the wheels (depending on direction of correction needed) until the heading is correct again, but this seems to be a slightly antiquated method, so I'm looking for a cleaner and more smooth method.</p>

答:
If you have the path you want the robot to stay on, it sounds like you need Pure Pursuit.  In this simple algorithm, you steer the robot to some look-ahead point on the path.  The gist of it is illustrated by this image.

This only works for non-holonomic robots, but even if you have a holonomic robot, you can do something similar.

Original algorithm proposed here:
Implementation of the Pure Pursuit Path Tracking Algorithm. R. Craig Coulter, CMU, 1992.