问:
Cascaded PID controller implementation
For a project I am building and programming my own flight controller. I have seen the implementation of a single loop PID controller. But after doing some more research I've found that a cascading PID controller could be used for improved performance.

For so far I was able to create the single loop PID for the angle stabilization. This PID controller input uses RC values with Euler angles calculated IMU as feedback controller. But when I was thinking about the implementation of the rate stabilization controller, some information I couldn't work out.

The inner rate stabilization loop should be faster than the outer angle stabilization loop. This way the rate stabilization has a higher refresh rate that should keep the quadcopter more stable.

If I am using a fixed loop frequency of 400 Hz, and both PID controller are in the same while(1)  loop, there is no speed difference between the inner and outer loop?

So if this is correct it means to use a cascaded loops both loops should be programmed on an other core (multi core programming)? Or is there a way that cascaded loops should work within the same while(1) loop?

答:
Instead of using multi-core programming, you could employ multi-threading on the one hand, where basically you assign the two loops to two different concurrent tasks. The scheduling of the tasks is generally managed by some services of the underlying operating system.

Because of concurrency, you ought to pay attention to the exchange of information between the two tasks that must happen in a thread-safe way, by using for example a mutex.

On the other hand, you could simply partition your while(1) main loop such that if it runs at rate R, you may foresee a subloop as in the following metacode:

