问:
How to Contol the RPM of a Micro Metal Gearmotor with Encoders (Pololu) using PID?
I use the PID algorithm to control the RPM of a micro Gearmotor.  My motor and encoder specifications are given below.

Motor: https://www.pololu.com/product/2378 - 6V 29.86:1 gear ratio - please kindly note that I'm using a 9V battery.
Encoder: https://www.pololu.com/product/4760 - 12 counts per revolution of the motor shaft

Also, using Arduino Mega MCU, and this is my sketch:

```<code>#include <util/atomic.h> // For the ATOMIC_BLOCK macro

#define ENCA 3 // YELLOW
#define ENCB 2 // WHITE
#define PWM 5
#define IN2 6
#define IN1 7

volatile int pos_i = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevT = 0;
int posPrev = 0;

volatile float velocity_i = 0;
volatile long prevT_i = 0;

float v1Filt = 0;
float v1Prev = 0;
float v2Filt = 0;
float v2Prev = 0;

float eintegral = 0;
float eprev = 0;

void setup() {
  Serial.begin(9600);

  pinMode(ENCA,INPUT);
  pinMode(ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);

  pinMode(PWM,OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);

}

void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }
}

void loop() {

  int pos = 0;
  float velocity2 = 0;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
    pos = pos_i;
    velocity2 = velocity_i;
  }

  long currT = micros();
  float deltaT = ((float) (currT-prevT))/1.0e6;
  float velocity1 = (pos - posPrev)/deltaT;
  posPrev = pos;
  prevT = currT;

  float v1 = velocity1 / 89.58  * 60.0;
  float v2 = velocity2 / 89.58  * 60.0;

  // Low-pass filter (25 Hz cutoff)
  v1Filt = 0.854*v1Filt + 0.0728*v1 + 0.0728*v1Prev;
  v1Prev = v1;
  v2Filt = 0.854*v2Filt + 0.0728*v2 + 0.0728*v2Prev;
  v2Prev = v2;

  float vt = 60;

  float kp = 2.0;
  float ki = 4.5;
  float kd = 0.0;

  float e = vt - v1Filt;
  eintegral = eintegral + e*deltaT;

  float dedt = (e-eprev)/(deltaT);

  float u = kp*e + ki*eintegral;

  // Set the motor speed and direction
  int dir = 1;
  if (u<0){
    dir = -1;
  }
  int pwr = (int) fabs(u);
  if(pwr > 255){
    pwr = 255;
  }
  setMotor(dir,pwr,PWM,IN1,IN2);
  eprev = e;

  Serial.print(vt);
  Serial.print(" ");
  Serial.print(v1Filt);
  Serial.println();
  delay(1);

}

void readEncoder(){
  // Read encoder B when ENCA rises
  int b = digitalRead(ENCB);
  int increment = 0;
  if(b>0){
    // If B is high, increment forward
    increment = 1;
  }
  else{
    // Otherwise, increment backward
    increment = -1;
  }
  pos_i = pos_i + increment;

  // Compute velocity with method 2
  long currT = micros();
  float deltaT = ((float) (currT - prevT_i))/1.0e6;
  velocity_i = increment/deltaT;
  prevT_i = currT;
}
</code>```

However, the motor RPM is rapidly oscillating, and it is not reaching the desired RPM level. I tried several Kp and Ki values, but still no luck.

Any help regarding this issue is highly appreciated.

Best regards,
Upul

答:
An alternative to PID control is a constant step controller.  It's a controller that looks at the error and then just adds (or subtracts) a constant amount (usually very small).  It's very easy to implement and very intuitive.  However, it is slow to reach the desired velocity.  This can be mitigated with a feed forward mechanism; when starting from 'stopped' calculate the initial control value (throttle pwm value) based on measured values for stall throttle (the throttle (pwm value) below which the motor will not turn; so this also corresponds to the minimum possible non-zero velocity).  Given the target velocity, you can do a linear interpolation between min and max velocity to estimate the feed forward value for throttle.  Then the constant step controller will converge quickly from a standing start.  If there is a large perturbation while moving it will still respond slowly.

This differential drive robot uses a feed forward constant step controller for speed control and a constant step controller for heading control during goal to goal behavior.  https://github.com/Ezward/Esp32CameraRover2  Speed control is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/wheel/drive_wheel.cpp#L323, kinematic pose estimation is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/rover/rover.cpp#L313, and heading controller is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/rover/goto_goal.cpp#L333  Here an example of it in operation https://youtu.be/TjE9ceNOTJE


问:
Differential Drive Control Techniques
I have a mobile robot that has a differential drive technique. I try to control the robot with PID controller. Is the best way for controlling differential drive robots?

If I want to compare different control techniques for this robot, which techniques do I search for?

答:
One way to do this easily is with a constant step controller.  It's a controller that looks at the error and then just adds (or subtracts) a constant amount (usually very small).  It's very easy to implement.  However, it is slow to reach the desired velocity.  This can be mitigated with a feed forward mechanism; when starting from 'stopped' calculate the initial control value (throttle pwm value) based on measured values for stall throttle (the throttle (pwm value) below which the motor will not turn; so this also corresponds to the minimum possible non-zero velocity).  Given the target velocity, you can do a linear interpolation between min and max velocity to estimate the feed forward value for throttle.  Then the constant step controller will converge quickly from a standing start.  If there is a large perturbation while moving it will still respond slowly.

This differential drive robot uses a feed forward constant step controller for speed control and a constant step controller for heading control during goal to goal behavior.  https://github.com/Ezward/Esp32CameraRover2  Speed control is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/wheel/drive_wheel.cpp#L323, kinematic pose estimation is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/rover/rover.cpp#L313, and heading controller is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/rover/goto_goal.cpp#L333  Here an example of it in operation https://youtu.be/TjE9ceNOTJE

ContentLicense: CC BY-SA 4.0

Title: Differential Drive Control Techniques


问:
What is the meaning of joint space and configuration space and task space?
I'm reading an article about teleoperation systems and it talks about three spaces that include joint space, configuration space and task space frames. I do not know what these are, can you explain?</p>

答:
All "spaces" are used to define the current state of the robot, but with differences. The state of the robot is a point (also known as a configuration or pose) in a space. Please note that the definitions are somewhat fluid and different researchers attribute a somewhat different meaning to the spaces you mentioned, which adds to the confusion.

The simplest to start with is the Cartesian Space. In a general case this is a 3 dimensional space, with XYZ dimensions. Everybody agrees here. The state of the TCP in Cartesian space is refered to as position

If we place a robot in this space, the End Effector of this robot will have a certain position. We can project the Tool Center Point to the XYZ axes and we can get the XYZ coordinates of the Tool Center Point. Obviously, the TCP will also have an orientation, but if we only describe the TCP in 3 dimensions that information gets lost. So...to deal efficiently with the orientation another 3 dimensions can be added to the XYZ Cartesian space. This way we get a space with 6 dimensions the 3 Cartesian dimensions and 3 dimensions describing the rotation around the 3 Cartesian Axes. (Please note that the rotations can be described also otherwise, e.g. RPY, or any other Euler or Brian-Trait angles).

Usually this space, 3 Cartesian dimensions and 3 rotations is the basis of defining the workspace of the robot and in all cases this is the space on the basis of which the operational space and the task space is defined.
The operational space is in most cases exactly this space, in which the robot "operates". In some cases this is also named the tasks space, in which the robot executes the tasks. In some other cases the task space has nothing to do with the robot, but only on the task and is defined only as the dimensions needed to define the task, which are later checked if they match a robots operational space or not and if it matches the workspace or not.
The workspace, when defined in operational space, describes all the points which are reachable by the robot, considering only the inherent limitations (like joint limits and self collisions), but not considering any obstacles. So if a task space fits into the workspace, which is a subset of the operational space, all is good!

Even if the robot has fever degrees of freedoms, it can be described with 6 dimensions in operational space (although in most cases it makes sense to match the number of degrees of the robot and disregard some dimensions in the 6D task space, e.g. which are constant 0 or which cannot be controlled). Not all 6 dimensions can be controlled independently for any robot structure e.g. for a SCARA robot you can control 4 dimensions XYZ and rotation around Z. The other coordinates (rotation around X an Y are constant 0). The state of the robot in the operational space is also known as the pose (position and orientation together)

If we take the operation space coordinates of the TCP we can solve the inverse kinematics problem and get the joint space coordinates. The joint space dimensions equals the number of degrees of freedom of the robot and, only specifying the task space coordinates does not unambiguously describe the joint space configuration. More then 1 points in the joint space can have the same operational space configuration.

The configuration space is a subset of the joint space. It can be imagined as a limitation on the joint space. If we imagine any arbitrary point in joint space it can happen that the robot cannot assume that given joint space point (or joint configuration) this might be due to physical limitations on the joints (e.g. for a linear X axis the point would be outside the joint limits) or due the fact that the robot would collide with an obstacle when assuming the joint space configuration. The configurations space refers to the subset of joint configurations in joint space which are valid. If we disregard the external limitation (e.g. obstacles) and we only consider the inherent limitations (e.g. joint limits and self collision) we get the workspace defined in the joint space.

Configurations space is in most cases defined in joint space, but the more relevant definition is that is describes the robot as one point in a space and that point can move around freely in the configuration space, without any other consideration (like obstacles). So the TCP coordinates are relevant and the robot geometry as "added" to the obstacles to find the bounds of the configurations space. In theory, this can be applied also in operational space. There is no mathematical necessity to bind configuration space to joint space it is just in most cases done like that.

To conclude:

operational space is similar to Cartesian space (with added rotational dimensions) tasks space may be the same as operational space, or it may be derived exclusively from the task. Joint space is the space of joint coordinates. Configurations space is the joint space without the invalid poses, but nothing stops it from being the same in operation space. Workspace in operational space is all the points the robot can reach, workspace in joint space is all the points the robot can reach. If there are no obstacles in the workspace joint space workspace is the same as configurations space, if there are obstacles, the configuration space is a subset of the joint space workspace. Configuration space is mostly defined in joint space, workspace is mostly defined in joint space. The way form transforming from joint space to operational space is the forward kinematics problem, from operational space to joint space you need the inverse kinematics problem. You need to know all obstacles to define the configurations space (but you usually just sample and compute on the fly, not a priory)


问:
Cons and pros of wireless technologies for rescue robot
robotics enthusiasts!

I'm a member of a team which has to develop a mobile rescue robot to cooperate with firemen (e.g. on earthquake sites).

The problem we have is connection of a commander post with the robot. The robot has to enter buildings, so it is desirable that the connection can go through several decimeters of walls and have a reach 50-100 meters. On the other hand, we need to send a lot of data (camera images, point clouds, maps) which can easily eat 10 Mbps or more.

At this time we use 2.4 GHz WiFi for this connection. As for the speed, with direct visibility it seems to be sufficient, but only when a single robot is operating (we can use up to 3 non-overlapping channels, so in theory 3 robots can work together; but usually you have the environment messed up with home routers). We need at least 5 robots to be operating simultaneousely.

We have tried 5 GHz WiFi, but it has problems with penetrating walls, so it can only be used for UAVs.

My idea was to use some mobile connection technology like LTE. I found that LTE can be run on 800 MHz, which could be great for the wall penetration performance. I also found that the LTE's theoretical upload speeds (for clients) are 70 Mbps, but nobody says if it is on 2.6 GHz and how would it be changed when running LTE on 800 MHz.

Moreover, we cannot rely on some provider's coverage. I have found that you can build your own LTE transmitter for about €2000, which seems to be interesting to us. Maybe it is possible to build it even cheaper. But we think both 2.6 GHz and 800 MHz are regulated frequencies. However, the cooperation with firefighters could persuade local regulators to give an exception to us to setup our own small LTE base station.

And now to the question: do you think such setup would give better results than using WiFi? Or do you know of any other technologies that would help us to either increase the bandwidth or the wall penetration performance? What are their cons and pros?

答:
According to this report, there is a fairly predictable measurement of penetration loss in buildings as a function of radio frequency:

Based on that, here's a chart I made that lists the wireless technologies found in Integration of ICT in Smart Organizations (specifically, the chart on page 207):

You will probably want a technology toward the upper-right of the chart, but the decision to favor greater speed or greater signal penetration will be yours to make.

Another useful resource might be the spectral efficiency comparison table on wikipedia, which gives a more basic value of "kbps per MHz".

ContentLicense: CC BY-SA 3.0


问:
Denavit Hartenberg parameters for 3R spatial manipulator - excercise
I'm learning about robotics and comparing two ways to calculate homogeneous transformation matrix. I'm comparing PoE (product of exponentials) and Denavit-Hartenberg parameters. For the same base frame and same position and orientation of the final (end effector frame) these two ways should produce the same result (i.e. coordinate transformation between base - fixed frame and tool's frame).
However, I'm not getting the same result and this is because I don't use D-H parameters correctly. I'm pretty sure the error is the third row of D-H table. Mutual positions of the joints is in such way, I cannot determine the axis correctly.

Here is what I did so far. Any help or advice is greatly appreciated.

However, if I do step by step calculation in Matlab, I got correct result

The code is:

```<code>L1 = 0.4; L2 = 0.3; th1 = pi/4; th2 = pi/3; th3 = pi/6;
%step by step Tms
% rotate around z by th1
T1m = rotm2tform(rotz(th1*180/pi));
%translate on x axis by L1
T21m = [1 0 0 L1;0 1 0 0;0 0 1 0; 0 0 0 1];
T22m =  rotm2tform(rotx(90)) * rotm2tform(rotz(th2*180/pi));
% rotate around x by -90°
T31m= rotm2tform(rotz(-90))
% translate on x axis by L2
T32m = [1 0 0 L2;0 1 0 0;0 0 1 0; 0 0 0 1];
%rotate around x by -90°then rotate around z by th3
T33m = rotm2tform(rotx(-90)) * rotm2tform(rotz(th3*180/pi));
%calculate final transf matrix
Tms = T1m*T21m*T22m*T31m*T32m*T33m
</code>```

The correct result should be:

```<code>    0.1768   -0.9186    0.3536    0.4666
    0.8839    0.3062    0.3536    0.4666
   -0.4330    0.2500    0.8660   -0.1500
         0         0         0    1.0000
</code>```

Thank you in advance.

答:
For PoE, the zero-configuration matrix and the screw axes in the spatial frame are
$$
\begin{align}
M&=\begin{bmatrix} 0&0&1&L_1\\0&1&0&0\\-1&0&0&-L_2\\0&0&0&1\end{bmatrix} \\
\mathcal{S}_1&=\begin{bmatrix} 0&0&1&0&0&0\end{bmatrix}^T \\
\mathcal{S}_2&=\begin{bmatrix} 0&-1&0&0&0&-L_1\end{bmatrix}^T \\
\mathcal{S}_3&=\begin{bmatrix} 1&0&0&0&-L_2&0\end{bmatrix}^T. \\
e^{[\mathcal{S}_1]\theta_1} &=
\begin{bmatrix}
e^{[\omega_1]\theta_1} & \Big[I\theta_1+(1-c_1)[\omega_1]+ (\theta_1-s_1)[\omega_1]^2\Big] v\\0 &1
\end{bmatrix}= \begin{bmatrix} c_1&-s_1&0&0\\
s_1&c_1&0&0\\
0&0&1&0\\
0&0&0&1
\end{bmatrix} \\
e^{[\mathcal{S}_2]\theta_2} &=
\begin{bmatrix}
e^{[\omega_2]\theta_2} & \Big[I\theta_2+(1-c_2)[\omega_2]+ (\theta_2-s_2)[\omega_2]^2\Big] v\\0 &1
\end{bmatrix}= \begin{bmatrix}
c_2&0&-s_2&-L_1(c_2-1)\\
0&1&0&0\\
s_2&0&c_2&-L_2s_2\\
0&0&0&1
\end{bmatrix} \\
e^{[\mathcal{S}_3]\theta_3} &=
\begin{bmatrix}
e^{[\omega_3]\theta_3} & \Big[I\theta_3+(1-c_3)[\omega_3]+ (\theta_3-s_3)[\omega_3]^2\Big] v\\0 &1
\end{bmatrix}= \begin{bmatrix}
1&0&0&0\\
0&c_3&-s_3&-L_2(c_3-1)\\
0&s_3&c_3&-L_2s_3\\
0&0&0&1
\end{bmatrix}\\
T^0_3 &=e^{[\omega_1]\theta_1}e^{[\omega_2]\theta_2}e^{[\omega_3]\theta_3}M
\end{align}
$$
The result is

For DH, the table is
$$
\begin{array}{cccc} \hline
\text{Link} & a_i & \alpha_i & d_i & \theta_i \\ \hline
1 & L_1 & 90^\circ & a_1 & \theta_1 \\ \hline
2 & L_2 & -90^\circ & 0 & \theta_2-90^\circ \\ \hline
3 & 0 & 0 & 0  & \theta_3 \\ \hline
\end{array}
$$

The transofmration matrix is
$$
T^0_3 = T^0_1T^1_2T^2_3
$$
The result is

They are identical.


问:
6D pose estimation of a known 3D CAD object
I'm looking for a codebase for 6DOF pose estimation of a known 3D CAD object with RGB or RGBD. It must be:

-Usable commercially (licensed under BSD, MIT, BOOST, etc.), not GPL.

-Easy to setup and use (having a running colab example would be great)

-The training time required for a new CAD object should be on the order of hours, not days.

-State of the art of near state of the art results. (See https://bop.felk.cvut.cz/home/ for benchmarks)

Are there any libraries fitting these requirements?

答:
I would like to know about this too.
Vuforia combines libraries for several platforms (Android, iOS) and provides pose estimation. It uses 2D feature extracted from CAD and the feature extraction is fairly quick (minutes, not hours), but it is not CAD based pose estimation and the results are average (drift, stability, accuracy).
I came across this:
http://track.virnect.com/1.2.1/
Not tested</p>


问:
Where do sensor models come from?
i'm new to robotics. I understood why we talk about probabilities and probabilistic robots, but i don't understand where those probabilities come from, how do i go about building a sensor model? Does the robot use the sensor model to then discard any measurement?</p>

答:
Sensor models are useful in a lot of contexts

When we simulate our system, we want to simulate the noise properties of the sensor in order to get a feeling for the impact of measurement noise on the control signal etc. If the sensor has a certain probability of dropping a measurement, we want to know the impact this has on our closed-loop performance.

When we do control design, we want to both make sure that we do not amplify noise too much, but we also want to make sure that the feedback loop is robust and has adequate gain and phase margins. A sensor typically has limited bandwidth, i.e., it acts as a lowpass filter on the true signal to be measured. The lowpass filter property introduces phase lag that can be detrimental to the control performance.

You may get a sensor model in many different ways depending on the type of sensor model you're after.

Statistical models of the noise may, for instance, be built by recording data in a scenario where the correct measurement is known. Consider an accelerometer that does not move at all, this will allow you to gather some data and determine the distribution of the measurement errors.

Dynamical models of sensors may sometimes be obtained from datasheets, i.e., the datasheet may list a bandwidth property that indicates that the sensor acts like a lowpass filter and can not reliably measure signals that vary faster than the bandwidth. This type of model can also be obtained by comparing measurements with a sensor that is known to be of higher performance. For some sensors, you may know the mechanism with which it works and can model this. A sensor that measures rotational speed may, for instance, work by measuring the time it takes for the sensor to turn 1/n revolutions for some integer n. You will thus typically get good readings for large velocities, but if n is small, get terrible resolution for low velocities. Some sensors + digital converters have a low resolution, in which case you may have to model quantization effects as well. If the sensor requires some data processing, there might be time delays introduced that must be modeled etc.

Static models of sensors could perhaps also be considered, e.g., including constant offsets or biases that need to be found and compensated for. This procedure is often called calibration. In the accelerometer case, one may need to find the orientation of the (3DOF) accelerometer w.r.t. the field of gravity etc.

Title: Where do sensor models come from?


问:
Is it possible to use coordinates to modify the floor into a polygon in Webots?
I'm using Webots to model an irregular polygon floor for a coverage problem. I want to define the floor using the coordinates of the vertices.
So far I have been able to generate an irregular polygon with desired coordinates using polygons.wbt. But I'm not sure if this would function correctly as the "floor" or "ground" for the robot to go over.</p>

答:
Webots offers defaults of some object floors in the ~/projects/objects/floors/protos/ directory from your Webots HOME directory. These however don't give you the option to modify the floor on the basis of coordinates.

To do what you have asked, however, will require you to create a new PROTO file that you can do by playing around with the instructions as given here. Another stack user had a question regarding changing the coordinates of the origin of the floor and you may want to check that out too.


问:
Camera Calibration Matrix accuracy
When we do camera calibration, we have to find calibration matrix $M$, which is found by first finding extrinsic matrix and then intrinsic matrix. To validate the accuracy of calibration matrix, we compare groundtruth projections $points \times intrinsic-matrix$ against prediction $points \times extrinsic-matrix \times intrinsic-matrix$. To illustrate results, I plotted the figure below. Source

Question: Why the projections from $points \times intrinsic-matrix$ are regarded as the groundtruth please?

答:
The reason that it's the ground truth is because in the tutorial it's all synthetic data and it's using the ground truth parameters of the model to calculate/construct those values exactly.

First we define the necessary parameters and create the camera extrinsic matrix and intrinsic matrix. These are required to build the pipeline and prepare the ground truth.

This example is not how to do a camera calibration in the real world but is teaching you the fundamentals of how the calibration algorithms work, thus you can know the ground truth before you try to do the optimization so see the performance.

Title: Camera Calibration Matrix accuracy


问:
Can ROS run on a Raspberry Pi?
Can ROS run on a Raspberry Pi?

ROS is resigned to run on a network of machines, with different machines, even different cores on the same machine doing different jobs. Can one of those machines be a Raspberry Pi?

I am considering using an R-Pi as the EtherCAT master on a mobile robot, communicating with the main PC over WiFi, using a dongle.

Can an R-Pi even run ROS at all?

Would an R-Pi have enough processing power to do some 1kHz servoing?

Would it be possible to run some servoing on the host through the WiFi connecion?

答:
This question was well-answered by ThomasH, but in addition I just want to suggest the possibility of wireless tethering the quadcopter to a laptop. That is, just write a nice-and-fast wireless (wifi?, bluetooth?) communication protocol for the quadcopter, then do the heavy CPU stuff on a laptop, while transmitting the instructions and sensor queries to the R-PI. We tried many implementations, and settled on a similar setup for all our small robots. Also almost every jaw-dropping quadcopter implementation is set up in this way. It makes life easy, and allows you to use the big-hitter libraries without sacrificing speed.

Let's be honest here, that quadcopter is very likely not going to get outside wireless range of your laptop anyway.

Title: Can ROS run on a Raspberry Pi?


问:
Line follower robot program
I am working on a line follower robot as part of my Microelectronics project, and am confused over what sort of code to use to program the "pic18f" microcontroller I'm using. Can someone give me source code or a layout of the code and what should be in there?</p>

答:
Here is the Basic source code you could start with. Then you can make it more complex:

```<code>#define INA_2   10   // This is the left forward control of the robot, I defined each pwm pin that is connected to the arduino
#define INB_2   11   // This is the left backward control of the motor, e.g set this high and INB_1 high and others low you will do a reverse
#define INA_1   5    // This is the right forward control of the motor
#define INB_1   6    // This is the right backward control of the motor

void setup() {              // This is always the 1st void function in an arduino program
    Serial.begin(9600);
    pinMode(INA_2, OUTPUT);      // each pwm pin was placed as an output for the program
    pinMode(INB_2, OUTPUT);
    pinMode(INA_1, OUTPUT);
    pinMode(INB_1, OUTPUT);
}

void loop() {      // This is a loop shows the sensor and the driver funtion
    sensor();
    driver();
}

void driver()       // This is the driver function, it reads the sensors and drives the motor using pwm signal
{
    int lowVal = 60;          // I did set my sensor low value to be 60 note this is not the lowest value the sensor can go
    int highVal = 300;        // I did set my sensor high value to be 300 note this is not the highest value the sensor can go

    int s0 = analogRead(A0);            // This defines the sensor value s0, it puts the reading of analogread(A0) into s0
    int s1 = analogRead(A1);
    int s2 = analogRead(A2);
    int s3 = analogRead(A3);
    int s4 = analogRead(A4);
    int s5 = analogRead(A5);

    ////////////////////////////////////////////////
    if ((s2 >= highVal) && (s3 >= highVal))    // This is the 1st if statement of the program, it says if s2 and s3 see's a black line robot should move straight at highest speed
    {
        analogWrite(INA_2, 255);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if ((s3 >= highVal) && (s4 >= highVal))
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 240);
        analogWrite(INB_1, 0);
    }
    else if ((s4 >= highVal) && (s5 >= highVal))
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
    else if((s1 >= highVal) && (s2 >= highVal))
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if ((s0 >= highVal) && (s1 >= highVal))
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 0);
        analogWrite(INB_1, 0);
    }
    else if(s0 >= highVal)
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 0);
        analogWrite(INB_1, 0);
    }
    else if(s1 >= highVal)
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 0);
        analogWrite(INB_1, 0);
    }
    else if(s2 >= highVal)
    {
        analogWrite(INA_2, 255);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if(s3 >= highVal)
    {
        analogWrite(INA_2, 255);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if(s4 >= highVal)
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
    else if(s5 >= highVal)
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
    else if((s0 <= lowVal) && (s1 <= lowVal) && (s2 <= lowVal) && (s3 <= lowVal) && (s4 <= lowVal) && (s5 <= lowVal)) // This says if its all white light the robot should make a 180 degree left turn
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 200);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
}

void sensor(){               // This function basically prints the sensor reading so you can see what sensors are working.
    int s0 = analogRead(A0);
    int s1 = analogRead(A1);
    int s2 = analogRead(A2);
    int s3 = analogRead(A3);
    int s4 = analogRead(A4);
    int s5 = analogRead(A5);

    Serial.print("s0 ");    // This prints out the reading for each sensor
    Serial.print(s0);
    Serial.print(" s1 ");
    Serial.print(s1);
    Serial.print(" s2 ");
    Serial.print(s2);
    Serial.print(" s3 ");
    Serial.print(s3);
    Serial.print(" s4 ");
    Serial.print(s4);
    Serial.print(" s5 ");
    Serial.print(s5);
    Serial.print(' ');
    Serial.println();
}
</code>```

Title: Line follower robot program

Body:
Here is the Basic source code you could start with. Then you can make it more complex:

#define INA_2   10   // This is the left forward control of the robot, I defined each pwm pin that is connected to the arduino
#define INB_2   11   // This is the left backward control of the motor, e.g set this high and INB_1 high and others low you will do a reverse
#define INA_1   5    // This is the right forward control of the motor
#define INB_1   6    // This is the right backward control of the motor

void setup() {              // This is always the 1st void function in an arduino program
    Serial.begin(9600);
    pinMode(INA_2, OUTPUT);      // each pwm pin was placed as an output for the program
    pinMode(INB_2, OUTPUT);
    pinMode(INA_1, OUTPUT);
    pinMode(INB_1, OUTPUT);
}

void loop() {      // This is a loop shows the sensor and the driver funtion
    sensor();
    driver();
}

void driver()       // This is the driver function, it reads the sensors and drives the motor using pwm signal
{
    int lowVal = 60;          // I did set my sensor low value to be 60 note this is not the lowest value the sensor can go
    int highVal = 300;        // I did set my sensor high value to be 300 note this is not the highest value the sensor can go

    int s0 = analogRead(A0);            // This defines the sensor value s0, it puts the reading of analogread(A0) into s0
    int s1 = analogRead(A1);
    int s2 = analogRead(A2);
    int s3 = analogRead(A3);
    int s4 = analogRead(A4);
    int s5 = analogRead(A5);

    ////////////////////////////////////////////////
    if ((s2 >= highVal) && (s3 >= highVal))    // This is the 1st if statement of the program, it says if s2 and s3 see's a black line robot should move straight at highest speed
    {
        analogWrite(INA_2, 255);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if ((s3 >= highVal) && (s4 >= highVal))
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 240);
        analogWrite(INB_1, 0);
    }
    else if ((s4 >= highVal) && (s5 >= highVal))
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
    else if((s1 >= highVal) && (s2 >= highVal))
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if ((s0 >= highVal) && (s1 >= highVal))
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 0);
        analogWrite(INB_1, 0);
    }
    else if(s0 >= highVal)
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 0);
        analogWrite(INB_1, 0);
    }
    else if(s1 >= highVal)
    {
        analogWrite(INA_2, 200);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 0);
        analogWrite(INB_1, 0);
    }
    else if(s2 >= highVal)
    {
        analogWrite(INA_2, 255);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if(s3 >= highVal)
    {
        analogWrite(INA_2, 255);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 255);
        analogWrite(INB_1, 0);
    }
    else if(s4 >= highVal)
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
    else if(s5 >= highVal)
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 0);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
    else if((s0 <= lowVal) && (s1 <= lowVal) && (s2 <= lowVal) && (s3 <= lowVal) && (s4 <= lowVal) && (s5 <= lowVal)) // This says if its all white light the robot should make a 180 degree left turn
    {
        analogWrite(INA_2, 0);
        analogWrite(INB_2, 200);
        analogWrite(INA_1, 200);
        analogWrite(INB_1, 0);
    }
}