int cnt = 0;
while (1) {
  // inner-loop operations at rate R

  if (cnt++ % 10 == 0) {
    // outer-loop operations at rate R/10
  }
}
```

Bear also in mind that you could easily let the outer-loop run at the same rate as the inner-loop, hence letting the while(1) run at the fastest rate that is shared between the loops without any ad-hoc subsampling.

Generally, this approach does not harm, aside from the unnecessarily waste of resources. However, be careful of some usually disregarded pitfalls of increasing control rate.

Title: Cascaded PID controller implementation

Body:
Instead of using multi-core programming, you could employ multi-threading on the one hand, where basically you assign the two loops to two different concurrent tasks. The scheduling of the tasks is generally managed by some services of the underlying operating system.

Because of concurrency, you ought to pay attention to the exchange of information between the two tasks that must happen in a thread-safe way, by using for example a mutex.

On the other hand, you could simply partition your while(1) main loop such that if it runs at rate R, you may foresee a subloop as in the following metacode:

int cnt = 0;
while (1) {
  // inner-loop operations at rate R

  if (cnt++ % 10 == 0) {
    // outer-loop operations at rate R/10
  }
}

Bear also in mind that you could easily let the outer-loop run at the same rate as the inner-loop, hence letting the while(1) run at the fastest rate that is shared between the loops without any ad-hoc subsampling.

Generally, this approach does not harm, aside from the unnecessarily waste of resources. However, be careful of some usually disregarded pitfalls of increasing control rate.


问:
Derivation of EKF slam landmark initialization
I have read a couple posts which gives the formula for initializing a new landmark in EKF slam but the derivation is not given.

EKF-SLAM initialize new landmark in covariance matrix

Could someone sketch how it is done?

答:
You simply expand the size of your state vector and covariance matrix.

Lets say our current state is simply a 2D robot pose.

$$s = \begin{bmatrix}
x \\
y \\
\theta
\end{bmatrix}$$

which will have a covariance matrix of

$$cov = \begin{bmatrix}
\sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} \\
\sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} \\
\sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta}
\end{bmatrix}$$

Now we want to add our new landmark($l_x,l_y$) to our state vector so we simply append it to our current one. The first values of $l_x,l_y$ would come from some sort of initialization routine.

$$s* = \begin{bmatrix}
x \\
y \\
\theta \\
l_x \\
l_y
\end{bmatrix}$$

And we augment our covariance matrix also to a new size.

$$cov* = \begin{bmatrix}
\sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} & \sigma_{x,l_x} & \sigma_{x,l_y} \\
\sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} & \sigma_{y,l_x} & \sigma_{y,l_y} \\
\sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} & \sigma_{\theta,l_x} & \sigma_{\theta,l_y} \\
\sigma_{l_x, x} & \sigma_{l_x, y} & \sigma_{l_x, \theta} & \sigma_{l_x,l_x} & \sigma_{l_x,l_y} \\
\sigma_{l_y, x} & \sigma_{l_y, y} & \sigma_{l_y, \theta} & \sigma_{l_y,l_x} & \sigma_{l_y,l_y}
\end{bmatrix}$$

One can notice that the 3x3 top left block is the same as our previous covariance, so those values are just copied over. So we just need to determine the cross covariances( values like $\sigma_{x,l_x}$) and the variance ($\sigma_{l_x,l_x}$) of the landmark.

The cross covariances are simply set to 0. Since we don't know how the landmark currently correlates with the rest of the state. You don't have to worry about them because as the Kalman filter continues to run they will update automatically.

The landmark variances ($\sigma_{l_x,l_x}$ and $\sigma_{l_y,l_y}$) is dependent on your application. Things like sensor type, sensor quality, how you are computing landmark pose all play a role. Generally people will just manually guess a desired value. So something like we generally believe our landmark initialization is about 10cm accurate so $\sigma_{l_x,l_x}=\sigma_{l_y,l_y}=0.1$. You can increase it our decrease it depending on your confidence in the landmark initialization process. You can also tune it by running your filter on the same dataset multiple times, and seeing which values perform the best.

So your new covariance matrix on the first initialization would look something like this(assuming we use the 10cm of error for the landmark init).

$$cov* = \begin{bmatrix}
\sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} & 0 & 0 \\
\sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} & 0 & 0 \\
\sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} & 0 & 0 \\
0 & 0 & 0 & 0.1 & 0 \\
0 & 0 & 0 & 0 & 0.1
\end{bmatrix}$$

If we were to add even more landmarks to the state then we would do the same thing. Increase the size of the state vector. Copy over your old covariance matrix to the new one. Set the cross correlations of your new landmark to 0, and set some initial uncertainty values. This can be visualized in block matrix form as seen below.

$$modified\:cov = \begin{bmatrix}
old\:cov & 0  \\
0 & landmark\:cov
\end{bmatrix}$$

$old\:cov$ is our old covariance matrix copied over. The 0s are all the cross correlation terms, and the $landmark\:cov$ is just the covariance of the single new landmark.

$$landmark\:cov = \begin{bmatrix}
\sigma_{l_x,l_x} & 0  \\
0 & \sigma_{l_y,l_y}
\end{bmatrix}$$

You can see this in action in this EKF slam in this project.

ContentLicense: CC BY-SA 4.0


问:
Force sensors output when inclined
So here is the context:

I am working on a motorcycle simulator. It consists of a motorcycle frame mounted on different actuators (see picture on the left).

In particular, the two actuators at the front serve the purpose of rendering the motorcycle roll angle. There is a force sensor on both sides between the motorcycle frame and the actuator (see picture on the right). The force sensors are tensile/compressive force transducers. These are are 1-axis only force sensors.

Imagine a rider on the simulator.

So here is the behavior that I observe in terms of sensor output :

at rest, i.e. when the roll angle is $0°$, both sensor outputs are equal (let's call it $V_0$).

when the roll angle is $>0°$ (the motorcycle and the rider are inclined towards the left), the output of the sensor on the left is $V_l > V_0$ while the output of the sensor on the right is $V_r < V_0$.

when the roll angle is $<0°$ (the motorcycle and the rider are inclined towards the right), the output of the sensor on the left is $V_l < V_0$ while the output of the sensor on the right is $V_r > V_0$.

I would like to model the expected output of both of the sensors as a function of the roll angle. However, I have thus far been unsuccessful.

My hypothesis is that at rest the sensors both measure half of the weight of the frame + the rider and when inclined they measure some kind of function of this weight and of the roll angle. But when I draw the free body diagram I always find an equal force applied on both sides, which is not coherent with the actual results.

Edit: This is not a homework question, but let me show my work anyway.

At rest, the forces involved are:

$W$, the weight of the frame + the rider (which is along the $y$-axis);

$R_{l_x}$ and $R_{l_y}$ the reaction of the left support (resp. along $x$ and $y$-axis);

$R_{r_x}$ and $R_{r_y}$ the reaction of the right support (resp. along $x$ and $y$-axis).

So the equilibrium equations are:

$R_{l_x} - R_{r_x} = 0$

$R_{l_y} - W + R_{r_y} = 0$

$-W \times L/2 + R_{r_y} \times L = 0$ (where $L$ is the distance between the two sensors)

Because the sensors are only sensible to traction/compression along their y-axis, I am not interested in equation (1). Equations (2) and (3) result in $R_{r_y} = W/2$ and $R_{l_y} = R_{r_y} = W/2$. According to this, it makes sense that at rest the sensors output the same voltage $V_0$ (which corresponds to them being subjected to a compressive force of magnitude $W/2$).

What I am having trouble doing is using the same logic in the inclined case.

Could somebody please help me figure this out?

答:
The thing you're missing from your equations of motions are the torques. Typically in these kinds of problems we assume the actuator ends are "pinned" connections so you can't transmit a torque through that mounting point. This lets you assume that the actuator force must cancel out any moments generated in your system.

Consider the following graphic:

The center of mass is the red circle. The left case has the equal torques about A and B. Remember that the equation for torque is $r \times F$. The vector $r$ is the vector from the pivot to the center of mass, and $F$ is the force vector, which is $mg$ and points down.

The equations you're missing are the ones that reconcile the applied and reactionary torques. If you consider just the torque about the pinned joint A, and your test platform is stationary, then the sum of the applied torque due to the center of mass and the reactionary torque applied at B must be zero:

$$
\left(r_{A\rightarrow COM} \times mg \right) + \left( r_{A\rightarrow B} \times F_{B_{reaction}} \right) = 0 \\
$$

Then you have to consider the torque about B and do it all over again. In the left scenario you wind up with the same counter torques and you get the load sharing - each actuator supports half the weight.

In the right scenario, in my graphic, the center of mass is nearly over B. In this case you'll get a very large torque about A that B's reaction needs to support. Because the center of mass is almost directly over B, there's not very much torque about B, and so A has virtually no reactionary force.

If you were to extend this more, if the center of mass is directly over B then there is NO torque about B and A doesn't support any weight at all. If you had the following scenario:

If the distance from A to the center of mass is twice the distance from A to B then you wind up with B in compression at double the weight and A's reaction is in tension equal to the weight - this becomes a lever and fulcrum problem.

Title: Force sensors output when inclined


问:
Is there regulation of LiDAR in the public space?
Are there any regulations (in the US or the EU), plans to regulate or initiatives that think about the "pollution" with or amounts of laser emission in urban spaces in general and the impact it may have on health?

Given the prevalence of LiDAR in robotics and autonomous vehicles - for navigation, mapping or collision detection - I've come to worry about the safety aspect of such vast amounts of laser light that is emitted into the public space.

I know that LiDAR scanners are usually "eye safe" and infrared, but the human eye isn't the only optical sensor on earth and to other non-human participants in traffic, an array of laser sources on an autonomous vehicle (AV) or other robotic or automatic objects might "look" like a "radiation porcupine". That might not be a problem today, with relatively few AVs, but might become one in the future.

That said, I've learned in a recent discussion that LiDAR, at least with autonomous vehicles, begins to fall in popularity as the technology of choice and will probably be fully replaced in driving support systems and future production model AVs. The reason: LiDAR introduces all sorts of problems with maintainability - moving parts, transparent windows, dirt, etc. - while radar reduces mechanical complexity and gives wider design choices. The impact on health of prevalent radar is another story...

答:
The radiation produced by LiDARs is just light in a specific frequency. One way to think of it is a very focused very small flashlight which only puts out one color, that we can't quite see. That color is also known as infrared, which is also known as heat.

The amount of light energy emitted by LIDAR sensors is quite small and there is not a significant worry about the accumulation. Picking a random outdoor rated sensor the SICK outdoorScan3 I see in the datasheet that is produces only 9.2mW on average.

LED headlights, the most efficient and comparable use in the range of 10-20W of power. (Xenon reference, paper on motorcycle headlights efficiency) So if you're accumulating 1000 LIDAR sensors on a car, the output would be the same as having 1 headlight on the car. Most cars have 2 headlights as well as a whole bunch more lights which will produce much more light radiation than the sensors.

Also for reference sunlight hitting the ground delivers over 1kW of light energy per square meter. Which would take over 100000 LIDARS pointing at the same square meter of space to produce the same effect. However most LIDAR sensors point outward and scan all around which means that the power is distributed around. Which means that at say 5m distance, only 3% go into a specific meter radius. And as such you're now looking at needing on the order of 3 million sensors around you at 5 meters away. Back calculating that to a 5 meter sphere you would need over 10000 sensors per square meter of that sphere, which is by multiple orders of magnitude higher than you could physically achieve.

And that's only to produce the level of light that the sun provides, which is a level of light that I am quite happy to walk around in.

As such I don't worry about any health effects of the many LIDARs that might be used in public spaces.


问:
How to use a PID controller for different kinematics robots
I implement different drive odometry for a robot. Trying to implement (proportional–integral–derivative controller) PID-based coordinate movements x, y, theta:

```<code>public void getX(double dt) {x += Math.cos(theta) * robotV() * dt;}
public void getY(double dt) {y += Math.sin(theta) * robotV() * dt;}
public void getTheta(double dt) {
    theta += ((rightLenV()-leftLenV()) / Constants.transmission_width)*dt;
    theta = adduction(theta);
}
public double adduction(double a){
    while (a > Math.PI) {a = a-2*Math.PI;}
    while (a < -Math.PI) {a = a+2*Math.PI;}
    return a;
}
</code>```

my implement pid controller (bad working)

```<code>pidControllerVelocity = new PIDController(10, 0, 5);
public void velocity(double x_W, double y_W) {
    x_D = x_W - x;
    y_D = y_W - y;
    dS = Math.sqrt((Math.pow(x_D, 2) + Math.pow(y_D, 2)));

    u = Math.atan2(y_D, x_D);

    pidControllerVelocity.setSetpoint(adduction(u));
    double PD_velocity = pidControllerVelocity.calculate(theta);

    if (dS < 0.015) {
        leftMotor.set(0);
        rightMotor.set(0);
    } else {
        leftMotor.set(0.1+PD_velocity);
        rightMotor.set(0.1-PD_velocity);

    }
}
</code>```

I'm working with Java library wpilib.

答:
The kinematics - odometry part seems okay. I think your general configuration for the controller is incorrect.

First of all, you are going to need to use two high-level controllers. One of them is going to provide the reference forward speed of your robot, V. The other one is going to provide the reference steering of the robot. Then you can map these desired values to the wheel speeds, in order to actuate your robot.

You have already calculated dS. You feed this dS to a controller (a sole P Controller may work just OK, you may not need the other terms). The output of the controller is going to be the desired speed - V_des. You also have already calculated angle "u", which is the relative angle of the vehicle to the goal point. You subtract the vehicle "theta" from this angle "u" to have the orientation error. Then, you can feed this error to a separate controller - a sole P controller may be enough for this too. The output of this controller is going to be the desired rate of orientation - Theta_dot_desired.

From your odometry function, we already know that the Theta_dot is equal to the  speed difference between the right and left wheel, divided by the wheel base. It is not written in your odometry code directly, but I believe that the robotV variable must be equal to the sum of the wheel speeds, divided by two. From these two equations, you can solve for the wheel speeds, given the desired robotV and Theta_dot.

You can use the following equations I have written as a reference:

General Kinematics, given the Wheel Speeds:

Controller Equations:

Substituting the Controller Outputs to the Kinematic Equations, in order to calculate the Inverse Kinematics:

I am not very sure about how to use your library, but I believe that you can work it out if you understand the controller structure.


问:
Raspberry Pi 4 to PCA9685 Steering Working but not Throttle
I am using a raspberry pi 4 connecting to a PCA9685 and following the instruction on the donkeycar doc site (the link can be found below). I am at the step "fine tuning calibration." When I run "python manage.py drive" the steering works using my keyboard controls but the throttle is not causing any movement with the keys. I know that the throttle is not broken because the wheels turn when I do the calibration of the throttle. How do I solve this?

https://docs.donkeycar.com/guide/calibrate/

答:
There was a known bug in one version of DonkeyCar that could cause this problem.  That bug has been fixed, so I would update to the latest tagged version.  Open a console in your donkeycar project folder, then

```<code>git checkout $(git describe --tags `git rev-list --tags --max-count=1`)
git pull
pip3 install -e .[pi]
</code>```

The fix involved changing how the drivetrain configuration was done, so you may need to update your mycar folder.  I tend to create a new one, then edit the myconfig.py file base on my older version.  The many of the drivetrain configurations changed from being a set of constants to a python dictionary, so you need to uncomment the appropriate dictionary.  So if your drivetrain in myconfig.py is

```<code>DRIVE_TRAIN_TYPE = "PWM_STEERING_THROTTLE"
</code>```

Then you want to uncomment the following lines in myconfig.py and update the values with your pin connections and calibration values.

```<code>PWM_STEERING_THROTTLE = {
"PWM_STEERING_PIN": "PCA9685.1:40.1",   # PWM output pin for steering servo
"PWM_STEERING_SCALE": 1.0,              # used to compensate for PWM frequency differents from 60hz; NOT for adjusting steering range
"PWM_STEERING_INVERTED": False,         # True if hardware requires an inverted PWM pulse
"PWM_THROTTLE_PIN": "PCA9685.1:40.0",   # PWM output pin for ESC
"PWM_THROTTLE_SCALE": 1.0,              # used to compensate for PWM frequence differences from 60hz; NOT for increasing/limiting speed
"PWM_THROTTLE_INVERTED": False,         # True if hardware requires an inverted PWM pulse
"STEERING_LEFT_PWM": 460,               #pwm value for full left steering
"STEERING_RIGHT_PWM": 290,              #pwm value for full right steering
"THROTTLE_FORWARD_PWM": 500,            #pwm value for max forward throttle
"THROTTLE_STOPPED_PWM": 370,            #pwm value for no movement
"THROTTLE_REVERSE_PWM": 220,            #pwm value for max reverse throttle
}
</code>```

Body:
There was a known bug in one version of DonkeyCar that could cause this problem.  That bug has been fixed, so I would update to the latest tagged version.  Open a console in your donkeycar project folder, then

git checkout $(git describe --tags `git rev-list --tags --max-count=1`)
git pull
pip3 install -e .[pi]

The fix involved changing how the drivetrain configuration was done, so you may need to update your mycar folder.  I tend to create a new one, then edit the myconfig.py file base on my older version.  The many of the drivetrain configurations changed from being a set of constants to a python dictionary, so you need to uncomment the appropriate dictionary.  So if your drivetrain in myconfig.py is

DRIVE_TRAIN_TYPE = "PWM_STEERING_THROTTLE"

Then you want to uncomment the following lines in myconfig.py and update the values with your pin connections and calibration values.

PWM_STEERING_THROTTLE = {
"PWM_STEERING_PIN": "PCA9685.1:40.1",   # PWM output pin for steering servo
"PWM_STEERING_SCALE": 1.0,              # used to compensate for PWM frequency differents from 60hz; NOT for adjusting steering range
"PWM_STEERING_INVERTED": False,         # True if hardware requires an inverted PWM pulse
"PWM_THROTTLE_PIN": "PCA9685.1:40.0",   # PWM output pin for ESC
"PWM_THROTTLE_SCALE": 1.0,              # used to compensate for PWM frequence differences from 60hz; NOT for increasing/limiting speed
"PWM_THROTTLE_INVERTED": False,         # True if hardware requires an inverted PWM pulse
"STEERING_LEFT_PWM": 460,               #pwm value for full left steering
"STEERING_RIGHT_PWM": 290,              #pwm value for full right steering
"THROTTLE_FORWARD_PWM": 500,            #pwm value for max forward throttle
"THROTTLE_STOPPED_PWM": 370,            #pwm value for no movement
"THROTTLE_REVERSE_PWM": 220,            #pwm value for max reverse throttle
}


答:
This is me trying again with the help of @Chuck's answer.

So in the general case, the torque equilibrium equation about A is $\vec{r}_{A \rightarrow COM} \times \vec{W} + \vec{r}_{A \rightarrow B} \times \vec{F}_{B_{reaction}} = \vec{0}$

Let's write the Cartesian components of each of these vectors:

$ \vec{r}_{A \rightarrow COM} = \begin{pmatrix}|\vec{r}_{A \rightarrow COM}| \cdot \sin(\theta_W) \\
|\vec{r}_{A \rightarrow COM}| \cdot \cos(\theta_W) \\
0\end{pmatrix}$  with $\theta_W$ is the angle between $\vec{r}_{A \rightarrow COM}$ and $\vec{W}$. Also, we have $\theta_W=\theta_{W_0} - \alpha$ where $\theta_{W_0}$ is the angle between $\vec{r}_{A \rightarrow COM}$ and $\vec{W}$ when the motorcycle is not inclined (left case in Chuck's graph) and $\alpha$ is the roll angle of the motorcycle (in the right case in Chuck's graph, $\alpha < 0°$). This will be useful later;

$\vec{W} = \begin{pmatrix}0 \\
-W \\
0\end{pmatrix}$;

$ \vec{r}_{A \rightarrow B} = \begin{pmatrix}L \cdot \cos(\alpha) \\
-L \cdot \sin(\alpha) \\
0\end{pmatrix}$;

$\vec{F}_{B_{reaction}} = \begin{pmatrix}F_{B_{reaction}} \sin(\alpha) \\
F_{B_{reaction}} \cos(\alpha) \\
0\end{pmatrix}$.

Now if we take the norm of the torque equilibrium equation and substitute everything, we have:
$|\vec{r}_{A \rightarrow COM}| \cdot \sin(\theta_{W_0} - \alpha) \cdot (-W) + L \cdot F_{B_{reaction}} \cdot (\cos^2(\alpha) + \sin^2(\alpha)) = 0$

$\theta_{W_0}$ is such that $\sin(\theta_{W_0}) = \frac{L/2}{|\vec{r}_{A \rightarrow COM}|}$ and $\cos(\theta_{W_0}) = \frac{h}{|\vec{r}_{A \rightarrow COM}|}$ where $h$ is the height of the center of mass. With a few more developments and if we do the same for the torque about B, this is the result:

$F_{B_{reaction}} = \frac{W \cdot [\frac{L}{2} \cdot \cos(\alpha) - h \cdot \sin(\alpha)]} {L}$

$F_{A_{reaction}} = \frac{W \cdot [\frac{L}{2} \cdot \cos(\alpha) + h \cdot \sin(\alpha)]} {L}$

Title: Force sensors output when inclined

Body:
So here is the context:

I am working on a motorcycle simulator. It consists of a motorcycle frame mounted on different actuators (see picture on the left).

In particular, the two actuators at the front serve the purpose of rendering the motorcycle roll angle. There is a force sensor on both sides between the motorcycle frame and the actuator (see picture on the right). The force sensors are tensile/compressive force transducers. These are are 1-axis only force sensors.

Imagine a rider on the simulator.

So here is the behavior that I observe in terms of sensor output :

at rest, i.e. when the roll angle is $0°$, both sensor outputs are equal (let's call it $V_0$).

when the roll angle is $>0°$ (the motorcycle and the rider are inclined towards the left), the output of the sensor on the left is $V_l > V_0$ while the output of the sensor on the right is $V_r < V_0$.

when the roll angle is $<0°$ (the motorcycle and the rider are inclined towards the right), the output of the sensor on the left is $V_l < V_0$ while the output of the sensor on the right is $V_r > V_0$.

I would like to model the expected output of both of the sensors as a function of the roll angle. However, I have thus far been unsuccessful.

My hypothesis is that at rest the sensors both measure half of the weight of the frame + the rider and when inclined they measure some kind of function of this weight and of the roll angle. But when I draw the free body diagram I always find an equal force applied on both sides, which is not coherent with the actual results.

Edit: This is not a homework question, but let me show my work anyway.

At rest, the forces involved are:

$W$, the weight of the frame + the rider (which is along the $y$-axis);

$R_{l_x}$ and $R_{l_y}$ the reaction of the left support (resp. along $x$ and $y$-axis);

$R_{r_x}$ and $R_{r_y}$ the reaction of the right support (resp. along $x$ and $y$-axis).

So the equilibrium equations are:

$R_{l_x} - R_{r_x} = 0$

$R_{l_y} - W + R_{r_y} = 0$

$-W \times L/2 + R_{r_y} \times L = 0$ (where $L$ is the distance between the two sensors)

Because the sensors are only sensible to traction/compression along their y-axis, I am not interested in equation (1). Equations (2) and (3) result in $R_{r_y} = W/2$ and $R_{l_y} = R_{r_y} = W/2$. According to this, it makes sense that at rest the sensors output the same voltage $V_0$ (which corresponds to them being subjected to a compressive force of magnitude $W/2$).

What I am having trouble doing is using the same logic in the inclined case.

Could somebody please help me figure this out?


问:
Bluetooth module HC-05 giving ERROR :(0)
I am working right now with Arduino UNO and HC-05 bluetooth module.I followed the instruction given on this link for wiring. So there are 2 mode of working with this HC-05 module

Simple serial communication

Working in AT command mode so as to change the parameters of HC-05 module

As long as I work in simple serial communication mode, everything works fine but when I tried to change the parameters of module, it didn't work out. For working in At command mode, PIN NO 34 of HC-05 module needs to be high which I had taken care of. Lately I found that in mu module they had knowingly not connected the Berg strip to PIN 34, so I connected the PIN directly, even though I am not able to change the parameters of module and when I write any command on COM port of arduino IDE, I get this response

```<code>Enter AT commands:
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
ERROR:(0)
ÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿÿõÿýì¢^
</code>```

I think that garbage is due to my code

Here is my code:

```<code>#include <SoftwareSerial.h>