void sensor(){               // This function basically prints the sensor reading so you can see what sensors are working.
    int s0 = analogRead(A0);
    int s1 = analogRead(A1);
    int s2 = analogRead(A2);
    int s3 = analogRead(A3);
    int s4 = analogRead(A4);
    int s5 = analogRead(A5);

    Serial.print("s0 ");    // This prints out the reading for each sensor
    Serial.print(s0);
    Serial.print(" s1 ");
    Serial.print(s1);
    Serial.print(" s2 ");
    Serial.print(s2);
    Serial.print(" s3 ");
    Serial.print(s3);
    Serial.print(" s4 ");
    Serial.print(s4);
    Serial.print(" s5 ");
    Serial.print(s5);
    Serial.print(' ');
    Serial.println();
}


问:
Redundancy and Null space projection
I'm reading "Theory of Robot Control" by Carlos Canudas Wit, Bruno Siciliano, Georges Bastin. In Task space control chapter, exploiting the redundancy by means of null space as shown below is not clear to me. What is $\dot{q}_0$ in the below picture? I do understand the null space of a matrix though. For robot, the null space of Jacobian is the set of joints' velocities vectors that yield zero linear and angular velocities of the end-effector but I'm not able to understand the idea of projecting such vectors. Any further suggestion how to understand this topic?

答:
If you multiply both members of the equation (3.9) by $J$, you'll get:
$$
J\dot{q} = JJ^+v + \left( J -JJ^+J \right)\dot{q_0}.
$$

Then, we can exploit that $JJ^+=I$, obtaining:
$$
J\dot{q}=v,
$$
which is the well-known forward differential kinematics law.

Until now, we didn't specify anything about $\dot{q_0}$, which indeed can be whatever vector of the same size of $\dot{q}$, meaning we can plug whatever $\dot{q_0}$ into (3.9) that the fundamental differential law will be still guaranteed. Thus, the resulting velocities $\dot{q}$ complies with the law and at the same time will be the closest to $\dot{q_0}$ in the least-squares sense.

Further, the equation (3.9) is clearly made up of two contributions:

$J^+v$ that is the primary task as defined by $v$.

$\left( I -J^+J \right)\dot{q_0}$ that is the secondary task as defined by $\dot{q_0}$.