SoftwareSerial BTSerial(10, 11); // RX | TX

void setup()
{
  pinMode(9, OUTPUT);
  digitalWrite(9, HIGH);
  Serial.begin(9600);
  Serial.println("Enter AT commands:");
  BTSerial.begin(38400);
}

void loop()
{
  uint8_t x;
  char CommandFromSerial[50]=" ";
  char ResponseFromBluetooth[50]= " ";

  if ((Serial.available())){
    if(Serial.available()>0){
      for(x=0;x<50;x++)
        CommandFromSerial[x]=Serial.read();
      BTSerial.println(CommandFromSerial);
    }
  }

  if ((BTSerial.available())){
    if(BTSerial.available()>0)
      for(x=0;x<50;x++)
        ResponseFromBluetooth[x]=BTSerial.read();
    Serial.println(ResponseFromBluetooth);
  }
}
</code>```

I am not able to figure out what I am doing wrong. I used this command on COM port
AT\r\n and many other commands but every time I get the same response.

Did I mess up with my bluetooth module unknowingly?

答:
I had the same problem, so please try this code but with IDE 1.0.0 and baud rate 38400 in serial monitor:</p>

```<code>void setup() {
  Serial1.begin(38400);
  Serial.begin(38400);
}

void loop() {
  if(Serial1.available())
  {
    byte a=Serial1.read();
    Serial.write(a);
  }
  if(Serial.available())
  {
    byte a=Serial.read();
    Serial1.write(a);
  }
}
</code>```

ContentLicense: CC BY-SA 3.0

Body:
I had the same problem, so please try this code but with IDE 1.0.0 and baud rate 38400 in serial monitor:

void setup() {
  Serial1.begin(38400);
  Serial.begin(38400);
}

void loop() {
  if(Serial1.available())
  {
    byte a=Serial1.read();
    Serial.write(a);
  }
  if(Serial.available())
  {
    byte a=Serial.read();
    Serial1.write(a);
  }
}


问:
What's the difference between a sonar and ultrasonic sensor?
After having read the introductions of the Wikipedia articles about "sonar" and "ultrasonic sensor", it is still not clear to me what the differences between the two are. In the context of robotics, is there any difference between these two sensors (terms)?</p>

答:
In terms of functionality, an ultrasonic sensor performs a single distance measurement in a given direction (strictly speaking, such a sensor emits a sound cone, the angle of which depends on how the sensor is implemented).

A SONAR, on the other hand, seems to generally refer to a device that performs aggregated range measurements, i.e., a 360 degree scan combining many single distance measurements.

Similarly, a LIDAR combines many single laser distance measurements.


问:
irobot create 2: random changes in driving speed
I am trying to make the Create2 dance a specific choreography. I´m not using the sensors, I just let it drive and turn for a specific amount of time. In general it works fine.

But after working with it for a while, it suddenly doesn't do a full turn any more. It seems like something changes and than driving in a specific angle and turning in place takes a bit longer or shorter. It also happens, that Roomba doesn't drive straight any more but slightly turns to the right. And than it changes again and it´s back to the first configuration.

I tried changing the baudrate, I did manual resets, I charged Roomba and tried in full battery power and low battery power but I can´t find an explanation. The bug occurs without changing code or anything.
Does anyone have an idea, what the problem might be?

Another problem that I ran into: I let Create2 play songs. Most of the time it works. But from time to time I get some weird issues. The tune doesn't play until the end but lingers on one note for a moment. Than the tune stops. Often the next song won´t play at all. Next time I run the code it works just fine.
Any ideas?

The whole thing makes me go crazy because it messes up the whole choreography and makes it totally unreliable...

My set up:
Arduino Nano connected to serial port of Create2.
Arduino-Create2 Libary https://github.com/brinnLabs/Create2

```<code>// Include Create2 libary
#include <iRobotCreate2.h>

// connect Roomba TX to pin 10 and Roomba RX to pin 11,
// connect Roomba BaudRateChange to pin 5
iRobotCreate2 roomba(true, 10,11,5);

// set pins for LED and Startbutton:
const int ledPin =  13;
const int buttonPin = 4;
int incomingByte = 0;

// variable for reading the Startbutton status
int buttonState = 0;

// full rotation of roomba with VEL 100
int fullspin100 = 7319;
int fullspin200 = 3659;
int fullspin300 = 2439;

void setup(){

  Serial.begin(115200);

  // set pinmode for LED and Startbutton
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);

  // reset BaudRate to 19200
  roomba.resetBaudRate();
  delay(2000);

  // start communication and safe mode
  roomba.start();
  roomba.safeMode();
  delay(1000);

  // song nr. 1
  Notes songNotes[16];
  songNotes[0].note = 31;
  songNotes[1].note = 43;
  songNotes[2].note = 31;
  songNotes[3].note = 43;
  songNotes[4].note = 55;
  songNotes[5].note = 67;
  songNotes[6].note = 79;
  songNotes[7].note = 91;
  songNotes[8].note = 103;
  songNotes[9].note = 91;
  songNotes[10].note = 79;
  songNotes[11].note = 67;
  songNotes[12].note = 55;
  songNotes[13].note = 43;
  songNotes[14].note = 31;
  songNotes[15].note = 103;

  songNotes[0].duration = 8;
  songNotes[1].duration = 8;
  songNotes[2].duration = 8;
  songNotes[3].duration = 8;
  songNotes[4].duration = 8;
  songNotes[5].duration = 8;
  songNotes[6].duration = 8;
  songNotes[7].duration = 8;
  songNotes[8].duration = 8;
  songNotes[9].duration = 8;
  songNotes[10].duration = 8;
  songNotes[11].duration = 8;
  songNotes[12].duration = 8;
  songNotes[13].duration = 8;
  songNotes[14].duration = 8;
  songNotes[15].duration = 8;

  roomba.createSong(1, 16, songNotes);

  // turn on PowerLed red to indicate that roomba is ready for dancing
  roomba.setPowerLEDs(255, 255);
  delay(10);
}

void loop(){
  // listen to input from the button
  buttonState = digitalRead(buttonPin);

  // if button is pressed, start dancing
  if (buttonState == LOW) {
    digitalWrite(ledPin, LOW);
    dance();

  }
  if (buttonState == HIGH) {
   digitalWrite(ledPin, HIGH);
  }
}

void dance(){

  roomba.turnCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.playSong(1);
  delay(1300);

}
</code>```

答:
A little update. Using the roombas in the performance for over one year made me realise, that the roomba is just missing commands from time to time. It just happens randomly. Sometimes everything is fine for four runs and in the fith it is leaving out a "turn  command". I figured that it is more reliable with a baud rate of 115000 instead of 19200 wich I used in the beginning.
But still the skipping of commands happens sometimes.</p>


问:
Best way to Inculde System Characterstics (e.g. Laplace Equ.) in ODE for building Control Strategy
Let's consider the following ordinary differential equation (ODE):
$$
\begin{align*}
\frac{\partial x}{\partial t} &= f(x,u)\\
0 &= g(x,u)\\
y &= h(x,u)
\end{align*}.
$$
We denote $x\in \mathbb{R}^{n_x}$ as the states, $u \in \mathbb{R}^{n_u}$ as the inputs.

For the sake of simplicity, let's have a look at the ODE of a simple model of a planar robot:

$$
\begin{align*}
\frac{\partial x}{\partial t} &= v_{veh} \cdot \cos(\theta)\\
\frac{\partial y}{\partial t} &= v_{veh} \cdot \sin(\theta)\\
\frac{\partial \theta}{\partial t} &= \omega
\end{align*}.
$$

We introduce the states
$$x = [x,y,\theta]^{T}$$

and the inputs
$$
\begin{align*}
u = [v_{veh},\omega]^{T}
\end{align*}.
$$

From an engineering point of view, $ x $ and $ y $ are the coordinates of the robot in a x-y-Plane, $ \theta $ is its rotation. The input $ v_{veh} $ is the velocty in the driving direction and $ \omega $ is the heading rate.

Further, here is the relation of the robot velocity $ v_{veh} $ and the set point velocity $ v_{set} $, in terms of a 1st-order differential equation (Notation from Wikipedia):
$$
\begin{align*}
T \cdot \dot{v}_{veh}(t) + v_{veh}(t) = K\cdot v_{set}(t)
\end{align*}.
$$

My doubt now is how to incorporate this equation, which describes the system dynamics, into my original ODE.
Technically speaking, my goal would be to incorporate the time delay of the speed (that is, that the vehicle does not accelerate infinitely fast) into the ODE. This would allow me to build my control strategy accordingly.

答:
If you have an equation like:

$$
T \cdot \dot{v}_{veh}(t) + v_{veh}(t) = K\cdot v_{set}(t)
$$

then solve for the highest-order derivative:

$$
T \cdot \dot{v}_{veh}(t) =  - v_{veh}(t)  + K\cdot v_{set}(t) \\
\dot{v}_{veh}(t) =  - T^{-1} \cdot v_{veh}(t)  + T^{-1}K\cdot v_{set}(t) \\
$$

This would be the basis for the state-space form. You wind up with $\dot{v}_{veh}(t)$ as your $\dot{x}$, $- T^{-1}$as your $A$, $v_{veh}(t)$ is your $x$, $T^{-1}K$ is your $B$, and $v_{set}(t)$ is your $u$.


问:
How to deal with inexact pointcloud matching?
When running ICP(for example with SLAM) it's possible your next position does not have a point cloud which matches up with the prior point cloud due to resolution limits. How is this dealt with it? You could artificially add points to the prior point cloud(ex, linear interpolation though I'm not sure how that works in 3d).</p>

答:
You probably mean ICP, not IPC.

The most typical way is using plane features. Plane information could be extracted by voxelisation and NDT.

Plane-to-point or plane-to-plane is enough for the registration of sparse points. You don't need to do the interpolation at all.


问:
How to get transform between two fixed frames using a camera
In my task, I have two manipulators: one 6 dof robot (Manipulator 2 as shown in attached link picture), and one passive arm (Manipulator 1 as shown in attached link picture). Both the frames 1 and 2 are fixed. I would like to transfer the movement that I have with respect to the frame 1 to the frame 2, for this I need to compute the transform between the two fixed frames. To this end I would like to use a camera in a third frame and fiducials on the arms for defining the frames, how can I get the frames axes using the camera?

Thank you in advance for any helpful insights!

答:
The most simple and easy way is using a common third frame.

Attach a camera to the end effector of each robot and place a calibration board on the ground.

Do a hand to eye calibration using the calibration board

Find the base frame transformation matrix from the calibration board

Now you can calculate Frame 1 to Frame 2 ($^{F1}T_{F2}$) as follows

$^{F1}T_{F2} = ^{F1}T_{F1End} * ^{F1End}T_{F1Cam} * ^{F1Cam}T_{Calib} * (^{F2}T_{F2End} * ^{F2End}T_{F2Cam} * ^{F2Cam}T_{Calib} )^{-1} $

There could be multiple variants of this method.


问:
Body angle of a centre-articulated robot
Wondering if anyone can help me out with this one, or point me in the right direction. I am working on a centre-articulated robot, which is steered via a push-pull chain drive. The chain drive is driven via a servo motor (which is run through a high reduction gearbox to provide the required torque). I currently have an absolute encoder on the output stage of the gearbox so we know the current angle of the sprocket driving the chain. What I am trying to calculate is the resulting body angle given the current sprocket angle, see below a layout of this and the current known variables.

As in the image above, the required variable is the body angle, the known variables are the link length (from the centre articulation point to the chain attachment point), the chain length, sprocket size, and sprocket angle. Note that the sprocket will not rotate more than 45 degrees in either direction before the two bodies are at their steering limits.

In conclusion, I am chasing some general formula which relates the current sprocket angle to the current body angle given these known variables. Any help is appreciated!

答:
Let $P$ be the location of the body hinge (where the body angle $\gamma$ is measured)

Let $S$ be the center of the sprocket.

Let $L$ be the distance between $P$ and $S$.

Let $R_s$ be the sprocket radius. As the sprocket rotates through an angle $\theta$ radians, it moves a length of chain $R_s \theta$ from one of $c_1$ or $c_2$ to the other. The two segments $c_1$ and $c_2$ always sum to $c$, the length of the chain.

Let us assume that $R_s \ll L$. This simplification lets us model the chain segments as triangle sides meeting at point $S$. Without the simplification, the geometry is rather more complicated.

Now you have two triangles with sides $c_1$, $L$, $b$ and $c_2$, $L$, $b$. From the Law of cosines you can determine the triangles' internal angles at $P$. From those you can determine $\gamma$.

Combining all the parts, you have your formula for how $\gamma$ changes with $\theta$.


问:
how the two equatoin are linearized
corresponding paper

Question1: how the equatoin 3 is linearized in the neighborhood of the equilibrium point $x = x_{sp}, \dot x =0 $

Question2: how the equatoin 12 is linearized in the neighborhood of the equilibrium point $y = y_r$ ? what operations on $y \ \dot y \ {\dot y}^2 \ \ddot y$  and the constant ?

let $f_1 = \dot y,$  and
$f_2 = \ddot y= -(\lambda/2) (\dot y + 2 r_0) \dot y - U \lambda y - 2 U - (\lambda r_0^2) /2$

let $ d[y,\dot y]/dt=[f_1,f_2]=0 $ then  get the equilibrium point
$$ \dot y_r =0,y_r=-2/\lambda-r_0^2/(2U) $$
then I linearized $f_2$ around $[y_r,\dot y_r]$

get the two partial derivatives

$\frac{\partial f_2}{\partial \dot{y}} = -\lambda \dot y-\lambda r_0$

$\frac{\partial f_2}{\partial y} = -U \lambda$

get the value of the two partial derivatives at  $[y_r,\dot y_r]$

$\frac{\partial f_2}{\partial \dot{y}}|_{y_r,\dot y_r} = -\lambda r_0$

$\frac{\partial f_2}{\partial y}|_{y_r,\dot y_r} =-U \lambda $

obviously $f_2(y_r,\dot y_r)=0$

linearized differential equation in the neighborhood  becomes:

$\ddot y = f_2 \approx 0  +(- \lambda r_0)(\dot y- \dot y_r) +(-U \lambda) (y-y_r) = - \lambda r_0 \dot y- U \lambda y + U \lambda y_r$

final result is

$\ddot y + \lambda r_0 y + U \lambda y  = U \lambda y_r \ne 0$

not the same as in the paper.

答:
Consider the formula for the linearization of a function $f(\mathbf{x})$ about a point $\mathbf{x}_{e}$, where $\mathbf{x}, \mathbf{x}_{e} \in \mathbb{R}^{n}$:

$$f(\mathbf{x}) \approx f(\mathbf{x}_{e}) + \nabla f|_{\mathbf{x}_{e}} \cdot (\mathbf{x}-\mathbf{x}_{e})$$

I will only write out the math for problem 1, but problem 2 is hardly any different. So, we have:

$$\ddot{x} = f(x, \dot{x})$$
$$\ddot{x} = -\lambda \frac{|\dot{x}|\dot{x}}{2} - U \lambda (x-x_{sp})$$
Thus, when considering the linearization about the point $\mathbf{x}_{e} = [x_e \ \dot{x_e}]^T = [x_{sp}\  0]^T$ , we have:
$$f(\mathbf{x}_e) = 0$$
$$\nabla f|_{\mathbf{x}_e} \cdot (\mathbf{x} - \mathbf{x}_e) = \frac{\partial f}{\partial x}|_{x_{sp}} \cdot (x-x_{sp}) + \frac{\partial f}{\partial \dot{x}}|_{0} \cdot (\dot{x} - 0) $$
We can see the first term $\frac{\partial f}{\partial x}|_{x_{sp}} \cdot (x-x_{sp})$ is simply $-U\lambda (x-x_{sp})$. As the second term is the point of interest in this question, we will work it out in a more verbose manner:
$$\frac{\partial f}{\partial \dot{x}} = -\lambda \frac{2sgn(\dot{x})\dot{x}}{2} = - \lambda|\dot{x}|$$
$$\frac{\partial f}{\partial \dot{x}}|_{0} = -\lambda |0| = 0$$
$$\frac{\partial f}{\partial \dot{x}}|_{0} \cdot (\dot{x} - 0) = 0$$
Thus, our linearized differential equation in the neighborhood of $\mathbf{x}_{e}$ becomes:
$$\ddot{x} + U\lambda(x-x_{sp}) \approx 0 $$
The intuition as to why we see certain terms disappear is that as we approach neighborhoods of the origin, terms with degree $\gt 1$ (i.e. $|\dot{x}|\dot{x}$), or more generally first order Taylor expansions that are still functions of the original variable (for the most part), have derivatives which are negligible in a neighborhood of the origin - unlike a linear term. Remember that $f(\mathbf{x}_{e}) = 0$, so we should not see any $i^{th}$ order term where $\lim\limits_{\mathbf{x} \to 0} f^{(i)}(\mathbf{x}) = \infty$, so this removes the possibility of a first-order term like $1/\mathbf{x}$.

Finally, consider a point $\epsilon \approx 0$. Given functions $g(\epsilon) = \epsilon$ and $h(\epsilon) = \epsilon^{2}$, we note $g'(\epsilon) = 1$ and $h'(\epsilon) = 2\epsilon$. Since we know $\epsilon \approx 0$, $h'(\epsilon) \approx 0$ as well. Thus if we have the equation $f(x) = g(x) + h(x)$ modeling our dynamics, we would see that the linear term "dominates" close to the origin.

Edit: Let $y_{r} = -\frac{1}{U\lambda}(2U + \frac{\lambda r_{0}^{2}}{2})$ and $\dot{y}_{r} = 0$

Consider (12) rewritten as:

$$\ddot{y} = f(y, \dot{y})$$
$$\ddot{y} = -\frac{\lambda}{2}(\dot{y}+2r_{0})\dot{y} - U\lambda y - 2U - \frac{\lambda r_{0}^{2}}{2}$$
$$\ddot{y} = -\frac{\lambda}{2}\dot{y}^{2} - \lambda r_{0}\dot{y} - U\lambda y - 2U - \frac{\lambda r_{0}^{2}}{2}$$

Let us now consider $\frac{\partial f}{\partial y}$ and $\frac{\partial f}{\partial \dot{y}}$:

$$\frac{\partial f}{\partial y} = -U\lambda$$
$$\frac{\partial f}{\partial \dot{y}} = -\lambda\dot{y} - \lambda r_{0}$$

Now, let us consider the equilibrium point $\mathbf{y}_e = [y_{r} \ \dot{y}_{r}]^{T}$. So, using the linearization formula from part one, we get:

$$f(y_{r}, \dot{y}_{r}) = 0$$
$$\frac{\partial f}{\partial y}|_{y_{r}} \cdot (y-y_{r}) = -U\lambda (y-y_{r}) = -U\lambda y - 2U - \frac{\lambda r_{0}^{2}}{2}$$
$$\frac{\partial f}{\partial \dot{y}}|_{\dot{y}_{r}} \cdot (\dot{y}-\dot{y}_{r}) = -(\lambda \dot{y}_{r} - \lambda r_{0})\cdot (\dot{y}-\dot{y}_{r}) = -\lambda r_{0} (\dot{y}-0) = -\lambda r_{0}\dot{y}$$

Now, I would assume the author drops the constant terms as they have no impact on the stability characteristics at the point $\mathbf{y}_e$ (they only determine where the equilibrium point will be, not what the convergence actually looks like). After dropping these terms, we are left with:

$$\ddot{y} + \lambda r_{0}\dot{y} + U \lambda y \approx 0$$

in a neighborhood of the point $\mathbf{y}_e$.

Resources:

https://en.wikipedia.org/wiki/Linearization

https://en.wikipedia.org/wiki/Taylor_series

Title: how the two equatoin are linearized


问:
How much accuracy could I get position tracking with a 3-axis accelerometer and gyro sensor, and compass, and how would I do it?
Given a 12' x 12' field (4m x 4m), a reasonably cheap 3-axis gyro sensor and accelerometer, and compass, I plan to design a device capable of tracking its position to sub-centimeter accuracy for a minute of motion or so.

The device has a holonomic drive system, capable of moving any direction at a maximum of about 8mph (3.6m/s), with a maximum acceleration of about 2g's. However, there are some simplifying constraints. For one, the field is nearly flat. The floor is made of a tough foam, so there is slight sinking, but the floor is flat except for a ramp of known angle (to a few degrees). The device will, excepting collisions, not be rising above the floor.

Accuracy is preferred over simplicity, so any mathematics required on the software side to improve the system would be welcomed.

Before I definitively choose accelerometers as the method of position tracking, though, I would like some idea of how much accuracy I could get, and the best ways of doing it.

答:
First, here's what you CAN do with those sensors. Assuming you are not constantly accelerating you can use the accelerometer to know which direction is "down" (the gyro can be used as well for faster updates). If there aren't any magnetic field disturbances, you can also use the compass to know which direction is forward. Usually this is done using either an extended Kalman filter or direct nonlinear algorithms for attitude estimation (sometimes referred to as SO3).

Unfortunately, for position estimation your only option for that set of sensors is to integrate the accelerometer. In an ideal world that is enough, but in the real world all accelerometers have bias error. Over time this will just keep adding error to your position and velocity estimates. You can ballpark the error by
$$
e_p = \frac{1}{2} a_b t^2
$$
where $e_p$ is the position error, $a_b$ is the accelerometer bias, and $t$ is time. In your case, you can reverse this to estimate that 0.010m error after 60s requires an accelerometer bias of less than 5.6e-6m/s^2, which is not going to be found in low cost accelerometers.

To achieve sub-centimeter accuracy you will need another sensor. Usually people will use cameras or laser scanners for this purpose.


问:
Raspberry Pi car connection to laptop in the park where there is no internet
I have built a small Raspberry Pi 4 car which can be controlled via my laptop. Both Raspberry Pi and Laptop are connected to my home internet. I want to take the car outside in the park. There is no internet outside in the park. So how will my Raspberry Pi receive commands from my laptop when there is no internet connection? I do not want to use hotspotting from my mobile. I have seen videos where people are using their Raspberry Pi cars outside without any internet, but I do not know how they are doing it. I will appreciate some help. Thank you</p>

答:
Your Raspberry Pi has a quite capable WiFi. You can configure it to be a hotspot (access point). When it is operating in that mode, you can connect your laptop, phone, or other devices, to its network. Bear in mind, though, that tiny network is not connected to the Internet.

Do an internet search for "Raspberry Pi wifi hotspot" to find out the config details. Retain the previous config settings somehow, though, so you can restore them to be able to reconnect the Pi to your home net.


答:
Two LoRa modules (one for the laptop and one for the pi) should work well to control your Raspberry Pi from your laptop over a long distance, where there is no WiFi available.

Another option could be Bluetooth, but you would be limited by range.

Title: Raspberry Pi car connection to laptop in the park where there is no internet

Body: I have built a small Raspberry Pi 4 car which can be controlled via my laptop. Both Raspberry Pi and Laptop are connected to my home internet. I want to take the car outside in the park. There is no internet outside in the park. So how will my Raspberry Pi receive commands from my laptop when there is no internet connection? I do not want to use hotspotting from my mobile. I have seen videos where people are using their Raspberry Pi cars outside without any internet, but I do not know how they are doing it. I will appreciate some help. Thank you</p>


问:
why it is a dissipating term and is the second order?
corresponding paper

why it said that $-\lambda \frac{|\dot{x}|\dot{x}}{2}$  is a  small second order dissipating term ?

For a linear second order system, the velocity is a dissipating term, because I know the equation is derived from the mass-damper-spring phsyical system.

I am new to ordinary differential equation, the first derivative state term always plays a dissipating role no matter its power and its sign in a second order system?

答:
The word dissipation here is referring to the energy dissipated as heat in the canonical mass-spring-damper system. Usually this term is related to $\dot{x}$ in the form of $\ddot{x} = -b\dot{x}$, and in many cases we call this damping - but it is essentially just friction. Now, if we were concerned with our system's velocity and not position, we should clearly see that the above system is stable (assuming $b \gt 0$).

This is the simple linear case, but what happens if we do not care about small velocities, but want to heavily "dampen" large velocities. In this case, we could look for damping of the form $\ddot{x} = -b\dot{x}^{2}$. But what happens when this system starts out with a negative velocity? Clearly it is unstable. So, we will have to look for a different method of damping.

Consider the system described by $\ddot{x} = -b\ sgn(\dot{x})\dot{x}^2$. Whenever our velocity is negative, acceleration will be positive. Whenever velocity is positive, acceleration will be negative. This is now a stable system.

Finally, consider this system's behavior close to the origin. Our damping force decreases exponentially as we approach a velocity of zero. As this is the case, it makes sense to consider this a small dissipating term - as it will not dissipate all of our systems energy in a finite amount of time.

As a final note, let $b = \frac{\lambda}{2}$ and note that $|\dot{x}|\dot{x} = sgn(\dot{x})\dot{x}^{2}$.


问:
Error when compiling cv_bridge in Raspberry Pi 4B with Raspbian buster and ROS Noetic
Im using Raspberry Pi 4B, with Raspbian Buster and ROS Noetic. When I tried to compile my catkin ws with a cv_bridge package I got the following error:

CMake Error at vision_opencv/cv_bridge/CMakeLists.txt:31
(find_package):   Could not find a package configuration file provided
by "OpenCV" (requested   version 3) with any of the following names:

```<code>OpenCVConfig.cmake
opencv-config.cmake
</code>```

Add the installation prefix of "OpenCV" to CMAKE_PREFIX_PATH or set
"OpenCV_DIR" to a directory containing one of the above files.  If
"OpenCV"   provides a separate development package or SDK, be sure it
has been   installed.

I have install opencv with the following command: sudo apt-get install libopencv-dev python3-opencv

Here is my CMakeLists.tx file in cv_bridge package:

```<code>cmake_minimum_required(VERSION 3.0.2)
project(cv_bridge)