Because $\dot{q_0}$ can be anything, we say that the secondary task does not interfere with the primary task thanks to the projector $\left( I -J^+J \right)$.

Therefore, the flexibility offered by the redundancy of the manipulator at hand allows us to employ $\dot{q_0}$ for achieving a supplementary goal (i.e., the secondary task).

Usual secondary tasks are implemented to stay away from the joint bounds or to improve manipulability.

To this end, one can thus establish the relation:
$$
\dot{q_0} = k_0 \frac{\partial w(q)}{\partial q},
$$
where $w(q)$ is some sort of function of the joints $q$ that we aim to minimize/maximize through its gradient.

For example, if we want to stay away from the joint limits while converging to the primary target defined by $v$, we could set:
$$
w(q) = -\frac{1}{2n}\sum^n_{i=1}\left( \frac{q_i-\bar{q_i}}{q_{iM}-q_{im}} \right)^2,
$$
with $q_{iM}$, $q_{im}$ the upper and lower bounds of joint $i$, and $\bar{q_i} = \frac{q_{iM}+q_{im}}{2}$.

Just a concluding remark about the following statement:

For robot, the null space of Jacobian is the set of joints' velocities vectors that yield zero linear and angular velocities of the end-effector

This is often the case but it's not 100% true. It very depends on the primary task, which in turn dictates the form of the Jacobian $J$.

If the primary task aims to get full control of the tip frame (i.e., linear and angular velocity), then the statement above holds outright.

However, if our primary task deals only with the position of the tip frame, for example, and not its orientation ($J$ is defined in $\mathbb{R}^{3 \times n}$), then the secondary task may impact the orientation of the end-effector by applying to it nonnull angular velocities.

Title: Redundancy and Null space projection


问:
Difference between motion-only bundle adjustment and pose-graph optimization
I've been trying to figure out the difference between motion-only bundle adjustment and pose-graph optimization, when talking about systems that only use cameras to estimate motion between frames, but have so far had no luck finding a solid explanation explaining the differences between these two concepts.

Motion-only BA optimizes over camera poses, and treats triangulated landmark positions as being constant, where the error function is the reprojection error between triangulated 3D landmarks and their corresponding 2D feature points on the camera/image plane (e.g. a nice explanation of motion-only BA can be found on page 4 of the ORB-SLAM2 paper).

However, pose-graph optimization seems extremely similar, where again camera poses are optimized and triangulated landmark positions are treated as being constant during optimization. But, instead of having a reprojection-based error function, the error function for pose-graph optimization instead seems to be the difference between relative poses (e.g. a nice explanation of pose-graph optimization can be found on page 9 of the tutorial paper on visual odometry by Fraundorfer and Scaramuzza).

However, for vision-based systems it seems like the difference between relative poses has to be obtained using some type of reprojection-based error function (assuming the use of sparse features, and not dense tracking). As such, are motion-only BA and pose-graph optimization two names for the same thing, or are there more nuanced differences between the two?

答:
After going over ORB-SLAM 1, 2 and 3 paper, I think there may be a slight difference between what motion-only BA and local BA and, pose graph optimization are meaning. I think, the consideration should be given between motion-BA and local-BA, both using keyframes in constructing a directed graph and then optimizing the values of the nodes by nudging the values of edges (thats how I understand graph optimization in general).

But what motion-only BA is doing is refining the current pose estimation from the VO module with last known keyframe pose (which should have been optimized by local-BA). On the other hand, the local-BA is taking a bunch of keyframes within a certain distance from the current camera pose and optimizing those on a separate thread. Buth both are computing reprojection error to minimize the objective fuction. Thus my two cent on the question is that motion-only BA and local-BA are pose graph optimization where the number of keyframes (and map points) considered differes in an attempt to balance accuracy with speed


问:
How to perform time-offset calibration use GTSAM in Camera-IMU system?
Assume we have camera poses data and IMU data(which include gyr and acc data) with different timestamps, now I want to estimate this value, I known that this can be done in many ways(EKF,MSCKF), but I wanna to do this use factor graph model, how can we model this problem as an factor graph model and do optimization?</p>

答:
There is no off the shelf implementation you can use for computing the temporal offset in GTSAM. You will have to implement the algorithms yourself. For the specifics of the algorithm I know 3 different ways.

One is you can implement the same equations as implemented in the EKF/MSCKF framework. Factor graphs and Extended Kalman Filters actually solve the problem in the exact same way(just factor graphs are iterative and are in the Information form). Only problem with this approach is that it is inefficient due to the needing to relinearize multiple times.

For factor graphs to be more efficient with IMU processing you typically use an algorithm called IMU preintegration. This can be modified to also estimate the temporal offset as seen in this paper.

Final option is to implement the algorithm from VINS Mono. It works a little bit different then the above mentioned approaches in that it estimates a velocity for each landmark. Again you would have to implement this algorithm yourself, but it has the advantage that the code is open source and so it can be easier to code up.


问:
Servo motor torque
I am confused with the torque ratings of servo motors, or I guess it is general while choosing any motor.

For example a servo's torque is rated as 10kg-cm, so does that mean it can handle a 100N force from 1 cm distance to its center or 50N force from 2cm distance?

Am i interpreting it right?