find_package(catkin REQUIRED COMPONENTS rosconsole sensor_msgs)

if(NOT ANDROID)
  find_package(PythonLibs)

  if(PYTHONLIBS_VERSION_STRING VERSION_LESS "3.8")
    # Debian Buster
    find_package(Boost REQUIRED python3)
  #else()
    # Ubuntu Focal
    find_package(Boost REQUIRED python)
  endif()
#else()
find_package(Boost REQUIRED)
endif()

set(PYTHON_NUMPY_INCLUDE_DIR ~/.local/lib/python3.6/site-packages/numpy/core/include)
set(PYTHON_INCLUDE_DIR /usr/include/python3.6)

set(_opencv_version 4)
find_package(OpenCV 4 QUIET)
if(NOT OpenCV_FOUND)
  message(STATUS "Did not find OpenCV 4, trying OpenCV 3")
  set(_opencv_version 3)
endif()

find_package(OpenCV ${_opencv_version} REQUIRED
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  CONFIG
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${PROJECT_NAME}
  CATKIN_DEPENDS rosconsole sensor_msgs
  DEPENDS OpenCV
  CFG_EXTRAS cv_bridge-extras.cmake
)

catkin_python_setup()

include_directories(include <span class="math-container">${Boost_INCLUDE_DIRS} $</span>{OpenCV_INCLUDE_DIRS} <span class="math-container">${catkin_INCLUDE_DIRS})
link_directories($</span>{Boost_LIBRARY_DIRS})

if(NOT ANDROID)
add_subdirectory(python)
endif()
add_subdirectory(src)
if(CATKIN_ENABLE_TESTING)
  add_subdirectory(test)
endif()

# install the include folder
install(
  DIRECTORY include/<span class="math-container">${PROJECT_NAME}/
  DESTINATION $</span>{CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
</code>```

Any help?

Thanks

答:
The file OpenCVConfig.cmake is installed by libopencv-dev on Debian Buster arm64 file list so installing that package is the correct missing dependency.

You're now going to need to dig deeper to find out what your paths are that CMake is searching for and likely look into why it's not finding that version which is stated as being already installed on the system.


问:
What are the materials and methods used to build ROMAC artificial air-muscles?
So, I was reading about pneumatic artificial muscles, and to my surprise I found that there is a lot of types of air-muscles.

One that called my attention was ROMAC artificial muscle:

In this article it is said:

"The flexible walls of the ROMAC are not designed to work as
elastometers. Rather, the geometry of the individual pyramid elements
allows for greater contraction. Additionally, the wire restraining
cables and pyramid elements are combined into a "single surface"
actuator designed to eliminate sliding friction during contraction,
reducing wear on the soft parts and extending service life."

In this other article (page 7) is said:

"The bladder is made of a sheath, that is characterized by its high
tensile stiffness, its flexibility and its fluid-tightness (e.g.
impregnated para-aramid fiber fabric). The netting or harness is
comprised of nonstretchable flexible tension links which are joined at
nodes so as to form four-sided diamond shaped apertures in the
network, as shown in Figure 14. The harness expands radially and
contracts axially, thereby changing the base of each protruding lobe.
As a result of this mechanism the enclosed volume changes. The total
surface of this actuator is constant regardless of
contraction-elongation due to the tensile stiffness of the membrane
material. "

But unlike other articles detailing how the muscle is built, I couldn't find anything useful about ROMAC muscles. Only illustrations explaining how it works and just the above image showing how it works.

As it is said in the first article it is a patent "GUY IMMEGA AND MIRKO KUKOLJ", but I couldn't find the patent, neither the types of material used (besides the metal cable) to achieve such shapes in the muscle.

答:
I found the patent on google patents.

The method of fabrication:

Each base side of a protrusion is attached to a base side of an adjacent protrusion by a flexible seam or continuous fold, and each protrusion foldable about a plane dividing the protrusion into two parts from an axially-extended condition in which the base sides are substantially parallel to an axially-contracted condition in which the protrusion encloses a volume larger than that enclosed in the axially-extended condition.

A pair of axially-aligned end terminations is formed at each end of the enclosure with one of the end terminations being hollow. A pair of end connectors are each coupled to respective end terminations with one of the end connectors having an axial bore which provides fluid communication between an interior of the hollow enclosure and a source of pressurized fluid.

A provision of a plurality of protrusions articulating about their base seams and sides allows the use of substantially non-elastic material for the membrane of the hollow enclosure or bladder, thereby avoiding failure problems associated with elastomeric material.

Moreover, the hollow enclosure may be made from flat sheet material which is strong enough to withstand standard pneumatic line pressures. Alternatively, the hollow enclosure may be a single-curved hollow membrane.

Moreover, the output force exerted and work done by such an actuator is relatively large due to the large change in volume and a large percentage contraction achievable by the enclosure. The percentage contraction is large due to the ability of the enclosures to articulate without excessive radial bulging.

Furthermore, if the hollow enclosure is made from flat sheet material, the volume enclosed by the actuator in the axially-extended state becomes significantly minimized, thus increasing actuator efficiency.

Each base side of a protrusion is attached to a base side of an adjacent protrusion by a flexible seam or continuous fold, and each protrusion foldable about a plane dividing the protrusion into two parts from an axially-extended condition in which the base sides are substantially parallel to an axially-contracted condition in which the protrusion encloses a volume larger than that enclosed in the axially-extended condition.

A pair of axially-aligned end terminations is formed at each end of the enclosure with one of the end terminations being hollow. A pair of end connectors are each coupled to respective end terminations with one of the end connectors having an axial bore which provides fluid communication between an interior of the hollow enclosure and a source of pressurized fluid.

A provision of a plurality of protrusions articulating about their base seams and sides allows the use of substantially non-elastic material for the membrane of the hollow enclosure or bladder, thereby avoiding failure problems associated with elastomeric material.

Moreover, the hollow enclosure may be made from flat sheet material which is strong enough to withstand standard pneumatic line pressures. Alternatively, the hollow enclosure may be a single-curved hollow membrane.

Moreover, the output force exerted and work done by such an actuator is relatively large due to the large change in volume and a large percentage contraction achievable by the enclosure. The percentage contraction is large due to the ability of the enclosures to articulate without excessive radial bulging.

Furthermore, if the hollow enclosure is made from flat sheet material, the volume enclosed by the actuator in the axially-extended state becomes significantly minimized, thus increasing actuator efficiency.

Images of certain parts:

FIG. 9 is a perspective view of a four-sided pyramidal protrusion forming one of several which comprise a hollow enclosure.

FIG. 10 is a perspective view of a truncated pyramidal polyhedron which is adapted to form one of the plurality of polyhedrons of the hollow enclosure.

FIG. 11 is a perspective view of the forces on an element of fabric of the hollow enclosure.

FIG. 12 is a schematic force diagram showing the fabric-cable interaction.


问:
Electronic noses for detecting dog urine
I have very limited experience with sensors or robotic components at all, and I hope you will excuse the lack of detail in this Question. </p>

<p>I want to set up posts around my yard with electronic noses that detect dog urine. I want to use this information to make a map of my yard from a dogs perspective. Is it possible with todays technology? What would it cost? There may be information that is very relevant to me, but that I'm not requesting. This is because of lacking insight. If there is something you think I should consider or research, please say so. </p>

答:
Is it possible? Certainly. Is it worthwhile? Depends on how sensitive you need it to be.

Dog urine has a strong ammonia component (or maybe it's just my dogs that stink so bad) that could be used as the primary analyte to look for. Unfortunately, calibrated electronic ammonia sensors are expensive. I was actually looking into this just a week ago because I raise chickens and I wanted to measure ammonia levels in my chicken coop.

Now, there are some inexpensive gas sensors that claim to detect ammonia such as this one. If you're not concerned about accuracy of the sensor, then it may work. I haven't tried them out yet since I suspect they won't work for my purposes.

What it would cost depends on how much you're willing to learn. The cheap sensors themselves are around \$30 each and can be read with about \$5 worth of parts. However, you would have to spend a fair amount of time learning electronics and interfacing to sensors.

The industrial ammonia meters are upwards of \$500 each and require replacement of the sensor element frequently. I think one of the poultry farming articles I read on the subject suggested an average yearly total cost of about \$900 per instrument.

I don't know what other easily detectable components of dog urine there are, so it's possible that there is a simpler way to detect it.


问:
Modelling/Theory for Quadraped Robot Leg Example
I have been working on attempting to replicate the following robot developed by some MIT graduates.

https://dspace.mit.edu/handle/1721.1/98948

Presently, I am trying to work out the model for a single leg. That would include obviously both links and joints. My hope is to simulate it in ROS2 with Gazebo and some motor controller modules.

I am fairly new to robotics and all the material I have been studying focuses on robots that are structured with a base link, then some set of joints and links that finishes with the end effector. In my case for the leg of this robot, it seems that I have 5 joints, two that connect to the base (effectively the motor rotors) and then three free joints (connections between (1) left femur and left shin, (2) right femur and right shin, and (3) left shin and right shin). The base link would be the body of the robot that holds all four legs together. My understanding would be that the foot would be the end effector and I would use both motors to actuate the position of the foot. Each leg would be a 2DOF system since the foot can only move on a plane and there is no orientation control of the foot for a given position.

I am trying to understand how this sort of model fits into what I am currently learning about forward and inverse kinematics.

What techniques would be used to model and analyze this system?
How might one establish Denavit Hartenberg params for joints that will have rotation based on the rotation of other joint (ie free joints.)

答:
Most tutorials about kinematics and dynamics would teach you open-chain systems, like the typical robotic manipulator. While in this robot, the leg itself is a closed-chain system, which is quite different and indeed much more complicated. You can have a look at Chapter of Modern Robotics if you are really interested.

You're right the leg has 2 DoF and the foot-tip could be regarded as an end-effector when you try to do some analysis. I know little about closed-loop chain system kinematics and dynamics so I'm not able to give you more insights. But I would recommend you not to focus on DH params too much, because I think DH params are just providing a way to establish coordinate frames. Actually, it doesn't matter how you establish these frames and you can always use Generalized Transformation matrices to compute kinematics.


问:
Does the Bayes-Filter perform a convolution in the prediction step?
I am watching the (fantastic) SLAM lectures of Claus Brenner, where he introduces the Bayes-Filter (Kalman-Filter, Particle-Filter, Histogram-Filter).

He says, that the prediction step involves the convolution of distributions and the correction step a multiplication of distributions (link).
$$\text{prediction: }p(x)=\sum_y p(x\mid y)p(y)$$
$$\text{correction: }p(x\mid z)=\alpha p(z\mid x)p(x)$$
My problem is with the convolution. It makes sense the way he derives it, but I cannot make the connection to the standard definition of convolution as give, e.g. at Wikipedia:

$$p(Z=z) = \sum_{k=-\infty}^\infty p(X=k)p(Y=z-k)$$

Is this a mistake in the video, or am I missing something? It just looks like the law of total probability.

答:
See this nice tutorial:
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/02-Discrete-Bayes.ipynb
Using a Bayesian derivation for filtering, the prediction step can be seen as a "Marginalization". Instead the posterior, i.e. the filtered estimate, can be seen as a "Conditioning". The first uses the "Chapman-Kolmorov" equation, while the second one makes use of Bayes' rule.

When dealing with discrete random variables, I guess the integrals are substituted with sums, and the functions of two parameters, e.g. the current time step and the forward time step, can be seen as functions of one variable, e.g. the time lag (the difference between the time instants. This is the case for instance with Markovian stochastic processes, I guess.


问:
Drawing in gazebo ROS
I am trying to simulate motion for a ur5e robotic arm. I am using the concept of Rodrigues rotation to get the orientation for the end effector. Is there any way to draw a line in gazebo or RViz from one point to another?</p>

答:
The recommended way to draw arbitrary shapes in RViz is to use the Marker Display you can draw shapes in specific locations with colors and different alpha elements.

If you want to interact with them you an step up to interactive_markers

I recommend getting more info from the rviz Tutorials

Title: Drawing in gazebo ROS


问:
Control a differential drive robot
I have some questions about implementing a mobile robot. I have an old construction mobile robot wherein this robot the main controller has been removed, and I put in its place a Raspberry Pi 4. I have put Linux and ROS on Raspberry.

I found this library https://abyz.me.uk/rpi/pigpio/index.html I managed by sending pulses through the GPIO to have movement on the wheels.

These pulses can be converted to RPM with the appropriate code, so I can also have the wheel speeds in meters per second. Through forward kinematics, I can convert them to angular and linear velocities of the body entirely theoretically.
Is there a way to do this with ROS?

I made a Python script for ROS, which converts linear and angular velocities to wheel velocities, in other words, inverse kinematics.

Can these two coexist so that I can control the robot?

I appreciate any help you can provide.

答: Take a look at the donkeycar project. It's an autonomy framework for scale (RC) cars written in Python that supports a number of different kinds of drive trains.  Closed-loop speed control and pose estimation via encoders is in flight (see https://github.com/autorope/donkeycar/tree/921-tachometer-odometer-parts)</p>

Title: Control a differential drive robot


问:
What motor to use for reciprocating (reversive) movement
I want to make a copy of this machine Fisher Price Soothing Motions™ Glider and I'm wondering what motor to use? Simple DC motor with appropriate gearbox (slow rpm) or stepper motor?

Here is another instance of this idea.

答: A DC or AC gearmotor is all you need. If you're asking about a stepper because of the change in direction, you can use a crank shaft mechanism to convert motion from rotary to reciprocating linear like the one in the above image:


问:
Val3: Reading an Excel spreadsheet
I want to create a pPoint in Val3, which creates an array with the coordinates. The coordinates to be approached are in an Excel table. Is it possible to read this table using a command with Val3 and import it into the pPoint array?

Best regards

答:
There are a couple of possibilities. If you are using Staubli Robotics Studio (SRS). you could create the point variable or array and do a copy paste operation.

The other possibility is to save the excel spreadsheet as a Comma Separated Value file (CSV). Then install the "expansion" software add on for the SRC version installed on your controller. The expansion add on has "beta" instruction that are being tested for possible use in future releases of SRC (Staubli robotics control). The expansion add on has instruction for opening, reading, and close files. It will be possible to easily parse the CSV file and copy the data into the fields of a point variable.

The software add on such as "expansion" are versions specific. New versions have to be complied each time SRC is updated. The Software add ons are not publicly available and you will have to contact the Staubli Technical Support team for your area to get a copy.

Title: Val3: Reading an Excel spreadsheet


问:
What motor controller board is this?
I was watching a Mark Rober video in which he shows a prototype of his robot, does anyone know what the board in the attached screen-grab is?

答:
I think I found what you're looking for here on Amazon, called "DC Brush Motor Controller, DROK 16A Dual Channel H Bridge Motor Driver Brushed Board DC 5V-36V Motor Drive Control Smart Car PWM Speed Regulator"

I found it by looking at the board in your picture. I counted what appears to be eight beefy-looking transistors, and you mentioned it's for a robot so I assumed they were setup for two H-bridge circuits. Then the connectors on the end look like servo connectors, which are typically PWM, so I googled "Dual H-bridge board with PWM drivers" and found a version online.

Hope this helps! Looks like fun :)

Title: What motor controller board is this?


问:
Superposition in state space equation with multiple harmonics in the inputs
I have a question related to the superposition in state space equation. Suppose I have a state space equation below
\begin{equation}
\frac{d}{dt}X = AX + B\begin{vmatrix}
u_1(t)\\
u_2(t) \\
\end{vmatrix}
\end{equation}
where X is the state vector, A, B are constant matrixes. u is the input time varying signals.

Now assume u can be expressed as
\begin{equation}
u_1(t) = c_1\cos(w_1t)+c_2\cos(w_2t)\\
u_2(t) = d_1\cos(w_1t)+d_2\cos(w_2t)\\
\end{equation}
where c1, c2, d1, d2 are constant coefficients, and w1 and w2 are different.

Can I rewrite the state space equation into the following two equations:
\begin{equation}
\frac{d}{dt}X_1 = AX_1 + B\begin{vmatrix}
c_1\cos(w_1t)\\
d_1\cos(w_1t) \\
\end{vmatrix}
\end{equation}
and
\begin{equation}
\frac{d}{dt}X_2 = AX_2 + B\begin{vmatrix}
c_2\cos(w_2t)\\
d_2\cos(w_2t) \\
\end{vmatrix}
\end{equation}

And then I solve for time-domain steady state expression for X1 and X2. The final time domain expression for X would be
\begin{equation}
X = X_1+X_2
\end{equation}

Is there anything wrong with this method?

答:
That's correct!

It's the magic of linear systems.

Find it also on Wikipedia.


问:
How to install Gazebo to use along with ROS2 Humble Hawksbill on Ubuntu 22.04?
I have used ROS noetic before by installing the full desktop version which comes along with gazebo simulator and its plug-ins (gazebo_ros_pkgs) ready to use with ROS directly. Due to some requirements, I have to install ROS2 on Ubuntu 22.04 on a cloud/ virtual machine. I decided to move with ROS2 Humble Hawksbill because as per https://docs.ros.org/en/rolling/Releases.html, this release will be supported for a longer period of time compared to Foxy which will have its EOL next year.

I followed all the installation instructions present on their official website and did the full install using sudo apt install ros-humble-desktop. Publisher/ subscriber, rviz, rqt etc. are working fine but I observed that gazebo did not come along with it.

I am having a tough time figuring out a way to install gazebo and make it work with ROS2 Humble Hawksbill on Ubuntu 22.04.

I tried going through https://classic.gazebosim.org/tutorials?tut=ros2_installing&cat=connect_ros, https://gazebosim.org/docs/fortress/ros2_integration, and https://discourse.ros.org/t/ros-2-humble-hawksbill-released/25729 and observed that one website is saying something and another something else regarding ros2 and gazebo versions compatibility.

Any help in setting up gazebo (which version and how) in this scenario is highly appreciated.

Thanks in advance,

Dev

答:
Gazebo is part of desktop-full not the desktop variant.

You can use ros-humble-simulation as a convenient metapackage as well.

The metapackages are defined as variants in REP 2001 (Humble)


问:
Vision programming with OpenCV
I am currently working on a project on NiryoOne which requires facial detection of a person through NiryoOne camera (ELP SONY IMX). The code that I am using works on my laptop running ubuntu. However, I am currently getting an assertion error, which suggests that the file path has not been properly specified, but have tried various file path and none seem to be the solution. Not certain if I am missing some dependencies or packages on NiryoOne. Any help regarding the use of OpenCV would be greatly appreciated.

For both laptop and NiryoOne

Ubuntu: 16.04
Python: 2.7.13
OpenCV: 3.3.1-dev

```<code>
#!/usr/local/bin/python
# -*- coding: utf-8 -*-
import os, sys
import cv2
import numpy as np
import rospy
from std_msgs.msg import Int32

Start = False

rospy.init_node('my_third_node')
pub = rospy.Publisher('camera',Int32,queue_size=10)

#initialise cascade type
face_cascade = cv2.CascadeClassifier('/haarcascade_frontalface_alt2.xml')

# Open the device at the ID 1 (USB)
cap = cv2.VideoCapture(0)

#To set the resolution
#remove cv.cv depeding on version
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

#REPLACE WITH VOICE LATER
while(True):
    ret, frame = cap.read()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #convert to grey for haarcascade as per documentation

    faces = face_cascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=5) #1.5, 5 standard values ; scaling factor increase for higher accuracy
    # x,y = x,y coordinates, h,w = height, weight unit pixel
    for (x, y, w, h) in faces:
        print(x,y,w,h)
        pub.publish(x)
        pub.publish(y)
        pub.publish(w)
        pub.publish(h)

        #roi = region of interest
        roi_gray = gray[y:y+h+35, x:x+w+20]
        #roi_color = frame[y:y+h+35, x:x+w+20]
        img_item = "saved-img.png"
        cv2.imwrite(img_item, roi_gray)

        #bounding rectangle
        stroke = 2
        end_cord_x = x + w
        end_cord_y = y + w
        #arg 1;2;3: frame to draw, start coord, end coord
        cv2.rectangle(frame, (x,y), (end_cord_x,end_cord_y), (255, 0, 0))
        # Start == True
        # if Start == True:
        #       cap.release()
        #       cv2.destroyAllWindows()
    cv2.imshow('frame', frame)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break

#Check whether user selected camera is opened successfully.
if not (cap.isOpened()):
    print('Could not open video device')

# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()

</code>```

Error

```<code>niryo@niryo-desktop:~/sepra6$ python camhaar.py
OpenCV Error: Assertion failed (!empty()) in detectMultiScale, file /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/objdetect/src/cascadedetect.cpp, line 1698
Traceback (most recent call last):
  File "camhaar.py", line 31, in <module>
    faces = face_cascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=5) #1.5, 5 standard values ; scaling factor increase for higher accuracy