If it is right, then consider a motor mounted to the wall from its back and a rope is wrapped to its tip(the rotating part, i don't what it is called); then how should I calculate the maximum weight the motor will be able to lift upwards?

答:
Your comment is correct. However this value is max. torque value not nominal value.

If your engine is 10 kg-cm, we can simply say:

$$F = m * a$$

$$F = 10 kg * 9.80665 m/s^2$$

$$F = 98.0665 N$$

This value indicates the maximum force you can apply.

If we examine the torque and forces simply:

$$τ = r * F$$

If the distance increases, the force value to be applied decreases, so the load on the motor decreases. It is theoretically true that there is a linear relationship here. In practice, it tries to approach this line, but this is directly related to your engine quality.

In general, it would be more correct to give the following answer. You can also tow a load that you connect directly to the shaft of the motor. However, it may not be correct to use it at maximum torque for this. It doesn't mean it don't work, but the longer you stay at maximum torque, the more you can damage the motor itself or the gears. Therefore, I think that staying 10 percent below the specified value is sufficient as an operating range. Of course, we cannot verify this without the engine's datasheet.

Title: Servo motor torque


问:
Can you increase resolution/accuracy by mechanically averaging two actuators?
I'm working on a project using hobby servos to drive a revolute joint, and the servo mechanism doesn't seem to respond when I command a motion of less than about 1 degree. I want to increase the resolution without sacrificing range (otherwise I would just gear them down).

I've read that "parallel" robots such as delta-style 3D printers or pick-and-place machines are generally much more accurate than "serial" robots like a typical robot arm. Is there a linkage design which can combine the motion of two servos into a single "better" rotational actuator? The output would still just have one degree of freedom, but increased resolution compared with a direct coupling to one servo.

First I thought maybe you could reduce the motors by 2:1 and then add their outputs somehow, but it seems like this would also add the positional errors together, and you wouldn't gain anything:

Then I thought about it a bit more and came up with this arrangement, with a parallel linkage between the two motors fixed at a point between them.

I have no idea if this would actually improve my resolution though. Would I need to command different positions to each motor? e.g. if I want to achieve 90.5 degrees, would I command 90.0 to one motor and 91.0 to the second motor? Or is there a better way to approach this? (I realize I could just buy more expensive servos, but I'm trying to work with what I already have).

答:
Your intuition is right,  parallel mechanisms will improve precision, in general, when compared to serial mechanisms, but with a number of caveats:

Parallel mechanisms (as opposed to serial, chain like mechanism) have this aspect of averaging out positional errors, but this aspect is not constant throughout the workspace. It is more accentuated in the middle of the workspace and tends to fade out the closer the TCP is to the margins of the workspace.

You have to be more careful about the kinematic design. For a serial chain the influence of one small motion in one motor on the TCP is much more intuitive then in the case of a parallel mechanism.

The workspace of a parallel mechanism is much smaller then the workspace of a parallel mechanism.

Parallel mechanisms improve the precision because of their geometrical (kinematic) model (e.g. a small motion at the motor level causes a smaller motion at the TCP level then in the case of the serial version and the motion produced is in more dimensions then in the case of a serial version, so the error gets less amplified and is spread out among more dimensions), not because they are overconstrained and one tries to pull/push the other motor.

I am not sure how your mechanism could/should function, but is it essential to create a kinematic model (or maybe a simulation) to better understand how it works, how many degrees of freedom it has and how to map TCP motion to the motor level.


问:
Classical DH and Euler angles parametrization
Let DH convention is carried out as
$$
A_i = \text{Rot}_{z,\theta_i}\text{Trans}_{z,d_i}\text{Trans}_{x,a_i}\text{Rot}_{x,\alpha_i}
$$
This is the classical DH method stated in Spong's book. The homogenous transformation matrix $T^0_n=A_1 A_2\cdots A_n$ is
$$
T^0_n= \begin{bmatrix} R^0_n &p^0_n \\ 0&1\end{bmatrix}\in \text{SE}(3)
$$

The question is what is the Euler parameterization convention embedded  in the rotation matrix $R^0_n$ (i.e.  ZYZ,XYZ, ...,etc)? In the Spong's book, does the following occur accidentally? I'm asking this because I need to derive the analytic Jacobian for the angular part (i.e. $J_w$) which requires which Euler angles parameterization is being used.

答:
It appears that $R^{3}_{6}$ in equation 3.15 likely arises from the fact that the specific robot in the example has a "spherical wrist" structure. Considering joints $n-2$, $n-1$, and $n$, this structure occurs when joints $n-2$ and $n$ have colinear axes of rotation when joint $n-1$ is at some angle $\theta$ (this is typically the case at $\theta = 0$). Pictorially, this looks like:

We can see that the rotation of $\beta$ by another $\frac{\pi}{2}$ radians would align the first and third joints' rotational axes.

So, to answer your question the given excerpts from the Spong book do not happen accidentally - rather they occur purposefully. The kinematic structure of the spherical wrist is essentially a one-to-one mapping to the ZYZ euler angles. From a physical perspective, consider the fact that $\beta$ in the above picture is along the Y-axis of joint 1 and consider the case where joint 1, joint 2, and joint 3 all start at angles of $0$ (the end-effector pointing straight up). Thus, the end-effector's current Z axis will be aligned with joint 1. Any rotation along joint 1 will not disassociate the joint 2 axis of rotation with the current Y axis of the end-effector. Thus, any rotation of joint 2 will be equivalent to a current Y rotation. Finally, the formulation of the spherical wrist is such that the final joint always rotates along the current Z axis of the end-effector. Thus, joint 3 is equivalent to the final current Z rotation of the end-effector.

As a final set of notes - there are many, many different ways to parameterize orientation. Euler angles have the benefit that they can fully describe orientation, but in some cases they can suffer from gimbal lock as well as essentially being a 2-to-1 mapping to orientation space (in many cases there are two distinct Euler angle representations for any end-effector orientation). Under some constraints Euler angles can be made to be a 1-to-1 mapping, but some care must be taken. Finally, as far as I know, any choice of Euler angles can be used to compute the analytic Jacobian. In the example given in Spong chapter 4, it appears that if you use ZYZ Euler angles you need only follow equations 4.86 and 4.88 (and this should work for any robot structure, you will just need a way to convert the orientation portion of the end-effector's transformation matrix to Euler angles).


问:
End effector rotates 180 degrees when trying to set orientation with direction vector
I want to orient the robot endeffector using a direction vector that I calculate myself. The robot will always start from the same position with the endeffector pointing straight down. I already have a function that calculates a direction vector that is in the orientation that I want the endeffector to be. I have used the code in this answer to calculate the quaternion from the starting orientation to the goal orientation: https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another. For some example orientation the quaternion that the code calculates is:

```<code>0.38268343 0 0 0.92387953
</code>```

and the correct quaternion that I want is:

```<code>0 0.92387953 0.38268343 0
</code>```

Unfortunately, when I pass the data on to the robot the robot has the right orientation but spins it's endeffector 180 degrees. My goal is to keep the endeffector from spinning and just rotate around it self in the angle that I want it to. I have multiple questions about this:

What is the reason for the rotation around itself?

How would I make sure the robot only moves to the correct orientation but does not actually change the way the end effector is facing?

How can I control the rotation to make the endeffector rotate at an angle that I want? For example: 20 degrees clock or counter clockwise.

答: When you convert the physical motions to mathematical symbolism, you apply mathematical assumptions about the results.  For instance, does your algorithm’s acos function restrict the results to [0, 2$\pi$], or to [-$\pi$, +$\pi$]?   You can always normalize the motions by adding $\pm 2 \pi$ to the solutions, but you need to be careful not to cause windup of the axes (particularly the cables driving the axes).  You could implement a “nearest solution” approach to minimize the joint rotations in these configurations which are near to the wrist singularity.


问:
How to prove expression for relative pose in one frame given relative pose in another frame
Given the poseo of point 1 between $t_k$ and $t_{k+1}$ \begin{equation}
\mathbf{P}_{1}\left(t_{k}, t_{k+1}\right)=\left[\begin{array}{cc}
\mathbf{R}_{1}\left(t_{k}, t_{k+1}\right) & \mathbf{T}_{1}\left(t_{k}, t_{k+1}\right) \\
0 & 1
\end{array}\right] .
\end{equation}
and the relative pose between point 1 and 2, $\mathbf{P_{12}}$ why is the relative pose between $t_k$ and  $t_{k+1}$ in point 2 given as:

\begin{equation}
\mathbf{P}_{2}\left(t_{k}, t_{k+1}\right)=\mathbf{P}_{12} \mathbf{P}_{1}\left(t_{k}, t_{k+1}\right) \mathbf{P}_{12}^{-1}
\end{equation}.

Why is this expression true?

My attempt:
I have tried multiplying moth sides by $\mathbf{P}_{12}^{-1} = \mathbf{P}_{21}$.

I get:

$ \mathbf{P}_{21}\mathbf{P}_{2}\left(t_{k}, t_{k+1}\right)$ which in my opinion equals $\mathbf{P}_{1}\left(t_{k}, t_{k+1}\right)$ but instead according to the expression it equals  $\mathbf{P}_{1}\left(t_{k}, t_{k+1}\right) \mathbf{P}_{12}^{-1}$.

答:
Consider the graph below:

Starting from the point $2k+1$, we can count the transformations anticlockwise, ending up again at the same point; hence, the aggregate transformation shall be the identity matrix $I$.

In formula:

$$
P_{12}^{-1} \cdot P_1^{-1} \cdot P_{12} \cdot P_2 = I,
$$

from which you can easily derive the original equation.


问:
Why are GA applied for mobile robot path planning
I try to understand why most researchers are using GA for mobile robot path planning. In the Paper Path Planning for the Mobile Robot: A Review it shows its the most used. In the paper A Review of Optimization Algorithms for University Timetable Scheduling. They write that "GAs are particularly
applied to complex optimization problems, which are
challenges that have different parameters or characteristics that
need to be combined in search of the best solution and, at the
same time, cannot be represented mathematically"

答:
Well, mobile robot path planning is a complex optimization problem, a challenge that has different parameters or characteristics that need to be combined in search of the best solution and, at the same time, cannot be represented mathematically.

Seems to me that makes the genetic algorithm approach worth exploring for a given application, although I'd want to see the citation for it being "the most used". If you need to get into detail about the parameters, characteristics, and what to optimize, you're getting into Ben's book.


答:
That document claims

The results shows GA, PSO, APF, and ACO are the most used four
approaches to solve the path planning of mobile robot.

The survey shows GA (genetic algorithm), PSO (particle swarm
optimization algorithm), APF (artificial potential field), and ACO
(ant colony optimization algorithm) are the most used approaches to
solve the path planning of mobile robot

Which are both extremely suspect to me and given without evidence or a citation. Given the journal it is published in isn't even an engineering journal, let alone a robotics journal, I'm suspicious of anything written in that document that I don't already know is true. The author throws up a terrible straw man argument against search-based planners and suffocates other methods with only 1-3 sentences of lip service or ignores important methods like RRT and Navigation Functions.

I can say from personal experience at several robotics companies that I've never seen a GA algorithm being used for path planning across many domains of robotics. Its not to say they don't have their niche, but that niche is not the usual use-case. Traditional search and sampling based planners can yield high quality results in a fraction of the listed times in the GA papers and are widely seen across mobile, manipulation, and AUV navigation systems.

I believe this document is misleading and in many ways just factually incorrect, or at least inadequate.

Title: Why are GA applied for mobile robot path planning

Body:
I try to understand why most researchers are using GA for mobile robot path planning. In the Paper Path Planning for the Mobile Robot: A Review it shows its the most used. In the paper A Review of Optimization Algorithms for University Timetable Scheduling. They write that "GAs are particularly
applied to complex optimization problems, which are
challenges that have different parameters or characteristics that
need to be combined in search of the best solution and, at the
same time, cannot be represented mathematically"


问:
How to go about programming Robot Arm with 3D point
I have a 6 DOF robot arm that I am programming. I want to be able to move the arm's claw to a 3D point in space. In order to do that, it needs to know the best combination of movements that will take the arm to that position.

I am programming in Java, if anyone has any language specific suggestions.

The design of the claw is shown below:

My Research

The best existing code template have found is Processing.org's Reach example (https://processing.org/examples/reach2.html)

I have made a few attempts to adapt this code to my needs, but I have not been able to fully do so: (https://editor.p5js.org/codeLink100/full/EuZAnpQfz)

I like the example code because it moves the front of the tentacle (smallest components in my case) first, therefore saving the robot some energy.

However, there are a few things with that I will need to change before I can use it as my code:

Each segment needs to be a different length.

The segments should not cross thru each other.

The segments should only be allowed to go within a specific angle range.

The example does not consider the 3D rotation of the base motor.

The program should tell me if the claw cannot reach the point.

Update:

I understand that this problem is best solved using inverse kinematics.
Does anyone know any good tutorials for absolute beginners. I have looked around quite a bit and haven't found much.

答:
As far as I understand, you want to build a robot manipulation program by scratch. It will involve bit complex calculations but it is really good if you want to understand these transformations.

But if you do not want to start from zero, I would suggest to take a look at Robot Operating System (ROS/ROS2). It involve several useful packages e.g. MoveIt, which is exactly designed for robot arm manipulation fulfilling your listed requirements also.


问:
Actuator to control steam valve
I have a steam radiator at home and it has a valve similar to the picture below.

Please note that the valve doesn't have grooves on top to attach things to.

I want to build something to turn it on and off depending on the temperature at certain points in the room.

I have that taken care of but cannot find a way to attach a actuator(actuator is the right word in the context I guess?) to turn the valve in both directions.

Also It is a rented apartment so I would like to avoid making any modifications to the radiator itself.

答:
If the valve has enough traction with a rubber band1, you can use one to control the valve as in the image below.

To make sure the rubber band doesn't start falling over time due to gravity, you could attach a half-circular plate under the valve, so the rubber band wouldn't slowly slide down as it would rest on that plate. Note that the amount of the pressure on the plate should be very small, since the rubber band is quite stretched to provide enough stretch for rotation, so a not so strong glue might do the trick for that plate.

1 This both depends on the material it's made of and quite importantly, how much torque is required to turn the valve.

Title: Actuator to control steam valve


问:
How to use a EPM50S8 encoder?
How can I use this encoder with my robot applications?

I couldn't find any tutorial about that and it has a lot of cables. (EPM50S8)

I have read the datasheet but I don't understand the last three descriptions. What are these pins connected to on an Arduino?

答:
If you read the datasheet, you will see a number of example control I/O circuits.

For example:

Title: How to use a EPM50S8 encoder?


问:
Spawn robot model using rospy in Gazebo
I am trying to spawn my robot model using rospy in my python interface to Gazebo.
In the launch file, I spawn the robot using:

```<code><node
name="urdf_spawner"
pkg="gazebo_ros"
type="spawn_model"
respawn="false"
output="screen"
ns="<span class="math-container">$(arg robot_name)"
args="-urdf -model $</span>(arg robot_model) -param robot_description -z 1.015"/>
</code>```

What would be the python way of doing the same? I'd be happy to share more details if needed. Thanks in advance!

答:
I haven't tried it myself but found something similar. Give it a try and let me know as well if it works for you. Good luck!

This is the code that they used.

```<code>import roslaunch

package = 'rqt_gui'
executable = 'rqt_gui'
node = roslaunch.core.Node(package, executable)

launch = roslaunch.scriptapi.ROSLaunch()
launch.start()

process = launch.launch(node)
print(process.is_alive())
process.stop()
</code>```

In this, they used rqt_gui but not gazebo. I have a feeling that you need to use OS module to run gazebo using python but not sure how.

Title: Spawn robot model using rospy in Gazebo

Body:
I haven't tried it myself but found something similar. Give it a try and let me know as well if it works for you. Good luck!

This is the code that they used.

import roslaunch

package = 'rqt_gui'
executable = 'rqt_gui'
node = roslaunch.core.Node(package, executable)

launch = roslaunch.scriptapi.ROSLaunch()
launch.start()

process = launch.launch(node)
print(process.is_alive())
process.stop()

In this, they used rqt_gui but not gazebo. I have a feeling that you need to use OS module to run gazebo using python but not sure how.


问:
Husky robot shows rostopics but not message over them
I have a Clearpath husky robot setup with their ros noetic image (ubuntu 20.04 on amd64 architecture) clearpath-universal-noetic-amd64-0.5.14.iso which to my surprise did not come with ros and other packages pre-installed. So, I installed them (husky_base, husky_bringup, husky_control, husky_description, husky_gazebo, husky_msgs, husky_viz) manually. I followed the instructions on http://www.clearpathrobotics.com/assets/guides/noetic/husky/InstallHuskySoftware.html#installing-husky-software till the command sudo journalctl -u ros. rostopic list gives me the below output.

```<code>administrator@husky:~$ rostopic list
/cmd_vel
/diagnostics
/diagnostics_agg
/diagnostics_toplevel_state
/e_stop
/husky_velocity_controller/cmd_vel
/husky_velocity_controller/odom
/imu/data
/joint_states
/joy_teleop/cmd_vel
/joy_teleop/joy
/joy_teleop/joy/set_feedback
/odometry/filtered
/rosout
/rosout_agg
/set_pose
/tf
/tf_static
/twist_marker_server/cmd_vel
/twist_marker_server/feedback
/twist_marker_server/update
/twist_marker_server/update_full
</code>```

But I don't see any message over them as there is no publisher.

```<code>administrator@husky:~$ rostopic info /husky_velocity_controller/odom
Type: nav_msgs/Odometry

Publishers: None

Subscribers:
 * /ekf_localization (http://husky:42525/)
</code>```

For the same robot, I have ROS melodic on ubuntu 18.04 (trimmed down version supplied by clearpath) which can echo the same topics easily. The publisher for same topic in the working setup is husky_node. I think this is the issue over here but don't know how to fix it. My COMM light for noetic setup is RED and for melodic is GREEN.

```<code>administrator@husky:~$ rosnode list
/diagnostic_aggregator
/ekf_localization
/husky_node
/joy_teleop/joy_node
/joy_teleop/teleop_twist_joy
/robot_state_publisher
/rosout
/twist_marker_server
/twist_mux
</code>```

This is the output of rosrun rqt_robot_monitor rqt_robot_monitor for noetic setup.

答: Did you select the Husky option while installing the ISO?  Could you provide the output of sudo journalctl -u ros? (It wasn't include in the question).


问:
Imu orieantation data increase by oneself
I subscriber imu data and print it from my python script.I move imu sensor fast then stop the move but imu data still increasing by oneself. How can I slove this problem?</p>
```<code>imu orientation.z:  -0.332
imu orientation.z:  -0.333
imu orientation.z:  -0.308
imu orientation.z:  -0.324
imu orientation.z:  -0.315
imu orientation.z:  -0.422
imu orientation.z:  -0.373
imu orientation.z:  -0.396
imu orientation.z:  -0.344
imu orientation.z:  -0.378
imu orientation.z:  -0.349
imu orientation.z:  -0.367
imu orientation.z:  -0.404
imu orientation.z:  -0.376
imu orientation.z:  -0.35
imu orientation.z:  -0.326
imu orientation.z:  -0.304
imu orientation.z:  -0.39
imu orientation.z:  -0.567
imu orientation.z:  -0.555
imu orientation.z:  -0.65
imu orientation.z:  -0.64
imu orientation.z:  -0.621
imu orientation.z:  -0.621
imu orientation.z:  -0.621
imu orientation.z:  -0.621 ```
</code>```

答:
What you're observing is called drift. IMU's are integrating accelerations from the accelerometers and angular velocity from gyroscopes. However all sensors have noise and when that noise in the signal is not exactly balanced it's called bias. And as you integrate the noisy signals the bias results in drift.

Analog Devices has a decent overview: The Case of the Misguided Gyro

The drift rate is one of the primary performance metrics for an IMU. How Adafruit compares Gyros And here are a few examples of them evaluating them

And there are various ways to try to calibrate the bias to reduce it. Discussion of calibration


问:
Robot never goes straight
I am using 2 identical DC motors and a castor wheel. The motors are connected to L293D motor driver and are controlled by RPi.

The robot is not going straight. It veers off to the right. I am running both the motors at 100% PWM.

What I tried to correct the error:

I adjusted the PWM of the wheel going faster to 99%, but the robot just turns to the other side;

I adjusted the weight on the robot and the problem still persists.

I once tried to run the motor without any load. Is that the cause of this, as I was later told that, running a DC motor without any load damages them?

If that is not the cause, then please tell me how to solve this problem without using any sensors for controlling it.

答:
I faced the same problem: I used DC motors and L293D motor. Like everyone else said you need to use a sensor. I used MPU6050 gyroscope sensor, which is cheap and somewhat inaccurate but is good enough for driving straight. Implementing a sensor can be cheap and simple as shown on (which uses Arduino, but the same concept can be used on Raspberry Pi):

https://www.instructables.com/How-to-Make-a-Robot-Car-Drive-Straight-and-Turn-Ex/

I have also tried adjusting PWM to each motor, which does NOT work, since the surface/ground the car runs on changes.

Title: Robot never goes straight


问:
Making a tunnel follower robot with 2 IR side sensors and 1 ultrasonic sensor
We have a tunnel and the distance from the walls to the robot is about 30cm when the robot is centered, the tunnel curves to the right. The robot has an infrared sensor on its left and one on its right and has an ultrasonic sensor at its front (so basically its eyes). How can I make the robot turn appropriately while its moving so that it's centered in the tunnel. I just need an idea for this algorithm because I can't find any. All what the infrared sensors can do is measure the distance to the walls.</p>

答:
Ahmed talked about PID (proportional integral differential, which is very complicated) and bang-bang control (which is too simple and often over-corrects). I would recommend proportional control and an example of this is on:
https://www.instructables.com/How-to-Make-a-Robot-Car-Drive-Straight-and-Turn-Ex/
The website gives an example of a car trying to drive straight using a gyroscope on a flat surface, which is similar to your tunnel situation.

Proportional control is much simpler than PID, since you only need to change tune one variable, instead of three in PID. If you want a bit more complexity, you can try proportional integral control, which uses two variables.

Additionally, I am skeptical about the ultrasonic sensor, since the tunnel might form an echo chamber, creating a lot of noise interference.


问:
Making a robot go in a straight rate - sampling rate for motor PID
I'm trying to build a robot/rover based on Raspberry Pi 3
At the moment I'm straggling with a basic driving in a straight line.

I'm in a process of tuning my PID code andI'm not sure what sampling rate should I choose.

These are the specs of the motors:

Voltage: DC 6V

  Speed: 210 rpm

  Encoder motor end: 11 signals

  Rated voltage: DC 6V

  No-load speed: 210RPM 0.13A

  Max efficiency: 2.0kg.cm/170rpm/2.0W/0.60A

  Max power: 5.2kg.cm/110rpm/3.1W/1.10A

  Stall torque: 10kg.cm 3.2A

  Retarder reduction ratio: 1 : 34

  Hall resolution: hall x ratio 34.02 = 341.2PPR

When the robot moves at the desired speed I get 20 encoder signals if I take a sample every 100ms and I get 10 if I sample every 50ms.

My question is what is the reasonable frequency to collect the encoder counts?

At 50ms I get higher frequency, so my PID adjustments can occur more frequently, but I get only 10 signals, so the resolution seems to be a bit low.

At 100ms I get 20 ticks which should provide more consistent stats, but then maybe adjustments are not happening fast enough.

Is there any rule of thumb for choosing a sampling rate for PID when driving a robot rover?

答:
Instead of using encoders, have you considered using a gyroscope sensor (which is very cheap and simple), an example is on (where the video shows you just how excellent the MPU6050 gyroscope sensor worked):
https://www.instructables.com/How-to-Make-a-Robot-Car-Drive-Straight-and-Turn-Ex/

The reason I suggest this is because encoder has a few limitations:

assumes the robot is symmetrical

assumes the terrain is flat

As you found encoder gets a sample every 50ms, whereas MPU6050 can get one every 3ms.

Since MPU6050 can take so much more samples, you do not even need PID control. You can just use the simpler proportional control, which only requires you to tune one variable instead of three in PID.


问:
Get approach vector from Mano hand model
I'm trying to get approach vector/normal from Mano hand parameters that I get from my network. Because the hand parameters are dependent on how it picks an object, I am interested in getting the approach vector (or normal from the center of hand mesh).

I will appreciate any advice on how to get this or if there is a way we can use to get the vector from mano hand parameters.

答: What I ended up doing is taking average of normals from the vertices that represent the palm of the hand.</p>


答:
You can use a simple constant step controller that does not need tuning like a PID controller.  A step controller will simply start adding or subtracting a constant control value based on the input signal.

In this case the control signal involves steering.  If you are too close to the left wall, then start turning right; if you are too close to the right wall then start turning left.  What 'turning' means depends on your robot.  If you are using a differential drive configuration then turning means slowing one wheel down and speeding the other up; the turn will be in the direction of the slower wheel.  If you are using an car-like Ackermann steering, then just change the angle of the steering servo.

The input signal is the difference in distances to each wall (there would be a separate controller for the forward distance to stop the robot).  We use the difference because we just want to know which wall we are closer to (and so should move away from).  This handles the fact that the robot will often not be driving parallel to the walls, so the distance to the walls as measured by the sensor may be greater than the actual distance because the sensors are at an angle to the walls.

So if we use (left - right) distance, then a negative value indicates we are closer to the left wall, a positive value indicates we are closer to the right wall.  We can use some zero-tolerance to treat very small differences as zero.

If we are within the zero-tolerance, do nothing.  If we are closer to the left wall, then add a small constant (the step) to steering. If we are closer to the right wall then subtract that small constant from steering.  What that step value is and it's sign and how it's actually applied (how it's actuated) will depend on your robot.

Title: Making a tunnel follower robot with 2 IR side sensors and 1 ultrasonic sensor

Body: We have a tunnel and the distance from the walls to the robot is about 30cm when the robot is centered, the tunnel curves to the right. The robot has an infrared sensor on its left and one on its right and has an ultrasonic sensor at its front (so basically its eyes). How can I make the robot turn appropriately while its moving so that it's centered in the tunnel. I just need an idea for this algorithm because I can't find any. All what the infrared sensors can do is measure the distance to the walls.</p>


问:
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!

答:
This is a classic precision vs. accuracy problem.

I think your intuition is correct:  resolution of 0.1-0.2N doesn't buy you much if the accuracy is only 10N.

I would model both the resolution and accuracy as independent sources of error:

Sensor accuracy introduces uniformly distributed random errors between -10N and +10N

Sensor resolution introduces uniformly distributed random errors between -0.2N and +0.2N (using the worst case sensor)

Assume that accuracy and resolution errors are independent

 Using just one sensor

Your sensor reading can be expressed:

$$\hat{y}=y + \epsilon_r + \epsilon_a \doteq y + \epsilon$$

where $y$ is the true value (force, torque, whatever), $\hat{y}$ is the corrupted observation of $y$ that your sensor permits, and $\epsilon_r$ and $\epsilon_a$ are the errors introduced by sensor resolution and accuracy, respectively.

Assuming that the sensor errors are unbiased (i.e. no constant offset), then the expected value of the total error $\epsilon$ is zero and the error variance is given by:

$$ \sigma_\epsilon^2 = \sigma_{\epsilon_r}^2 +\sigma_{\epsilon_a}^2$$

The variance of a uniformly distributed random variable $x$ whose values are bounded by limits $a$ and $b$ is given by:

$$\sigma_x^2 = \frac{1}{12}(b-a)^2$$

Substituting the above limits (using the sensor with poorer resolution:

$$\sigma_{\epsilon_r}^2 = \frac{1}{12}(0.2-(-0.2))^2 = 0.01\bar{3}$$

$$\sigma_{\epsilon_e}^2 = \frac{1}{12}(10-(-10))^2 = 33.\bar{3}$$

$$ \sigma_\epsilon^2 = \sigma_{\epsilon_r}^2 +\sigma_{\epsilon_a}^2 = 33.34\bar{6}$$

So that the standard deviation of the sensor errors is:

$$\sigma_\epsilon = 5.774657\mbox{N}$$

Using the "improved" sensor with higher resolution doesn't have much impact, since the error is dominated by sensor accuracy:

$$\sigma_{\epsilon_r}^2 = \frac{1}{12}(0.1-(-0.1))^2 = 0.00\bar{3}$$

$$ \sigma_\epsilon^2 = \sigma_{\epsilon_r}^2 +\sigma_{\epsilon_a}^2 =
33.33\bar{6}$$

$$\sigma_\epsilon = 5.773791\mbox{N}$$

So whatever premium might be charged for the higher precision sensor probably isn't worth it.

There are quite a few things above that I glossed over (e.g. Are resolution and accuracy errors really uniformly distributed? What if there is a bias?) but maybe the modeling will give you some new ideas.

 When 2 different sensors are available at the same time

If you are averaging the reading from the 2 sensors then things get more interesting.  Now

$$\hat{y} = \frac{1}{2}\left(\hat{y}_1 + \hat{y}_2\right)
=
\frac{1}{2}\left[\left(y + \epsilon_1\right) + \left(y + \epsilon_2\right)\right]
$$

Here again $\mbox{E}[\hat{y}] = y$, but the variance of the overall error is reduced:

$$\sigma_\epsilon^2 = \frac{1}{4}\left(\sigma_{\epsilon_1}^2 +\sigma_{\epsilon_2}^2\right)
=\frac{1}{4}\left(33.34\bar{6}+33.33\bar{6}\right) = 16.66771$$

so that

$$\sigma_\epsilon = 4.08261$$

Having 2 independent sensors and averaging their measurements reduced the potential error of the force measurement by 1.691181N, or more than 29%.  Note, however, that it was just the fact that two sensors are being averaged that yielded the improvement - not the fact that one of the two sensors had better resolution.


问:
No mutually perpendicular line between z-axes, are there still valid D-H Parameters?
I am trying to set up a generalized approach to generate DH parameters for my simulated robot arms, but I am having trouble setting up the frames. To find the X axis of each joint I read that I have to find a line orthogonal to both Z axes of the previous and current joint.
e.g. as explained in this video around 0:40 https://www.youtube.com/watch?v=rA9tm0gTln8

However, in some cases I find that a line orthogonal to both Z axes does not seem to exist. So how do I then determine the X-axis direction of subsequent joints. Are there configurations for which DH-parameters simply do not exist? or is there some other rule tied to this? Or does this orthogonal line exist somewhere but I'm simply failing to find it?
I can't find my specific case online. I have enclosed a picture with the two Z axes drawn in the axis of revolution for 2 joints. the Z axes are not intersecting anywhere, not parallel and not perpendicular.

答: Absolutely.  Depending on which version of D-H you are using, look at the $d$ and $r$ terms to accommodate the offsets.


问:
problem with simulated sensor in Matlab?
I'm simulating a sensor in 3D. The sensor should determine ($p, \theta, \phi$) from the origin  where $\theta$ is the rotation about z-axis and $\phi$ is the rotation about x-axis. The sensor is given position of a point($x, y, z$). This is what I did

```<code>    p = sqrt(x^2 + y^2 + z^2);
theta = acos(z/p);   <---- I'm guessing the problem here
  phi = atan2(y,x);
</code>```

Now I need to get the Cartesian coordinates ($x',y',z'$). This is what I did

```<code>    [p theta phi] = getmeasurement(x, y, z);
    x' = p*cos(theta)*sin(phi);
    y' = p*sin(theta)*sin(phi);
    z' = p*cos(phi);
</code>```

The sensor is working fine at the beginning but at a particular point it behaves strangely. I have the state vector to compare it with the measurement. I'm guessing that $\theta$ might be the problem.

Edit:

I'm sorry for this mistake. The aforementioned calculations based on the following picture

So, the point will rotate first about z-axis ($\theta$) and then rotate about x-axis ($\phi$)

答:
I found the problem by switching the following</p>

```<code>    p = sqrt(x^2 + y^2 + z^2);
  Phi = acos(z/p);   <---- Phi not Theta
Theta = atan2(y,x);  <---- Theta not Phi
</code>```

Body:
I found the problem by switching the following

    p = sqrt(x^2 + y^2 + z^2);
  Phi = acos(z/p);   <---- Phi not Theta
Theta = atan2(y,x);  <---- Theta not Phi


问:
Time synchronization
I have a robot receiving images from a camera. Now, there is a denso device for time synchronization so that we get correct time stamping between of the camera frames. I am not sure what does that mean? Can you please explain this process?

答:
This is a confusing question for me because I don't understand why you're looking at the device if you're not sure what it does. Was this recommended to you? By whom? What did they say when you asked them why you would use one?

As jsotola mentioned, you haven't given us a datasheet or a clear use case, so I can't comment on specifics, but generally speaking I would expect it will do one of two things:

Give you a timestamp for when exactly the frames were captured, OR

Trigger the cameras to capture a frame at an exact timestamp.

In both cases, the point is synchronization between either the cameras or between each camera and the chassis.

If you're doing something like stereo vision, then you would want to know that differences between the left and right cameras are due to depth disparity and not due to object motion in the time between captures. This requires the scene to be stationary while the left and right images are captured. You can't control the scene, but you CAN control when the images are captured.

In a somewhat similar scenario, you may want to index your images to a particular position, for something like structure from motion, etc. This would mean indexing the image to the chassis position, which would require you to know when the image was taken.

Title: Time synchronization


问:
Adding a layer of business logic onto ROS bots
For the past few days I've been trying to find whether there is a tool that allows one to build a layer of business logic components on top of a ROS bot with basic functionality.

For example, say there is a basic autonomous drone running ROS, with capabilities to fly, avoid obstacles, navigate a plan etc. as "basic" capabilities. Is there a way to build on top of this drone, an extra layer of business logic components, that specializes the drone in some way i.e. now allows it to carry out some 'business goal" like "find the number of dogs from point x to y" (kind of like the concept of decorators).

I'm saying "an extra layer" because it would make sense to not incorporate the logic into the ROS computation graph as it should build on top of the basic capabilities rather than extend it directly.

So I guess I'm looking for a tool that can recognize the current computation graph of a hooked up bot and an API that allows one to make calls to different nodes/topics/services on the bot such that having written some business logic elsewhere I could incorporate to work with the bot through this API.

Does such a thing exist? Even if not - do you think it makes sense/would be good if it existed at least?
I know we don't want to give software recommendations here but rather I just wish to find out if such a thing exists and more so if anyone with more of a background in robotics than me thinks it should.

Cheers.

答:
What you're looking for is basically a regular program that includes the ROS libraries to make the appropriate service calls, publications, and/or subscriptions. ROS integration.

The program handles all your logic, then you write a publisher to send ROS commands to the drone.

If you want to get data back from the drone then write a subscriber (same link) to the drone's camera topic and you can do whatever image processing, etc. you want from the "business logic" application.


问:
Explanation of the Kalman Filter
I am a beginner in robotics, and I am learning about the Kalman filter. I do not seem to get it, though. I am a mathematician, and so it would be helpful if the Kalman filter could be explained in a mathematical method.</p>

答: I think you need to understand it from practical perspective. From mathematical point of view, it is matrix multiplications and it is simple. I understood the concept by simulating simple systems in Matlab in the presence of noise. The goal is to estimate the state vector in the presence of noise which makes sense to build such a system that has noise. Check out the INTERACTIVE COURSE ON EKF AND SLAM.</p>

Title: Explanation of the Kalman Filter


问:
Create LAN network between laptop and robot
First of all I don’t really know if this is the right community for the question.
And second, I’m a noob in telecommunications and I want to know what should I do to establish a good wireless connection (not internet) between my laptop and my robot with a raspberry pi as the main computer.

This connection needs to handle video streaming of 2-3 cameras from the robot to the laptop and send commands to the robot with a remote control plugged to the laptop. The robot will be at ~50 meters, might me more.

I repeat, I’m a noob and I don’t know if a have to buy a router to plug to the laptop or something like this.

Thanks.

答:
There are lots of alternatives and issues here, and what you wind up with will depend on the specifics of your situation and a lot of other things only you can determine — notably including your budget.

WiFi configuration

Your Raspberry Pi has a quite respectable WiFi built-in.

Pi as WiFi hotspot. (Do a web search for "raspberry pi hotspot wifi".) The laptop connects to the Pi's hotspot. The WiFi signal will limit how far the robot (carrying the Pi) can roam from the laptop.

Shared WiFi. You could cover the roaming region with several WiFi routers connected together with ordinary Cat5 ethernet cable. The Pi and the laptop both connect to the shared subnet. This would let the robot roam as far as you can afford to string cables.

Bandwidth

Consider that several cameras' worth of video will all contend for the same WiFi bandwidth. The bandwidth demand depends on the resolution, the frame rate, the video codecs, and the number of cams transmitting (maybe the controller doesn't need to look at all the feeds at the same time). The bandwidth available will degrade with signal quality, which can depend on distance, terrain, other wireless traffic nearby, and even how the robot is constructed.

Experiments without the robot

WiFi distance. Set up the Pi as a hotspot.  Set up the simplest possible web server on the Pi (web search: raspberry pi web server). Connect the laptop to the Pi's wifi, and the laptop's browser to the pi's server. Leave the pi where it is and start carrying the laptop farther and farther away. If the connection drops before your 50 meters, you proved the pi-hotspot can't work.

Cam bandwidth. Set up the pi serving all your cams, with the laptop receiving and displaying the video. Does it even work with everything co-located? If not, work on the res, frame rate, etc. If so, do the 50-meter distance check. Try it with somebody watching a youtube video nearby while someone else places a video call.

There are fancier (and more expensive) alternatives to WiFi, which you can ask about when you can formulate a more detailed and specific question.


问:
HC-05 not responding to AT Commands from Raspberry Pi Pico
I'm trying to write AT commands to HC-05 module using Raspberry Pi Pico and MicroPython, nothing complicated. But for some reason my module doesn't respond. It works well in data mode and I'm able to send/receive messages from my phone via BT connection.

I've tried doing the same thing but using Arduino and my HC-05 was working. The module is blinking with interval of 2Hz so it's clearly in command mode.

Changing Baud Rate doesn't change anything.

Connecting TX to TX and RX to RX causes the first message to be b'\x00', otherwise nothing is returned.

My Code:

from machine import Pin,UART #importing PIN and PWM
import utime #importing time

#Defining UART channel and Baud Rate
uart = UART(0, 38400)

while True:
    command = input("COMM>")

    uart.write(command)
    utime.sleep(1)  # to make sure that HC-05 has time to respond
    print("checking", uart.any())

    if uart.any(): #Checking if data available
        data=uart.read() #Getting data
        data=str(data) #Converting bytes to str type
        print(data)
```

Ultimately I want to use AT+INQ to retrieve data about RSSI of nearby devices.

答:
You want to connect TX <-> RX and RX <-> TX.  See https://www.instructables.com/Bluetooth-ModuleHC-05-With-Raspberry-Pi-Pico/

Here is another article that describes putting the HC-05 into AT mode, which involves driving the 'Key' pin high  https://www.instructables.com/Modify-The-HC-05-Bluetooth-Module-Defaults-Using-A/

Also note the default baud rate is 9600; your code is assuming 34800 baud.


问:
remove gravity from acceleration on Myo armband IMU measurements
I'm working with the Myo armband through the myo_ros package. The device is able to provide IMU measurements comprising orientation, linear acceleration and angular velocity. The orientation is expressed wrt an (unknown) reference frame, chosen when the device is turned on. We can refer to this frame as myo_ref. The linear acceleration is expressed wrt the myo_raw frame, which is a north-west-up moving frame attached to the device. I want to manually remove the gravity component from the accelerometer data.
These are the steps I'm doing:

calibration: I record the orientation when the accelerometer measure +9.81 over the z-axis (so I'm sure the device is aligned with the Earth's z-axis pointing upward).  This orientation, let's call it q_ref2aligned is used to publish a static transformation, describing the new frame myo_aligned wrt the frame myo_ref;

each IMU measurement has an orientation, let's call it q_ref2raw which expresses the current pose of the armband wrt the frame myo_ref

To the best of my knowledge,  the inverse quaternion of q_ref2aligned, that is q_aligned2ref, describes the transformation from the frame myo_aligned to the frame myo_ref

q_aligned2ref * q_ref2raw = q_aligned2raw should represent the current orientation of the armband wrt the frame aligned with the Earth's z-axis, right?

if lin_acc is the acceleration recorded in the current IMU measurement (so wrt the myo_raw frame) and G = [0, 0, 9.81]  is the gravity vector, if I multiply lin_acc by q_aligned2raw and then substract G I should be able to remove the gravity component, correct?

To accomplish this, I first turn q_aligned2raw into a rotation matrix M with tf.transformations.quaternion_matrix, then I use the matrix-vector multiplication with lin_acc and finally just substract G.

Am I missing something? This approach fails.
Here are some experiments:

1.

IMU lin_acc reading [x, y, z]: [-0.32561143, -0.80924016,  9.88805286]

expected lin_acc after rotation: [~0, ~0, ~9.81 ]

observed lin_acc after rotation: [-1.76936953, -4.4546028 ,  8.69254434]

2.

IMU lin_acc reading [x, y, z]: [-0.19153613, -0.01915361, -9.62947908]

expected lin_acc after rotation: [~0, ~0, ~9.81 ]

observed lin_acc after rotation: [ 1.58807182,  9.41955642, -1.23040848]

3.

IMU lin_acc reading [x, y, z]: [-0.09576807, -9.61990227,  2.36068284]

expected lin_acc after rotation: [~0, ~0, ~9.81 ]

observed lin_acc after rotation: [-8.92865455, -4.05394425,  1.40327425]

4.

IMU lin_acc reading [x, y, z]: [-0.36391865,  9.62947908,  0.70389529]

expected lin_acc after rotation: [~0, ~0, ~9.81 ]

observed lin_acc after rotation: [-8.56518971,  3.71455092, -2.48885704]

5.

IMU lin_acc reading [x, y, z]: [9.60553706e+00, 4.78840332e-03, 9.57680664e-03]

expected lin_acc after rotation: [~0, ~0, ~9.81 ]

observed lin_acc after rotation: [ 1.43719352,  7.26609646, -6.11594423]

6.

IMU lin_acc reading [x, y, z]: [-10.07480059,  -0.16280571,   0.09576807]

expected lin_acc after rotation: [~0, ~0, ~9.81 ]

observed lin_acc after rotation: [ 1.86515326,  7.72080671, -6.20061538]

答:
I see this kind of question a lot, and my answer is always the same - use the Madgwick filter. Here's the original site but if it's not working here's another and the Github page. It's PhD-level work that's available, for free, already written, in C, C#, and Matlab.

You don't need to implement your own calibration routine, and:

The odds of your accelerometer reading exactly 9.8100000 on the z axis are very small, and even if they did

You're not checking accelerations on the other axes, so you aren't discriminating between gravity and motion, and even if you were

You don't appear to be taking any angular velocities into account, so it's not clear how you're developing q_ref2raw, which in turn is critical to your conversion.

For reference, I took the magnitude (norm) of all of your example cases and I got the following: 9.9265, 9.6314, 9.9058, 9.5520, 9.6055, 10.077. None of these are 9.81, and so your conversion will never get any of those readings to [0, 0, (+/-)9.81]. Maybe there are readings on the other axes? I do get what you mean, though, in that the 9.81-ish readings aren't in the z position, but your algorithm isn't provided in detail here and I don't think it's suitable anyways.

Have you tried troubleshooting to determine when/if you're hitting the 9.81 on the z-axis? You may be looking at stale/uninitialized rotations.

Whatever the case, again, Madgwick has you covered. Use the free, open-source algorithm that works.


答: Given two linearly independent (read: not parallel) vectors, you can always find a vector which is perpendicular to both of them using the cross product: https://en.wikipedia.org/wiki/Cross_product.</p>

Title: No mutually perpendicular line between z-axes, are there still valid D-H Parameters?

Body:
I am trying to set up a generalized approach to generate DH parameters for my simulated robot arms, but I am having trouble setting up the frames. To find the X axis of each joint I read that I have to find a line orthogonal to both Z axes of the previous and current joint.
e.g. as explained in this video around 0:40 https://www.youtube.com/watch?v=rA9tm0gTln8

However, in some cases I find that a line orthogonal to both Z axes does not seem to exist. So how do I then determine the X-axis direction of subsequent joints. Are there configurations for which DH-parameters simply do not exist? or is there some other rule tied to this? Or does this orthogonal line exist somewhere but I'm simply failing to find it?
I can't find my specific case online. I have enclosed a picture with the two Z axes drawn in the axis of revolution for 2 joints. the Z axes are not intersecting anywhere, not parallel and not perpendicular.


问:
Actuator to control steam valve
I have a steam radiator at home and it has a valve similar to the picture below.

Please note that the valve doesn't have grooves on top to attach things to.

I want to build something to turn it on and off depending on the temperature at certain points in the room.

I have that taken care of but cannot find a way to attach a actuator(actuator is the right word in the context I guess?) to turn the valve in both directions.

Also It is a rented apartment so I would like to avoid making any modifications to the radiator itself.

答:
Well, being the manipulator removable, so you can attach other thing to the valve shaft, you have so much options.

Mechanical coupling

You could use a coupled rotary actuator to the shaft by the means of a shaft coupler. There's many types, this is a simple one, but you probably would not get the actuator axle full aligned with the valve shaft, so a coupler that permits some misalignment is desirable, like these:

rubber shaft coupler http://www.abssac.co.uk/uploads/products/cat_pp651_coupling%20jaw%202.jpg

Every coupler has its specifications and types of misalignment it will support.

You can use a belt or chain transmission too. Then you need to attach a pulley or sprocket to the valve axle.

Chain and sprocket transmission

Synchronous belt transmission

A possible problem with these two option is the lateral force the transmission could put on the valve shaft, as it has not made to support this, it probably will wear. One way to overcome this is to use a shaft coupler to a bearing with the transmission, so the lateral force will be supported by the bearing.

Electro-mechanical actuator

For actuating the valve, you can use a couple of options.

A stepper motor.

A DC brushed-motor with a reduction gearbox like this

The gears are show for illustration purposes, they are normally enclosed.

There's many types of reduction gearbox too, I will not extend into this on this answer.

You probably need some way of knowing the position of the valve shaft (feedback). It can be a multi-turn potentiometer for simple analog output.

RC servo motor

If the valve needs less than 1 turn you can try to use an RC servo-motor, they have a DC-brushed motor, a reduction gearbox, a potentiometer for feedback, and a control circuit. You then feed with the angle position in a signal modulated by a technique know as PWM. Along the fact that the valve will operate at less than a 360° angle, you need to be sure the torque of the RC servo motor will be superior to the needed. Also metal gears and shaft will provide a more robust system, considering the torque.

Assembly

@DiegoCNascimento the image can be used with credits

Here's a final assembly possibility. You should be sure the pipe could support the forces involved with a safety margin, if not, use the wall mount supports.

Note: some valves main axles will go up or down when rotated, if this is the case the mechanical coupler should support this.

I do not assume any responsibility by any damage or injure in the mounting of this, the user should have plenty attention he/she is working with high-temperature and high-pressure parts that can pose danger.

Title: Actuator to control steam valve