cv2.error: /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/objdetect/src/cascadedetect.cpp:1698: error: (-215) !empty() in function detectMultiScale
</code>```

答:
Without enough information it's hard to say more than you have a bug in your code at the line it points to.

```<code>faces = face_cascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=5) #1.5, 5 standard values ; scaling factor increase for higher accuracy
</code>```

One of the arguments is failing the assertion that it's not empty.

Title: Vision programming with OpenCV

Body:
Without enough information it's hard to say more than you have a bug in your code at the line it points to.

faces = face_cascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=5) #1.5, 5 standard values ; scaling factor increase for higher accuracy

One of the arguments is failing the assertion that it's not empty.


答: You haven't made a mistake. There is a bias in your accelerometer readings (which is a true statement for all accelerometers) and then you're numerically integrating that bias, which results in position drift.

Title: Kalman filter problem with the output

Body:
i want to use kalman filter to estimate my phone position, the measurments data is at this point just the accelerometer and the sampling rate is 3ms, i used the library pykalman, i have also wrote my own implementation of kalman filters and they both return the same results.
transition matrix :

```<code>dt = 0.003
F = np.matrix([[1.0, 0.0, 0.0,dt,0.0,0.0,0.5*(dt)*(dt), 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0,dt,0.0,0.0, 0.5*(dt)*(dt), 0.0],
[0.0, 0.0, 1.0, 0.0,0.0,dt,0.0, 0.0, 0.5*(dt)*(dt)],
[0.0, 0.0, 0.0,1.0,0.0,0.0,dt, 0.0, 0.0],
[0.0, 0.0, 0.0,0.0,1.0,0.0,0.0, dt, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,1.0,0.0, 0.0, dt],
[0.0, 0.0, 0.0,0.0,0.0,0.0,1.0, 0.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 1.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 0.0, 1.0]])
</code>```

measurment matrix

```<code>H =np.matrix([[0.0, 0.0, 0.0,0.0,0.0,0.0,1.0, 0.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 1.0, 0.0],
[0.0, 0.0, 0.0,0.0,0.0,0.0,0.0, 0.0, 1.0]])
</code>```

measurment noise

```<code>rp = 0.02  # Noise of Position Measurement
R = np.matrix([[rp, 0.0, 0.0],
               [0.0, rp, 0.0],
               [0.0, 0.0, rp]])
</code>```

initial state vector

```<code>X0 = np.matrix([0,0,0,0,0,0,measurments[0][0],measurments[0][1],measurments[0][2]])
</code>```

state covariance matrix

```<code>P0 =np.matrix([[1, 0, 0, 0, 0, 0,0,0,0],
    [0, 1, 0,0, 0, 0,0,0,0],
    [0, 0, 1, 0, 0, 0,0,0,0],
    [0, 0, 0, 1, 0, 0,0,0,0],
    [0, 0, 0, 0, 1, 0,0,0,0],
    [0, 0, 0, 0, 0, 1,0,0,0],
     [0, 0, 0, 0, 0, 0,1,0,0],
     [0, 0, 0, 0, 0, 0,0,1,0],
     [0, 0, 0, 0, 0, 0,0,0,1]])
</code>```

i load the accelerometer data, and then use pykalman to find x,y and z.

```<code>measurments = []
for i in range(len(df_acc)):
    measurments.append([df_acc.iloc[i]['x'],df_acc.iloc[i]['y'],df_acc.iloc[i]['z']-9.82])
n_timesteps = len(measurments)
n_dim_state = 9
filtered_state_means = np.zeros((n_timesteps, n_dim_state))
filtered_state_covariances = np.zeros((n_timesteps, n_dim_state, n_dim_state))

kf = KalmanFilter(transition_matrices = F,
                  observation_matrices = H,
                  observation_covariance = R,
                  initial_state_mean = X0,
                  initial_state_covariance = P0)

for t in range(n_timesteps):
    if t == 0:
        filtered_state_means[t] = X0
        filtered_state_covariances[t] = P0
    else:
        filtered_state_means[t], filtered_state_covariances[t] = (
        kf.filter_update(
            filtered_state_means[t-1],
            filtered_state_covariances[t-1],
            measurments[t]))
</code>```

the results are shown in the picture, the data i captured with my phone while walking in a small appartment i can't have walked 1 mile lol, plus i've been walking in a straight line the other axis claim that ive walked 200 meters along x and z which is impossible. I can'
t find where i made a mistake.


问:
Does torque affect prismatic joints in a gantry robot? If so, how can we reduce this?
I was posed a question based on my project on a gantry robot, "Cancelling torques that affect the prismatic joints can help us choose components with a lower cost which increases the cost-effectiveness of our system, how can we cancel these torques? (Think about external guides and guide rails)".

I've been thinking about this for a couple of days, googling different stuff, but I haven't been able to come up with answer. I can't fathom how torque would affect a linear joint, and thus coming up with a solution is difficult.

Any help would be appreciated.

答:
It sounds like you're thinking about the forces and strength of the powered components of the joints. The prismatic joints maintain their shape by transferring any torques applied to one link through to the connected link.

As an example, if you think about a classic tower crane with a tower and an arm across the top. When you pick up a load a the tip of the crane arm all of the new forces from the load are supported by torque at the top of the crane tower. This requires that joint to be very strong and take high torques due to the large lever arm. However if you were to build a second tower and attach it to the other end of the lifting arm you could then lift the same load with much less torque on any joint because the lever arm is much shorter to the closer support point. And the prismatic joints won't need to be build to counter as much torque. And thus the design can be more cost effectively built with lower load ratings for the prismatic joints. Lower load ratings will mean that you can have fewer or cheaper bearings to make sure that the prismatic joint keeps working.


问:
Understanding the readings of Force Torque sensor based on accuracy and precision

I am working with a 6-axis force-torque sensor to measure the thrust and moments of a model jet engine. I want to understand the significance of the accuracy (measurement uncertainty) and precision (resolution) of these sensors. Note: I understand the textbook definitions well, I want to understand how these translate to real-world applications.

A realistic sample problem is as follows:

There are 2 sensors in the market.

Say both have the Maximum calibration for Fz as 1000N.

Both have the maximum measurement uncertainty as 1% of the Full-Scale calibration.

And say the first one has a resolution capacity of 0.1N and the second one has a resolution of 0.2N.

Here is what I understand:

The results of measurements will be accurate within 1% of 1000N which is $\pm$10N.

The resolution of the measured value for the first sensor is 0.1N and for the second 0.2N.

So if the measured thrust is 100.4N then I should report my measurement as 100.4N $\pm$10N.

The questions:

What is the significance of resolution here?
Since accuracy is 10N which is more than resolution isn't resolution insignificant here?

Both force sensors seem to be giving almost the same information. Both the first and second sensor say my value will be something within the range 90.4N to 110.4N. How does resolution come into play here?

A better resolution than accuracy seems to be useless.

Does resolution come into play when talking about repeatability? For eg: When I do the measurement again the first sensor is expected to give me a value of 100.4N $\pm$ 0.1N whereas, the second sensor is expected to give a value of 100.4N $\pm$ 0.2N?

Please provide me with an explanation on the same.

Thanks!

答:
The question was becoming horrifyingly long so I am posting my further progress as an answer.

EDIT1: Some further understanding:

To understand a little more about resolution and accuracy imagine taking force measurements and then marking these measurements on a ruler. The ruler has the properties of the measuring device (I clarify this next).

Now the resolution will define how the markings on the ruler move. Eg: If the resolution is 0.1N then the ruler markings will move as 100, 100.1, 100.2, 100.3, 100.4, 100.5... . If the resolution was 1N then the ruler markings would move as 100, 101, 102, 103... . So the first device would display a reading of 100.4N whereas the second device can only show 100N so in this sense the first is better.

Now accuracy is another layer of uncertainty around the measured value. If the accuracy of the measuring device was 1N then the first result would be quoted as 100.4N $\pm$1N and the second result would be quoted as 100N $\pm$1N.

So basically resolution allows us to move on a finer underlying scale for measurements.

Q) How does finer resolution help though? Since accuracy is $\pm$1N what use is resolution lower than this? Since there will still be uncertainty of 1N?

EDIT2: This is to go beyond:

Imagine 2 devices. Both with an accuracy of 10. First one has a resolution of 1 and the second one has a resolution of 10.

Say the actual true value of some physical value is 105.2

The scale on the first device will move as 100, 101, 102, 103, 104, 105, 106... and the scale on the second device will move as 90, 100, 110, 120.
The first device will measure a value of 105 and the measurement will be reported as 105 $\pm$ 10 which is the range [95, 115]. Whereas, the second device will measure a value of 110 (assuming that 105 is measured as 110 not as 100) and the measurement will be reported as 110 $\pm$ 10 which is the range [100, 120]. Now let us visualize this to understand what is happening.

Note here that the distance between the lower limits of the intervals is of the order of Accuracy. This happens when we are comparing resolutions where one is of the order of Accuracy and the other is one order lower. Therefore, the shift between the intervals is large.

Had the resolutions we compared been one and two orders of magnitude lower respectively then the shift between the intervals would have been much smaller compared to Accuracy and we could neglect that.

In Conclusion: Resolution helps in making the measured value better. Decreasing resolution improves the measured value. But decreasing resolution only makes significant changes if it is one order of magnitude smaller than accuracy itself. Trying to make a device with a resolution several orders of magnitude lower than accuracy would therefore be quite useless.

EDIT3: This is to go even further beyond:

The above conclusion is specific to the case when our primary desire is to measure some physical quantity and get an estimate of its value.

But, if our primary desire is only to sense a change in the quantity and the actual value of the quantity is irrelevant then improving resolution monotonically improves our results.

Eg: Suppose someone presses a button. The sensor on the button has to detect if the force applied on the button increased or decreased, that is, whether there was a change in the environment of the button. Here the actual value of the force on the button is absolutely useless and only the change is of importance. Therefore, here resolution is of importance.


问:
[ROS2 Foxy]RVIZ crashes when using Nav2- Turtlebot3
I am currently working on my masters degree project

I am using Turtlebot3(Raspberry pi 3B+ version) and I am using ROS2 Foxy on a ubuntu Dualboot

I use this command on my ubuntu to launch the navigation2: ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=/path/to/map
This is the default launch file and I haven't made any changes to it.

I am facing the following errors when I run the navigation2 with RVIZ.
These errors seems to occur randomly.

InternalErrorException: Cannot create GL vertex buffer in GLHardwareVertexBuffer::GLHardwareVertexBuffer at /tmp/binarydeb/ros-foxy-rviz-ogre-vendor-8.2.6/obj-x86_64-linux-gnu/ogre-v1.12.1-prefix/src/ogre-v1.12.1/RenderSystems/GL/src/OgreGLHardwareVertexBuffer.cpp (line 46)

`(min.x <= max.x && min.y <= max.y && min.z <= max.z) && "The minimum corner of the box must be less than or equal to maximum corner"' failed. Aborted

I also got the same errors when using my ubuntu VM

I am new to ROS, can anyone suggest a solution for this? If anyone knows please let me know.

I greatly appreciate any help in this regard,

Thanks in advance

[EDIT] - link to the same question on ROS answers https://answers.ros.org/question/402195/ros2-foxyrviz-crashes-when-using-nav2-turtlebot3/

答:
RViz is having trouble creating the necessary GL context. You can find this issue listed in the rviz/Troubleshooting

This is caused by a lack of the GL_ARB_vertex_buffer_object OpenGL
extension. I have seen two causes for this apparent lack:

```<code>Running rviz remotely (which is not supported)
Version mismatch between libGL.so and the OpenGL driver.
</code>```

To me this suggests that you might need to make sure that you have your graphics drivers setup correctly. Both in your native installation or in your VM with access to the native graphics driver outside the VM such that you have access to the necessary GL resources.

Body:
RViz is having trouble creating the necessary GL context. You can find this issue listed in the rviz/Troubleshooting

This is caused by a lack of the GL_ARB_vertex_buffer_object OpenGL
extension. I have seen two causes for this apparent lack:

Running rviz remotely (which is not supported)
Version mismatch between libGL.so and the OpenGL driver.

To me this suggests that you might need to make sure that you have your graphics drivers setup correctly. Both in your native installation or in your VM with access to the native graphics driver outside the VM such that you have access to the necessary GL resources.


问:
Impact of using PID Output as a Change (Delta) vs Absolute
I am trying to control speed of a motor using PID. It seems like I have two approaches.

Using PID to calculate the absolute duty cycle value of the PWM

Using PID to calculate the change needed in the current duty cycle.

It seems like all the traditional PID controllers are used for calculating absolute value at the output. I need some help to understand which is the better approach to do this.

答:
All PID controllers are relative in the sense that they act on a reference error, which is the difference between the reference signal and the sensed signal. Assuming the integral error starts at the same value, you should get the same result when the reference is zero and sensed is 10% as when the reference is 50% and the sensed is 60%. In both cases, (reference - sensed) is (-10%).

This is where things like the "step test" are useful in evaluating PID performance - the absolute start and end values aren't really as important as the relative change in input values.

The big difference comes in when you look at how the output is used, as you state in your post. The PID controller acts on the reference error, but it doesn't have any model of how the system is going to respond, so it just continues to adjust the output until the sensed signal matches* the reference signal.

(* it will actually adjust until steady state conditions are met, convergence depends on your integral gain settings).

One of the big issues with PID controllers is that there IS no system model. When you adjust the control gains to get your desired performance you're only optimizing for that particular input type (ramp or step, etc.) and for that input change.

A PID controller that's adjusted for a really fast response to a small ramp input may be unstable for a large step input, etc., because it responds faster than the system can respond. You can adjust the controller to have a slower response, but then it's not as performant with smaller input changes.

This generally happens in the "absolute" use case you describe above, where the PID controller is providing the entire control signal to the system. The PID controller only acts on reference error so there must necessarily always be some delay between the reference input changing and the system response.

The alternative is kind of the "relative" use case you describe. But, if the PID controller is making adjustments to the control signal, then there must be some other source providing a control signal.

This is called a "feed-forward" controller. The feed-forward controller (typically) ignores the sensed value and acts only on the reference signal. This means the feed-forward controller becomes essentially a "coarse" adjustment, providing the bulk of the control signal, and the PID controller becomes the "fine" adjustment, making changes as necessary to achieve the reference signal.

When your control scheme is setup this way, you wind up never "asking" the PID controller to provide large bulk adjustments to the control signal. The feed-forward controller reduces the magnitude of reference error that needs to be addressed and this allows you to adjust the PID controller to have a faster response without worrying as much about instabilities in larger reference changes.


答:
accuracy is how close the reading is to reality. If it say 100, but is actually at 101 then the accuracy is on the order of +/-1. Precision is how repeatable the reading is. I measure 101.1, 101.2, 109.9 the accuracy is still approximately 1, but repeatability is +/- 0.1.

In many cases you are trying to measure a change and not the absolute value. I care more that a modification I made caused the thrust to increase by 5% than to know the exact thrust.

Precision can never be better than resolution. Resolution can often be determined by calculations. Precision, like accuracy usually has to be measured.

Title: Understanding the readings of Force Torque sensor based on accuracy and precision

Body:
I am working with a 6-axis force-torque sensor to measure the thrust and moments of a model jet engine. I want to understand the significance of the accuracy (measurement uncertainty) and precision (resolution) of these sensors. Note: I understand the textbook definitions well, I want to understand how these translate to real-world applications.

A realistic sample problem is as follows:

There are 2 sensors in the market.

Say both have the Maximum calibration for Fz as 1000N.

Both have the maximum measurement uncertainty as 1% of the Full-Scale calibration.

And say the first one has a resolution capacity of 0.1N and the second one has a resolution of 0.2N.

Here is what I understand:

The results of measurements will be accurate within 1% of 1000N which is $\pm$10N.

The resolution of the measured value for the first sensor is 0.1N and for the second 0.2N.

So if the measured thrust is 100.4N then I should report my measurement as 100.4N $\pm$10N.

The questions:

What is the significance of resolution here?
Since accuracy is 10N which is more than resolution isn't resolution insignificant here?

Both force sensors seem to be giving almost the same information. Both the first and second sensor say my value will be something within the range 90.4N to 110.4N. How does resolution come into play here?

A better resolution than accuracy seems to be useless.

Does resolution come into play when talking about repeatability? For eg: When I do the measurement again the first sensor is expected to give me a value of 100.4N $\pm$ 0.1N whereas, the second sensor is expected to give a value of 100.4N $\pm$ 0.2N?

Please provide me with an explanation on the same.

Thanks!