问:
Off the shelf way to power jetson nano and xavier from iRobot Create 2
I'm making an indoor companion bot, but requirement is that it easily traverse thick carpet, which is common all across the US in houses.  Seems no better platform than 'Roomba' which was designed for that. (iRobot Create 2)

I will have to power several cameras, a couple servos, webcam speakers and a Jetson xavier nx.

The PRIMARY requirement is that the robot can autocharge it self. It is 100% unacceptable to have any forced physical interaction.  Similar to the Vector robot, it is just suppose to be a little pet for entertainment, and needs to charge itself.

I have skills in the programming, but I need to avoid the wiring, soldering part, volts-watts current limits etc. so it can be used in a programming (not electronics) teaching setting.  I want to know what off-the-shelf things to buy be able to just assemble it and work on the code.

budget limit besides cameras, jetson, and 'roomba' is $500 USD

"how can I power a jetson and the mentioned peripherals FROM THE BATTERY of the Create."

The comments about price were to 'quantify the solution space'

The comments about 'self charging without intervention' were to  clarify the restriction of not using external battery packs, and not have the solution interfere with Roomba's autocharging feature

The comments about the choice and the robot were to clarify that other bases could potentially be solutions but would need to be as effective on carpet and handle weight and self charge

答:
The Create2 Open Interface Spec (warning: direct PDF download) gives a description of the pinout on the connector:

You can see there that you can tap into the Create2 battery by attaching to Pin 1 (V+) and Pin 7 (GND), etc. There's a note under this diagram that states:

Pins 1 and 2 (Vpwr) are connected to the Roomba battery through a 200 mA PTC resettable fuse. The continuous draw from these two pins together should not exceed 200 mA. Do not draw more than 500 mA peak from these pins, or the fuse will reset.

There's a document about using the Create2's battery power (warning: direct PDF download) that gives some more information, stating in part:

SERIAL PORT

Ratings

Voltage 20.5V – 10V

Current 0.2A

Power 2W

Always On: The serial port power comes from either the charger
when the robot is on the dock or from the battery while the
robot is running, serial port power is always on. We recommend
installing an on/off switch between the serial port and your
project. Remember to turn off your project when you’re done. If
you leave your project on and don’t return the robot to the dock
to charge you will eventually deeply discharge the battery and
damage it.

If you're going to power something off the battery, you'll need a voltage converter that can handle at least a 10-20.5 volt input range. Any converter capable of handling that range should work, then you'd just need to determine what the output voltage is that you need and the power requirements to make sure you're staying under the 2W serial port requirement.

Body:
The Create2 Open Interface Spec (warning: direct PDF download) gives a description of the pinout on the connector:

You can see there that you can tap into the Create2 battery by attaching to Pin 1 (V+) and Pin 7 (GND), etc. There's a note under this diagram that states:

Pins 1 and 2 (Vpwr) are connected to the Roomba battery through a 200 mA PTC resettable fuse. The continuous draw from these two pins together should not exceed 200 mA. Do not draw more than 500 mA peak from these pins, or the fuse will reset.

There's a document about using the Create2's battery power (warning: direct PDF download) that gives some more information, stating in part:

SERIAL PORT

Ratings

Voltage 20.5V – 10V

Current 0.2A

Power 2W

Always On: The serial port power comes from either the charger
when the robot is on the dock or from the battery while the
robot is running, serial port power is always on. We recommend
installing an on/off switch between the serial port and your
project. Remember to turn off your project when you’re done. If
you leave your project on and don’t return the robot to the dock
to charge you will eventually deeply discharge the battery and
damage it.

If you're going to power something off the battery, you'll need a voltage converter that can handle at least a 10-20.5 volt input range. <a href="https://www.digikey.com/en/products/filter/dc-dc-converters/132?s=N4IgjCBcoKwOwGYqgMZQGYEMA2BnApgDQgD2UA2uDGABwCcNIxtNNYdT49CYADJ%2BwAsNQYIF1EcfsQBMYGAl4xOcmrwBscFTJkIe2mer3b1YGWNkIaiRrPjrhK%2B4JlOaC2yEMbqKo3QkVOEEwPiCYdXVlWTg2GnUg1l9ZayMtYkVQwWiQRT0wJGJNeJy4MHUlTjpy6qqFXgsQOgi6Rr4NMoFeApEuivMBQV4RTzkEKOkveQCIWTA4GXcVWiNCrzkYXlcUqMiVOl4GRvUaK22QU6jZpoDBDmZ6CITmGBh456oJGHPabIYBETNDgAXWIAAcAC5QEAAZQhACcAJYAOwA5iAAL7MQSdaAgNCQLB4IikCheQ6se5eO7za4yBRjfbUeCcRQwHGjOiRfSyCSCKKcYLMkCgkCQ6FwpFozHMBgcPEEokEYhkSCUfii8WQWEIlHorEgAC0rgVUARAFcSarKMpgRiDQk8YiACbQw18a5akCcCEATzB%2BGhmFwaHtQA" rel="nofollow noreferrer">Any converter capable of handling that range should work, then you'd just need to determine what the output voltage is that you need and the power requirements to make sure you're staying under the 2W serial port requirement.


问:
How to calculate rotation angle from 3D points
I have 4 3d points(x, y, z). Using these points how can I calculate rotation angle? Consequently, I want to have a 4x4 transformation matrix including rotation and translation information.

4 points are in a plane and they are rectangle and I set the top-left point as the origin of the rectangle coordinate. Look at the bellow pictures.

Here I only have four 3D points(Xw,Tw,Zw) based on the world coordinate. According to the 4 points, I can calculate 4 points based on the rectangle coordinate. For instance, value [Xr] in P2 is the distance between P1 and P2. And [-Yr] in P3 is the distance between P1 and P3.

Then how could I get rotation angle of the rectangle based on the world coordinate using those information?

```<code>Additional information (3d points in the world coordinate):
P1(-401.428, 485.929, 772.921)
P2(-21.4433, 475.611, 772.511)
P3(-400.982, 483.56, 403.703)
P4(-21.589, 473.028, 403.242)
</code>```

答: Also check this blog post for an alternative solution: http://nghiaho.com/?page_id=671</p>

ContentLicense: CC BY-SA 4.0


问:
Track a moving object
If this has already been answered, by all means please point me to it.

I am in the process of building a quadcopter which I eventually plan to run autonomously by allowing it to track an object and take a video feed of it moving.

GPS is one of the options I've considered, basically:

GPS antena on moving object (person, car, bike, surfer)

GPS antena on quadcopter

Radio to transmit coordinates from moving object to quad copter

Some of the challenges I can foresee are

Line of sight for camera. How does the camera know exactly where to point?

Angle, how can I pre-program the quad to always record, say... 10m to the right of the moving object, or even better, program a set of angles to record from whilst keeping up with the object

GPS accuracy, what happens if the GPS lock is weak?

What are some of my other options? I saw this TED Talk where the quads are following a ball shaped sensor? I believe it uses Kinect cameras and lots of them which is not really an option for this challenge.

So I'm open to hearing some ideas before I start research and development of these features.

答: If you want sub-meter tracking of the moving object, GPS alone will not be sufficient.  I concur that passive vision-based techniques will require a huge amount of engineering to implement, not to mention substantial size, weight, and power.  I recommend using differential GPS techniques -- not exactly easy, but simpler from a system standpoint.  You will need a high-update rate GPS receiver that outputs Carrier Phase and Pseudo-Range data as well as a traditional GPS solution.  You will also need to transmit this additional data to your quadcopter from both a nearby reference station and the moving object.  It is useful to have a stationary reference station since its position is exactly known, so using the satellite ephemeris you can determine the propagation and timing errors from each satellite very precisely.  Since you know the approximate geometry to both the non-moving reference station and the moving object, and the paths to the satellites are approximately the same for both cases, you can adopt the precisely calculated propagation and timing errors learned from the reference station and apply them to the moving object.  Additional accuracy is gained by computing a relative position solution for both the quadcopter and the moving object that explains the observed changes in Carrier Phase over time as both of these move.  Since these are phase changes, the integer number of cycles is unknown and must be solved for using a well-known technique for finding integer solutions to least-square problems (name escapes me at the moment).  A Kalman Filter or Particle Filter is also typically needed to converge to the correct position solution using the carrier data after fusing with the pseudo-range-derived data.  Check out work done at Auburn University's Navlab for examples of this type of work.</p>

ContentLicense: CC BY-SA 3.0

Title: Track a moving object


问:
How the motor commands are obtained from gcodes?
I have started my research with scara and cartesian robots for drawing robot. The gcodes are converted and given to robot. There are firmware like grbl, marlin, etc for this process. But I couldn't find any good source to understand how they are converting gcodes to motor commands.

I would like to create my own program for this process. Inorder to do it I need to know how it is working. So, can I get some help in understanding it.

答:
The main role of the CNC controller is to "convert" G Code to motor commands. This is a highly complex function and many books have been written about this.

As a top level overview, the G-Code is first interpreted, by the Interpreter. This converts to textual form of the G-Code to an in-memory representations. At this point separating motion related commands to any other commands is done.

All motion related commands are processed in a motion buffer (used for the lookahead functionality). The lookahead is especially important for smooth trajectories. The geometrical path is created (corners are sightly rounded, tool radii are added/subtracted). After the path is geometrically created the trajectory (velocity, acceleration, jerk) components are added, based on the target speed set in the G-Code and the dynamical capabilities of the machine.

After the trajectory is created it is sent set as reference for the motor controller. Here the calculated values might be interpolated if needed.


问:
Multivariable PID controller
I'm making a robot that uses two DC motors to move, and has two distance sensors on its right side. I want to make a PID controller to control the angle of the robot (using a wall as reference) and also its distance to the wall.

I made a PID controller that can minimize the difference between both sensors' measurements, making the robot go always parallel to the wall. I also made a controller for controlling the distance one sensor measures, for the robot to go always at the same distance to the wall.

What I want to do is combine those two controllers into one. I know there's something called MISO PID controller (multiple inputs single output) but I don't know how to use it or even design it. I thought of adding those two variables (the difference and one sensor's measurement) and using that as the controller's input. Is that right? How can I achieve this?

PS: I also thought of adding the difference between each sensor's measurement and the distance I want, or using those two differences as the variables of my MISO controller.

答:
If I'm interpreting your description correctly (the two sensors are measuring the distance to the wall in a direction roughly perpendicular to the direction of travel and the two sensors are displaced so that one is more "forward" in the direction of travel) , you need to change the angle of the robot relative to the wall in order to change the distance to the wall.

This is a good use case for a cascaded or backstepping controller where an outer control loop's output is a reference for an inner loop. Your inner loop will control the angle of the robot relative to the wall and the outer loop will control the distance to the wall.

So the outer loop will look like:
desired_angle = -k_p * (distance_to_wall - desired_distance)

Note that only a P (or PI) controller should be sufficient since if moving at a constant velocity, a constant angle will be a constant velocity towards or away from the wall.

Since the outer loop outputs an angle, you can convert that into a desired sensor reading difference, given the geometry of the robot and the placement of the sensors. Or more simply, you can just interpret the output of the outer loop as a sensor reading difference, as long as you keep the output to a reasonable limit.

The inner loop will look like:
motor_control = -k_p * (current_sensor_difference - desired_sensor_difference) - k_d * (...)

This inner loop will be the same inner loop you used before to control the angle to 0, but with a non zero set point.

Hope this helps!

Title: Multivariable PID controller


问:
Identifying joints from transformation matrix (DH matrix)
Given a transformation matrix formed using the Denavit Hartenberg method, is there a way to identify joint paramters: for example the order of joints?

I've come across a solution to an exam question that seems to imply so but I am struggling to see how the order of joints can be identified by looking at a matrix.

Many thanks in advance for any help!

答:
Okay, so let me preface this with the statement that I think this is a lame question. First, because I can't imagine a scenario where you're going to be given just the transform matrix and have to divine the structure, and second because if you chain your $T_{01}T_{12}T_{23}$ transforms together you don't wind up with the transform provided in the question statement; the rotation matrix portion is a lot messier.

That said, here are some things to consider:

If you look only at the rotation matrix portion of the transform (upper-left 3x3 submatrix) the z row/column has a single 1 in it. This indicates that there's one axis that's not getting any rotation in or out of the z-plane. This, in turn, could indicate a planar/2D robot because you can't twist into the third dimension.

If you look only at the translation vector (upper-right 3x1 subvector) then you can see $d$ is occupying the $z$ position.

So right off now you can see that there is some rotation "stuff" in your 3x3 rotation matrix that will modify x/y points, and there's some value $d$ in the translation vector that will modify z points, so this looks like a planar robot that can telescope up and down. This eliminates the first option, which is the only answer to call it a 2D robot.

The only thing I have left to look at to try to solve the problem now is that translation vector. If you go back and look at the x/y translation entries, you see that the x-values are offset by $c_{\phi}(l_1 + l_2)$ and the y-values are offset by $s_{\phi}(l_1 + l_2)$.

What's notably missing there is the other rotation term, $\theta$. You know there's something rotating with $\theta$, because of the rotation matrix portion, but it's not affecting your translation or your link offsets $l_1$ or $l_2$, which must mean that the $\theta$ rotator is at the end of the robot arm.

Similarly, because both $l_2$ and $l_1$ are modified by $\phi$, this must mean that the $\phi$ rotator is in a position to move both links.

This leaves you with choice D - a 3D arm with a $\phi$ rotator first, the $\theta$ rotator last, leaving the $d$ prismatic joint in the middle.

You might be able to pick apart the transforms for all configurations; it's late and I'm not positive about this, but wow it's tedious. Typically stuff like this you'd just plug into Matlab or Simulink or something and get the answer. You might get to the point with enough practice that you'd get some intuitive feel for looking at the transform matrices, but typically I'd just call the total transform T03 or whatever and not ever look inside it.


问:
Some doubts regarding SLAM and its technical terms

I have some questions regarding SLAM and some general technical terms used in the context of it.

Q1) **Online SLAM vs Offline SLAM**. Can "Online" SLAM be also called "real-time" SLAM? What is the use of offline SLAM if it can't be used on a real robot in real-time?

Q2) **Datasets for SLAM**: Are there datasets different for offline and online slam algorithms? What is the difference exactly?

答:
The use of terms Online vs. Offline may be different in different sources.

However, I am used to the meanings presented here: https://control.com/technical-articles/an-introduction-to-simultaneous-localization-and-mapping-slam-for-robots/

This actually distinguishes between Online SLAM vs. Full SLAM, where Online SLAM means the estimation of the current robot pose together with the state of the environment, while Full SLAM estimates the complete set of robot poses over time (and state of environment, which typically can be recovered given poses + sensor data).

While Online SLAM is handled by different computational approaches and generally much less computationally expensive, there is no implication that Full SLAM could not work in "real-time" (actually, a more appropriate term might be again "online", but it depends on the definition of real-time you want to apply, and probably also on the specific implementation, e.g. do you need and get guaranteed response times?).


问:
Hi everybody! I would appreciate some help with this DH convention application. It involves a slider-crank mechanism and I'm not sure I did it right

Okay, I'm editing the post and adding some clarifications: I'm considering the slider-crank now as a closed kinematic chain, and for this reason I isolated two different branches. My attempt with the following script is to get a simulation of its motion, but since I've still errors, I think I'm not getting something about the Symbolic Toolbox and about DH convention as well.
Edit 2: I'm attaching the picture of a second solution proposal, this time I get complex numbers on MATLAB.. Now I'm sure I miss something about solving closed kinematic chains.

```<code>    clear all
close all
clc
%% Input Parameter (Change k in order to vary end-effector trajectory)
% Conversion factor from centimetres to metres
conv = 0.01;
% Incremental step
h = 0.1;
% Lenght of crank (link L1) in [m]
l = 5*conv;
k = 2;
% Length of links of mechanism

L1 = 9*l;
L3 = 18*l;
L2 = 6*l;

% Length of virtual links for closed loop equations
%L0 = k*L1;

% Actuator - Costant angular velocity
Theta1 = [0:h:2*pi];
Theta1_ = Theta1 + pi;
beta = asin(L1/L2)*sin(Theta1);

%% Denavit Hartenberg - Forward Kinematic
syms  theta1 d3 theta2 theta3

% For example T30_1s means transformation matrix from Joint 3 to Joint 0
% for the 1 closed loop (p), if second branch is considered (s)
%%
% Transformation matrix Joint 1 to Joint 0 (workspace)
T10 =  DH(L1,0,0,theta1);

% Transformation matrix Joint 2 to Joint 1
T21 = DH(L2,0,0,theta2);

T20 = T10*T21

T32 = DH(0,pi/2,0,theta3);

T30_p = T20*T32; %this is the first branch of my closed kinematic chain

T30_s = DH(L3,-pi/2,d3,pi/2); % this is the second one, it goes from Joint3 (the prismatic one) to Joint0

%%

% Closure equation
eq2 = T30_p(1:3,end) == T30_s(1:3,end)

[Theta2, Theta3, D3] = solve(eq2,[theta2 theta3 d3]);

  angle2 = double(subs(Theta2(1),theta1,Theta1));
  angle3 = double(subs(Theta3(1),theta1,Theta1));
  displ = double(subs(D3(1),theta1,Theta1));

%%

% Joint vector position
J0 = [0 0 0]';
Joint1 = T10(1:3,end);
Joint2 = T21(1:3,end);
Joint3 = T30_s(1:3,end);

% Compute joint trajectory
for i = 1:length(Theta1)
    J1(:,i) = double(subs(Joint1,{theta1 theta2(1)},{Theta1(i) angle2(i)}));
    J2(:,i) = double(subs(Joint2,{theta1 theta2(1) theta3(1)},{Theta1(i) angle2(i) angle3(i)}));
    J3(:,i) = double(subs(Joint3,{d3(1)},{displ(i)}));

end

%% Movie
figure('units','normalized','outerposition',[0 0 1 1])
view(2)
set(gca,'nextplot','replacechildren');
v = VideoWriter('SingleLegFoot.avi');
open(v);
hold on
grid on
for i = 1:length(Theta1)
    clf
    hold on
    grid on
    hold on
    grid on
    axis equal
    xlim([-0.5 1.2]);ylim([-1.6 0.5]);xlabel('X [m]');ylabel('Y [m]')
    title('1 DOF Leg mechanism');
    set(gca,'FontSize',14)
    h12 = plot([J0(1,1),J1(1,i)],[J0(2,1),J1(2,i)],'r','LineWidth',2);
    h13 = plot([J1(1,i),J2(1,i)],[J1(2,i),J2(2,i)],'r','LineWidth',2);
    h14 = plot([J2(1,i),J3(1,i)],[J2(2,i),J3(2,i)],'r','LineWidth',2);

    hold on

    h1 = plot(J0(1,:),J0(2,:),'blackv','MarkerSize',10);
    text(J0(1,1)+0.02,J0(2,1),'0');
    h2 = plot(J1(1,i),J1(2,i),'o','MarkerSize',7,'MarkerEdgeColor','black','MarkerFaceColor',[0.7529 0.7529 0.7529]);
    text(J1(1,i)-0.01,J1(2,i)+0.03,'1');
    h3 = plot(J2(1,i),J2(2,i),'o','MarkerSize',7,'MarkerEdgeColor','black','MarkerFaceColor',[0.7529 0.7529 0.7529]);
    text(J2(1,i)+0.02,J2(2,i),'2');
    h4 = plot(J3(1,i),J3(2,i),'blackv','MarkerSize',10);
    text(J3(1,1)+0.02,J3(2,1),'3');

    hold off
    frame = getframe(gcf);
    writeVideo(v,frame);
end
close(v);
%% Denavit - Hartenberg Transformation Matrix

function dh = DH(link,a_i,d_i,theta_i)
    dh = [cos(theta_i) -sin(theta_i)*cos(a_i)   sin(theta_i)*sin(a_i)     link*cos(theta_i);
          sin(theta_i)  cos(theta_i)*cos(a_i)   -cos(theta_i)*sin(a_i)     link*sin(theta_i);
               0         sin(a_i)                   cos(a_i)                        d_i;
               0             0                          0                           1       ];
end
</code>```

答:
There's a lot going on here, and it's not specific what your question is as jsotola mentions.

First, you are putting the output of a solve function into Theta2, but then you're using THETA2 as inputs to subsequent subs functions. Matlab is case-sensitive; those are two different terms.

Second, assuming you're using symbolic variables (based on your error) you can't, as the error states, mix symbolic and doubles. This is probably happening in your line eq2 = T_end(1:3,end) == [0;0;0]; which instead needs to be eq2 = T_end(1:3,end) == sym([0;0;0]); to work.

Third, I can't see where any of your stuff is actually defined - your Theta1, THETA3, etc.

Fourth, you're using a DH function but that's not a built-in Matlab function.

Put everything you're trying to do in a script, start that script with the clear or clear all command, and try running as a saved script. Any errors you get will give you line numbers, pointing you to exactly where the problem is. If you still have trouble, please post the entire script so we can follow along with your variable declarations, etc.


问:
How to compute the Hessian matrix in the robustified Gauss-Newton method for optimization
When robustifying the Gauss-Newton method used to find a local minima of a squared error function expressed by:
$$f(\mathbf{x})=\frac{1}{2}\Delta\mathbf{z}(\mathbf{x})^T\Delta\mathbf{z}(\mathbf{x})$$,
as explained in works like Bundle adjustment: A Modern Synthesis (page 17), we have to introduce a strong nonlinearity into this cost function, which is denoted by $\rho(\cdot)$ (it can be e.g. the Huber loss).

Thereby to reduce the influence of outliers, we are no longer interested in minimizing $f(\mathbf{x})$, but the robustified version of it: $\rho(f(\mathbf{x}))$. Hence, the gradient vector ($\mathbf{g}$) and the approximate Hessian matrix ($\mathbf{H}$) are given by:

$$
\mathbf{g} = \rho'\mathbf{J}^T\Delta\mathbf{z},\quad\quad \mathbf{H} \approx \mathbf{J}^T(\rho'\,\mathbf{I} + 2\rho''\Delta\mathbf{z} (\Delta\mathbf{z})^T)\mathbf{J}
$$
Where $\rho'$ and $\rho''$ are the first and second derivatives of $\rho(f(\mathbf{x}))$ w.r.t $f(\mathbf{x})$. And $\mathbf{J}$ is the jacobian matrix given by: $\mathbf{J} = \partial \Delta\mathbf{z}/\partial \mathbf{x}$.

Given this, what I don't understand is how this approximation of the Hessian matrix is computed.

My attempt to reach it, is the following:
$$\begin{align}
\mathbf{H} &= \frac{\partial^2\rho(f(\mathbf{x}))}{\partial \mathbf{x}^2} = \frac{\partial\rho(f(\mathbf{x}))}{\partial f(\mathbf{x})}\frac{\partial^2 f(\mathbf{x})}{\partial \mathbf{x}^2} + \frac{\partial^2\rho(f(\mathbf{x}))}{\partial f(\mathbf{x})^2}\left(\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\right)^2 && \text{By applying chain rule}
\end{align}
$$
Where:
$$\begin{align}
&\frac{\partial\rho(f(\mathbf{x}))}{\partial f(\mathbf{x})}\frac{\partial^2 f(\mathbf{x})}{\partial \mathbf{x}^2} \approx \rho'\mathbf{J}^T\mathbf{J}\quad\leftrightarrow\quad\text{Gauss-Newton approximation}\\
\\
&\frac{\partial^2\rho(f(\mathbf{x}))}{\partial f(\mathbf{x})^2}\left(\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\right)^2 = \rho''\,\mathbf{J}^T\Delta\mathbf{z} (\Delta\mathbf{z})^T\mathbf{J}
\end{align}
$$
Thereby I am getting an approximated Hessian where the second term is not multplied by 2:
$$
\mathbf{H}_{\text{attempt}} \approx \mathbf{J}^T(\rho'\,\mathbf{I} + \rho''\Delta\mathbf{z} (\Delta\mathbf{z})^T)\mathbf{J} \neq \mathbf{J}^T(\rho'\,\mathbf{I} + \color{red}{2}\rho''\Delta\mathbf{z} (\Delta\mathbf{z})^T)\mathbf{J}
$$
Can please someone help me to see where I am doing the wrong step?

Thanks in advance!

答: Have a look at A.129 and A.158 of my thesis. This was my conclusion after reading multiple articles on the approximation method but I can't find the source articles.</p>


问:
Obtaining Heading vector from IMU
First time here.

I'm an electronics engineer who has been tasked with this problem: Given the 4D quaternion representing a vehicle orientation, how do you get the heading vector?

The vehicle orientation quaterions was obtained through the transformation of the IMU reference frame to the vehicles reference frame.

Now, I'm not extremely expert in quaterions, though I have been reading considerably about them recently.

Thank you

答:
Probably the easiest way to do this would be to convert from Quaternion to Roll-Pitch-Yaw rotations, and then your heading is the Yaw angle.

I'll note that the Yaw angle is not fixed/correct unless your IMU has a magnetometer. Accelerometers can fix roll/pitch by detecting the gravity/down vector, but North only comes from the magnetometer.

You can get more information from the Wikipedia article here, but I'll reproduce the Quaternion to Euler RPY code here for posterity:

```<code>#define _USE_MATH_DEFINES
#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}
</code>```

Title: Obtaining Heading vector from IMU

Body:
Probably the easiest way to do this would be to convert from Quaternion to Roll-Pitch-Yaw rotations, and then your heading is the Yaw angle.

I'll note that the Yaw angle is not fixed/correct unless your IMU has a magnetometer. Accelerometers can fix roll/pitch by detecting the gravity/down vector, but North only comes from the magnetometer.

You can get more information from the Wikipedia article here, but I'll reproduce the Quaternion to Euler RPY code here for posterity:

#define _USE_MATH_DEFINES
#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}


问:
Static transform difference between command line and launch file
I'm having a problem setting up the static transform between my base_link and my camera.

If I use rosrun tf static_transform_publisher in a different command line the camera is alligned with my axes but when I incorporate the same transform in a launch file it is out of allignment...

Here are my commands:

```<code>rosrun tf static_transform_publisher name=tracking_camera args=0.2 0 0 1.57 0 1.20  base_link camera_depth_optical_frame 100

<node pkg="tf2_ros" type="static_transform_publisher" name="tracking_camera" args="0.2 0 0 1.57 0 1.20  base_link camera_depth_optical_frame" />
</code>```

Why does this happen?

Edit: Here are the rqt_tree as requested in the comments.

Additionally if I input param 100 in the launch file as well I get the following error when trying to generate the tree:

```<code>rosrun tf2_tools view_frames.py
[INFO] [1629904078.746465]: Listening to tf data during 5 seconds...
Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "100" from authority "/tracking_camera" because of an invalid quaternion in the transform (1.570000 0.000000 1.200000 0.000000)
         at line 255 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
[INFO] [1629904083.765947]: Generating graph in frames.pdf file...
</code>```

答:
You're using a different executable from a different package in your rosrun arguments and your launch arguments.

The static transform publisher in tf uses 9 or 10 arguments including the period. http://wiki.ros.org/tf#static_transform_publisher

Whereas the tf2_ros static_transform_publisher uses 8 or 9 arguments as it no longer needs the period as an argument http://wiki.ros.org/tf2_ros#static_transform_publisher

tf2 has a higher level concept of a static transform which is timeless. The behavior of the tf instance was not changed to avoid breaking use cases.

If you use the tf instance of static_transform_publisher in your launch file you'll get the same results instead of the tf2_ros one. However I recommend that you use the tf2_ros one as it will be more efficient and performant.


问:
Significance of tf2_ros::TransformListener in constructors
Is there a reason that a lot of constructors in ROS based code take in tf2_ros::TransformListener as one of the input? I want to get an idea from the design perspective.

答: The tf2_ros::TransformListener takes up a relatively high amount of processing power because it subscribes to the publications of the high volume /tf topics and caches the results locally in memory. It's much more efficient to have one copy which you access from multiple places in your code by passing a reference to it and querying it directly rather than instantiating multiple instances which will increasing network traffic, memory usage, and cpu usage for each instance.


问:
can't install smach_viewer on ros melodic
I tried installing smach on ROS Melodic with the command:

```<code>$ sudo apt-get install ros-melodic-smach ros-melodic-smach-ros ros-melodic-executive-smach ros-melodic-smach-viewer
</code>```

from the book i'm reading ROS Robotics Projects by Ramkumar Gandhinathan, Lentin Joseph. while the other packages got installed the smach_viewer package gave a 404 error. After checking the package on this link i found out that some of the deb files were missing. please how do i solve this problem.

this is the error i'm getting:

```<code>Reading package lists... Done
Building dependency tree
Reading state information... Done
The following packages were automatically installed and are no longer required:
python3-click python3-colorama
Use 'sudo apt autoremove' to remove them.
The following additional packages will be installed:
libatk-bridge2.0-dev libatspi2.0-dev libdbus-1-dev libepoxy-dev libgtk-3-dev
libxkbcommon-dev libxtst-dev python-gi-cairo wayland-protocols
x11proto-record-dev
Suggested packages:
libgtk-3-doc
The following NEW packages will be installed:
libatk-bridge2.0-dev libatspi2.0-dev libdbus-1-dev libepoxy-dev libgtk-3-dev
libxkbcommon-dev libxtst-dev python-gi-cairo ros-melodic-smach-viewer
wayland-protocols x11proto-record-dev
0 upgraded, 11 newly installed, 0 to remove and 3 not upgraded.
Need to get 83.6 kB/1,551 kB of archives.
After this operation, 16.1 MB of additional disk space will be used.
Do you want to continue? [Y/n] y
Err:1 http://packages.ros.org/ros/ubuntu bionic/main amd64 ros-melodic-smach-viewer amd64 3.0.0-1bionic.20210414.224955
404  Not Found [IP: 64.50.233.100 80]
E: Failed to fetch http://packages.ros.org/ros/ubuntu/pool/main/r/ros-melodic-smach-viewer/ros-melodic-smach-viewer_3.0.0-1bionic.20210414.224955_amd64.deb
404  Not Found [IP: 64.50.233.100 80]
E: Unable to fetch some archives, maybe run apt-get update or try with --fix-missing?
</code>```

please can anyone help me solve this problem. Thank you

答: This looks like a standard apt problem. You need to make sure to run an apt-get update soon before you run apt-get install ... to make sure that you have the latest index of packages. If new versions have been released since you last did an update it will fail to find the packages with the 404 error you are seeing.


问:
rosrun does not appear to do anything. Node exits immediately
I have a ROS node that seems to exit immediately or not even run at all. I have test print statements before the main loop and it never seems to get to that point. roscore is running and no error messages appear in the terminal window.

```<code>#include <ros/ros.h>
#include <iostream>

#define ROS_LOOP_RATE 2000

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

    std::cout << "TEST1\n";
    ros::init(argc, argv, "network_wdg_node");

    std::cout << "TEST2\n";
    ros::NodeHandle wdgNodeHandle("~");

    std::cout << "TEST3\n";
    ros::Rate loop_rate(ROS_LOOP_RATE);

    std::cout << "TEST4\n";

    while(ros::ok())
    {
        std::cout << "INSIDE LOOP\n";
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
</code>```

Here's my terminal output:

```<code>[rosrun] Looking in catkin libexec dirs:
/home/catkin_ws/devel/lib/test
/home/catkin_ws/devel/share/test
/home/catkin_ws/src/test
[rosrun] Looking in rospack dir: /home/catkin_ws/src/test
[rosrun] Searching for network_wdg_node with permissions /111
[rosrun] Running  /home/catkin_ws/devel/lib/network_wdg/test_node
</code>```

I can't seem to figure out why the node doesn't start running or even get to the first debug statement.

答:
I don't have the ability to promote the above comment where the OP confirms the answer:

I solved/figure out the issue. I was running "rosrun test_node
test_node" when it was just building an executable named "node". It
was my fault for not reading CMakeLists closely enough


问:
ROS meta-package ros-melodic-desktop-full doesn't include mav packages
Why doesn't the installation of the meta-package ros-melodic-desktop-full on Ubuntu 18.04 Melodic include the packages ros-melodic-mav-msgs, ros-melodic-mav-msgs and ros-melodic-mav-planning-msgs?

答: The melodic desktop-full metapackages is defined in REP 150 with several of the other core metapackages.


问:
How rational is it to use Intel Realsense for my affordable indoor autonomous robot?
Right now, I am using ESP32 for the motors and planning to add either Raspberry Pi Zero or ARM Cortex-M4 (STM32) and use ROS with Intel RealSense RGBD camera.

How sensible is it? I am not expert in hardware and asking whether this will be rational and would such camera be computationally expensive? I am planing to do vSLAM with it.

答:
The Realsense sensors are reasonable to use on indoor autonomous robots.

"Affordable" is subjective, so I've left it out of my above response.

And if you want to use it make sure that you have enough compute capability and USB bus available to stream the data coming off the sensors. Depending on your use case and settings, smaller processors like the ones you're listing may have trouble operating on the full speed data.

Note that there's also some new information about future availability of Realsense sensors


问:
running UWSim commands in ROS
Where can I find a good documentation about the UWSim in ROS. Actually having the source files is not enough and it is actually hard to follow all the functions. for example, how can I use these command correctly :

& rosrun UWSim gotoAbsolutePosition   0 0 0 0 0 0

I know that there is a node 'gotoAbsolutePosition' in the Package 'UWSim' and I knwo the variables, but I cannot set the two topics properly.

答:
The primary entry point for UWSim ROS documentation is at: https://wiki.ros.org/uwsim
This has the basic commands for running it too.

There are example programs such as gotoAbsolutePosition which are mostly thin wrappers on the ROS api for convenience.

You can find all the examples of using the interface here: https://github.com/uji-ros-pkg/underwater_simulation/tree/melodic-devel/uwsim/interface_examples

And it's main documentation wiki is at: http://www.irs.uji.es/uwsim/wiki/index.php?title=Main_Page

With a guide for interacting with the simulator on this subpage: http://www.irs.uji.es/uwsim/wiki/index.php?title=First_steps:_Interacting_with_UWSim

Title: running UWSim commands in ROS


问:
Flipping an old manual switch (physical one)
I Have an old audio amplifier that has those switches to turn it on.
I'm looking for the simplest motor/robotic arm (or any other relevant component) to control this switch - eventually via Raspberry Pi .

Are there any options ?

答:
If you just want to control the switches and not the knobs, some options are:

Dual acting solenoids or two simple acting solenoids.

Needs just a pulse of current in one direction to operate. In some dual acting solenoids, you need to reverse current to reverse the direction of force.

Image from http://www.magnetschultz.co.uk

RC servo-motor. (With that you can control the knobs too)

Normally more force for a given size/weight of solenoid. Slower than a solenoid but depending on the model still in the $ ms $ for a 60° rotation. Has more moving parts than a solenoid, so more wear. Major ones have a brushed DC motor so have the brushes wear too. Anyway, should last for maybe a decade without problem. It's a servo-mechanism, so has position feedback and precision positioning. Have the drive electronics, but you need to provide a PWM signal relative to the position you want it to rotate.

Car door lock actuator
Shares some of the properties from a RC Servo-motor, but has linear action like the solenoid, and don't have position feed-back (some have a open/closed feed-back). Needs a pulse of current to operate too, but needs reversing the current to reverse the direction of force.

Image from http://www.amazon.com/

Note: I presume by your question that you want to mechanical actuate the switch and electronic options are excluded.


问:
Can 2 spaces with different dimensions be topologically equivalent?
I am a bit confused with topology and am reading the next section after configuration spaces. I bumped into the question and got really confused.</p>

答:
just google "topology equivalence"
https://www.tandfonline.com/doi/abs/10.1080/00029890.1960.11990308?journalCode=uamm20

Basically topology equivalence means, that there is some 1-1 correspondence continuous in both ways.

That mean, that exist some correspondence, here for point "near" point A on first space are corresponding points on second place "somehow near" point corresponding A.

Basically it means, that all object on one site have corresponding objects on other site and number of "holes" and "islands" in such holes and holes in that islands and ... are the same in corresponding objects. Nothing more.

Basically triangel, circle, square ... of any sizes, are topologically equivalent, as they are "single objects without holes"

The same goes for any cubes, balls, dodecaeders ... regardless size.

If you wil make the object from something, witch could be infinitelly strenchable and can it somehow streng and bend to the other shape, but without connect it or tear it anywhere, then the original and results are topologically equivalent


问:
Searching for object in an unknown environment using ROS
Using ROS, I'm trying to build a robot able to search for specific objects in an unknown environment. Right now, I'm experimenting with a mobile robot in a simple gazebo maze, using a LIDAR for navigation/mapping and its camera for object recognition on "STOP" signs. Here's a picture from the Husarion tutorial (now deprecated):

As a ROS beginner, I find myself writing more code than I expected, and I'm wondering if I'm actually reinventing the wheel. After some googling, these are my findings:

The find_object package, that seems without activity since 2010.

Husarion's object search tutorial, that's been deprecated and not supported anymore.

Is this task well-known and I'm reinventing the wheel? If that's the case, is there a standard/best-practice way to approach it? I would very much appreciate if you can point me to any books/videos/tutorials/examples.

答:
Is this task well-known and I'm reinventing the wheel?

To truly answer this, you will have to conduct a full literature review of what is out there. 'find objects' is a bit too vague. Could you be more specific as to what objects are you trying to detect and what sensors are available to you?

I'd say that general object detection is well know in robotics and there are several methods to detect objects but classifying them is another story and one that depends on your application and available resources.

If you only want to detect objects in lidar point clouds you can use something like euclidean cluster extraction. If your aim is to classify objects in the image space you could use some NN based approach. The darknet_ros package would be a good starting point.


问:
How do you integrate an external library to your ROS catkin_Workspace?
So following example:
I have Package A with the function doA(); and I have Package B inside my catkin_ws. Now If I want to use doA(); in package B I would add the following to the package.xml of B: <depend>A</depend> and it works.

Now the case with an external library:
So lets assume my Package A needs an external library outside of my catkin_workspace. For example something in /home/tom/lib/. Now I link this external library to my Package A by changing the CMakeLists.txt:

```<code>find_library(EX_LIB el HINT /home/tom/lib/)
target_link_libraries(A <span class="math-container">${EX_LIB})
target_link_libraries(test_A $</span>{PROJECT_NAME}
    ${EX_LIB})
</code>```

No when I try to run doA(); in Package B it will not work. The dependency in the package.xml is not enough, I have to link the external Library in the CMakeLists.txt of B too. Why? And how can I integrate that external library the right way into my catkin_ws,  so I do not have to link it in every package that uses Package A.

Edit:
I think my problem is similar to this:ROS-External-Wrapper , but in my case I do not have the source files only the prebuilt libraries.

答:
Adding a dependency to the package.xml allows rosdep to resolve dependencies prior to building, as well as provides a list of dependencies but that doesn't provide any information about the contents of the package such as the libraries to link against.

For native catkin packages they use the catkin_package macro to export flags which can be leveraged automatically by downstream packages. Here's a related question Catkin packages leverage those exports to make things more automatic. However for your "external" project those helper exports are not available. These are not magic either, they come from the CATKIN_DEPENDS too.

For your case you have an external library and you have to make sure to link it against your executable. This is true of every CMake executable with libraries.

If you're writing a library in A (libA) that needs to link against your external library at runtime, and then trying to link against libA from package B you need to add to the CMake for libA to export the dependency on your EX_LIB.


问:
Battery weight in quadcopter
We all know that the fly time of a quadcopter is proportional to its weight (and the propellers size, etc), and a high capacity battery is heavy, but also a high capacity battery would make the fly time larger.

I need a chart or comparison between battery capacity, battery weight, fly time and propellers size for a quadcopter. Or comparison between some of them. I couldn't find that, so any link or help would be appreciated

答:
Every quadcopter design is relatively unique and there's not one answer for any given system. Many factors can change these numbers, for example the type of blade, the material the angle of attack, the efficiency of the speed controllers, the shape of the body and payload, payload battery consumption, etc. There's other factors of things like your flight profile and the environment too which will effect performance.

But a few helpful calculators that I found quickly are:

https://www.omnicalculator.com/other/drone-flight-time This one in particular requires an "Watts to lift 1kg" as a way to capture the overall efficiency of the drone flight system.

Here's another generic one: https://design215.com/dcal/toolbox/flight-time-amps

Title: Battery weight in quadcopter


问:
How do you handle angle discontinuities in estimation problems?
When one is implementing a state estimator in a system that involves kinematics, will inevitably face the problem of angle discontinuities, i.e., the fact that the angles have to be wrapped between within [-pi -> pi), or between ( 0 -> 2pi ], or else estimation algorithms such as a KF will not work due to the possibility of the error state becoming biased.

Roughly speaking, imagine the following

$$
\hat{e}_k = \hat{\theta}_k - \theta_k, \quad \text{where} \quad\hat{\theta}_k=2\pi \quad \text{and} \quad \theta_k = 0
$$

In that case the estimated pose $\hat{\theta}$ matches the actual pose $\theta$, but the estimator is creating an error equal to $\hat{e}_k = 2\pi$. This error is propagated into the estimators correction step and is creating a correction to the pose that shouldn't occur.

What kind of problems have you in general faced concerning this discontinuity issue and what was your solution?

Thanks a lot,
A student form
Denmark

答:
As mentioned by @long-smith the standard solution is to use quaternions.

However if you specifically are asking how to deal with errors using angles that are modulo $2\pi$ you are going to want to add logic to compute the smallest angle between two angles which take into account wrapping.

An example is shorted_angular_distance from the ROS angles package which can bed used for your error metrics and deals with the wrapping edge cases.


问:
How to install ROS melodic over Network proxy
Maybe it is a repeated question, but really i have no solution

i am trying to install ROS melodic on Ubuntu 18.04.3 LTS, using my Wifi the things went good, but at my office where i have a desktop with restricted policy to use a proxy i have some problem

the ubuntu software & update can't install the updates even i have connected to internet and the proxy setting  was setupped in system setting

when i triedsudo http_proxy=myproxy:myport apt-get update the update process failed with this message some index failed to download, they have been ignored or old ones used instead

i try to install ROS even though the ubuntu updates were failed but again the terminal shows Reading Package list...Done
Building dependency tree
Reading state information...Done
Enable to locate package ros-melodic-desktop-full

i tried multiple solutions via google but all were failed.

Why isn't enough to setup the proxy from system setting one time ?

If i have windows10 which accessing the net using the same proxy, Can i get the update by connecting ubuntu to windows hotspot ?

If i take the hard disk where the ubuntu installed and try to get update using another computer, Can i get them correctly?

答:
Aswered in the comment from @Mjd Kassam comment above

the command "sudo http_proxy=myproxy:myport apt-get update" work fine,
it was a conflict in my IP


答:
The simplest way to have values between [0, 2$\pi$] is using the modulo operator.

You can simply do

$\hat{e}_k = (\hat{\theta}_k - \theta_k) \% 2\pi$.

Title: How do you handle angle discontinuities in estimation problems?

Body:
When one is implementing a state estimator in a system that involves kinematics, will inevitably face the problem of angle discontinuities, i.e., the fact that the angles have to be wrapped between within [-pi -> pi), or between ( 0 -> 2pi ], or else estimation algorithms such as a KF will not work due to the possibility of the error state becoming biased.

Roughly speaking, imagine the following

$$
\hat{e}_k = \hat{\theta}_k - \theta_k, \quad \text{where} \quad\hat{\theta}_k=2\pi \quad \text{and} \quad \theta_k = 0
$$

In that case the estimated pose $\hat{\theta}$ matches the actual pose $\theta$, but the estimator is creating an error equal to $\hat{e}_k = 2\pi$. This error is propagated into the estimators correction step and is creating a correction to the pose that shouldn't occur.

What kind of problems have you in general faced concerning this discontinuity issue and what was your solution?

Thanks a lot,
A student form
Denmark


问:
Fusion multiple LaserScans to PointCloud
I am currently trying to do 3D SLAM with two 2D Lidars and possibly additional IMU in ROS. However, there seems to be no ROS package available to meet my needs. The closest one would be laser_pipeline stack, but the documentation only tells me about fusioning LaserScans from "one source" although what I need is fusioning vertical scan and horizontal scan into one 3D PointCloud2. So I currently need another package to fusion scans from different sources. Anybody have an idea how to solve this problem?</p>

答:
I think that you will find that the laser_assembler in the laser_pipeline does exactly what you are looking for.

The laser_scan_assembler node accepts one topic as input. But you can remap or mux multiple laser scanners into that input topic and the assembler should be able to handle it. The algorithm should be using the actual headers from the incoming laser scans and consequently should be able to assemble them separately.

ALternatively you could just assemble the two scanners separately and then fuse the point clouds using the point_cloud_assembler node in the same package.

There's a tutorial for using the package to assemble laser scans


问:
Tracking vehicle 6 states extended kalman filter required?
I'm trying to track an accelerating vehicle using a camera, an IMU, and a GPS.
I use for the state space equation a constant acceleration model:

The states are the position, the velocity, and the acceleration of the vehicle in the x- and y-direction. No input is used.
All the measurement equations are linear.
I'm wondering if I should use a discrete normal Kalman filter, or a discrete extended Kalman filter. (i.e. is my process non linear?)

答:
there is some error in your matrix I think. P(k+1)=V(k) And
V(k+1)=A(k) so I don't kwon what is the A(k+1) maybe the jerk(jolt) of the vehicle.</p>


问:
Kalman Filter and the state noise vector?
I'm reading Probabilistic Robotics by Thrun. In the Kalman filter section, they state that
$$
x_{t} =A_{t}x_{t-1} + B_{t}u_{t} + \epsilon_{t}
$$

where $\epsilon_{t}$ is the state noise vector.  And in
$$
z_{t} = C_{t}x_{t} + \delta_{t}
$$
where $\delta_{t}$ is the measurement noise. Now, I want to simulate a system in Matlab. Everything to me is straightforward except the state noise vector $\epsilon_{t}$. Unfortunately, majority of authors don't care much about the technical details. My question is what is the state noise vector? and what are the sources of it? I need to know because I want my simulation to be rather sensible. About the measurement noise, it is evident and given in the specifications sheet that is the sensor has uncertainty ${\pm} e$.

答:
The noise term $\epsilon_t$ is meant to capture uncertainty in the transition model, e.g. slippage due to an imperfect friction model for wheels. In other words, components of the model that were not incorporated either because they add to much computational complexity or simply cannot be modeled, such as disturbances that cannot be known in advance. It is usually assumed to be a zero mean Gaussian distributed random variable vector of the same size as the state variable $x$ that is independent between time steps. In other words, $x, \epsilon \in \mathbb{R}^n$ where $n$ is the dimension of the state-space, $\epsilon \sim \mathcal{N}(0,\Sigma)$, $\Sigma$ is the covariance of $\epsilon$, and $\epsilon_t$ and $\epsilon_{t+1}$ are identical and independently distributed. These assumptions permit closed form solutions to the Kalman filter which in term makes it relatively efficient to compute.

The noise term $\delta_t$ is basically the same but represents uncertainty in the sensing model. Usually $\epsilon_t$ and $\delta_t$ are not known a priori and need to be learned via some system identification method or can be learned from data using something like Expectation-Maximization.


问:
Mapping Joystick Inputs to Quadcopter
hope you are all doing well.

We are doing a project where a quadcopter is controlled by an ordinary joystick. We have constructed the Control Algorithm and the joystick to move the quadcopter in accordance with the joystick inputs (roll, pitch, yaw_speed, z_speed).

However, I have some questions on my mind.

Right now, we designed a linear controller, where the roll and pitch inputs are saturated within +/- 20 degrees. Therefore, the joystick directly gives roll and pitch reference angles. For the yaw angle, we read joystick as the yaw_velocity. Then we integrate it to have the reference yaw angle. We use thrust input from the joystick as the velocity in z-axis. Is this a logical mapping from the joystick to the quadcopter for linear control? Is there any (kind of) universal mapping for linear control?

The second part of the project contains non-linear control of the quadcopter, where we aim to have the quadcopter do 360 spins. In order to achieve that, how should we map the joystick inputs? Should we read all of them as roll, pitch, yaw velocities?

My final question is: Or mapping these values to the body frame (p, q, r, and w) is a more preferred way?

Edit: Since I received no answers, I wanted to up the question (I will not be doing that continuosly, this is the first and last time I do that).

答:
Let me preface this by saying I've never done quadcopter controls before.

With a vehicle, the gas pedal doesn't control speed or position, it controls torque. For a quadcopter, the equivalent would be the horizontal forces. The roll/pitch angles would be an input to the horizontal force, but thrust is also an input and the relationship is nonlinear: $F_{horizontal} = Tsin(\theta)$.

If you want to have the joystick inputs map directly to the roll/pitch angles then I'm sure that's fine and you'll definitely get results, but I think a more elegant way to do it would be to have the joysticks map to the horizontal force and let your controller calculate the appropriate roll/pitch angles. In doing it this way your horizontal response becomes linear - double the joystick motion produces double the acceleration - and also becomes thrust-independent, in that you get the same planar performance while changing altitude as you do at constant altitude.

Mapping to velocity probably makes the most sense for altitude control.

Title: Mapping Joystick Inputs to Quadcopter


问:
Syntax highlighting for source code of urdf & sdf (xml-based) files on Github
Note: I posted this on stack overflow but couldn't get an answer. I am trying it here.

I have files that are in urdf [Unified Robot Description Format] for ROS and sdf [Simulation Description Format] for gazebo. URDF is xml but Github only shows it in black. How can I get Github to show syntax highlighting on the file? Can I do this for any file extension including a missing extension?

NOTE: I am not referring to comments in markdown so back ticks don't apply.

```<code><sdf version='1.6'>
  <world name='default'>

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <pose frame=''>0 0 30 0 0 0</pose>
      <uri>model://robot</uri>
    </include>

...

  </world>
</sdf>
</code>```

Stack Overflow's syntax highlighting seems to get it.

答:
The GitHub page on code blocks has a section on syntax highlighting that says:

Syntax highlighting

You can add an optional language identifier to enable syntax highlighting in your fenced code block.

For example, to syntax highlight Ruby code:

```ruby
require 'redcarpet'
markdown = Redcarpet.new("Hello World!")
puts markdown.to_html
```
```

More information is provided below that example:

We use Linguist to perform language detection and to select third-party grammars for syntax highlighting. You can find out which keywords are valid in the languages YAML file.

In reviewing the third-party grammars it looks like URDF specifically isn't included, but XML is. Since the URDF "is an XML format for representing a robot model," I'd just tag the block as XML.

:EDIT:

Just re-read and you say you're asking about files, not comments, but I checked a URDF file here and it looks like the following for me:

If there's some other issue you've got please link a specific example where it's not working for you.

Body:
The GitHub page on code blocks has a section on syntax highlighting that says:

Syntax highlighting

You can add an optional language identifier to enable syntax highlighting in your fenced code block.

For example, to syntax highlight Ruby code:

```ruby
require 'redcarpet'
markdown = Redcarpet.new("Hello World!")
puts markdown.to_html
```

More information is provided below that example:

We use Linguist to perform language detection and to select third-party grammars for syntax highlighting. You can find out which keywords are valid in the languages YAML file.

In reviewing the third-party grammars it looks like URDF specifically isn't included, but XML is. Since the URDF "is an XML format for representing a robot model," I'd just tag the block as XML.

:EDIT:

Just re-read and you say you're asking about files, not comments, but I checked a URDF file here and it looks like the following for me:

If there's some other issue you've got please link a specific example where it's not working for you.


问:
How do you end up with 1 degree of freedom with one big link connecting 3 other links?
I was reading Modern Robotics by Kevin Lynch et al. and this robot was presented. They had introduced Grubler's formula which assumes independent joint which gives $dof = 0$ but this is not the case for the robot and they provide an alternative computation but it isn't clear how they got the values for Grubler's formula for this.

I understand that this is Grubler's formula and this is also from the book.

The part that is unclear to me is what is highlighted in red.

答:
Where they say, "...any one of the three parallel links...has no effect on the motion" that pretty much means you could delete any one of the three links. Doing so leaves N=4 and J=4. Plug them into the formula and get the numbers you highlighted in red.

The conclusion "not independent, as required" is sort of a proof by contradiction. More precisely: The thing can move, with a single dof, only in the specific case where the "parallel" links are indeed parallel, and the same length, making one of the three redundant (i.e. not independent). Otherwise (the general case, with weird lengths and angles) the system would be overconstrained and indeed couldn't move (dof=0) as the formula computes.


问:
Robotics Trends
Robotics has always been one of those engineering fields which has promised so much, but is taking a long time to deliver all the things that people imagine.

When someone asks: "How long before we have [X] type of robots?" Are there any resources we can call upon to try to calculate a rough answer. These resources might include:

Rate of progress of computational power, and some estimate of how much will be needed for various types of AI.

Rate of progress of electrical energy storage density, and some estimate of how much will be needed for various types of robot.

Rate of progress of actuation systems, and some estimate of what would be needed for various types of robot.

Lists of milestones towards various types of robot, and which ones have been achieved and when.

Are these types of studies performed, and are the results published?

Added:

In response to Jakob's comment, I am not looking for opinions or discussions on this subject. What I am looking for are published studies which might shed light on this question.

答:
In my opinion, the question "How long before we have [X] type of robots?" is flawed. It assumes some kind of linear progress towards a goal that we already know, but scientific progress doesn't work that way. One cannot honestly say "if we had 100x of today's computing power, we could achieve human-level AI" (although it sounds like something Ray Kurzweil might say). Also, we cannot predict today what robots 50 years from now will look like.

If you had asked anybody in the sixties about the future of computers, they wouldn't have predicted that all of us carry around a super computer in our pockets which we use to publicly answer a question asked by somebody on the other side of the planet.

Title: Robotics Trends


问:
How to know the desired orientation of a quadcopter?
I am trying to simulate a quadcopter model on Simulink. I want to implement a PID controller for each of X,Y,Z and phi,theta, psi angles. PID gets the error, as input, which is to be minimized.
For the X,Y and Z, the desired values are entered by the user and the actual values are calculated from the accelerometer data, hence, the error is the desired set value - actual value.

For phi,theta and psi, the actual values may be obtained from the gyroscope and accelerometer (sensor fusion) but I don't actually know how to calculate the desired values for each one of them since the user is usually interested in giving the position values X,Y and Z as desired not the angle values! The absence of the desired values prevents me form calculating the angular error which is needed for the PID controller.

答:
Defining the state of the quadcopter as $\bf{x} = \left[ \begin{matrix} \mathbf{p} & \mathbf{v} & \mathbf{r} & \mathbf{w} \end{matrix} \right]^T$ where $\mathbf{p}$, $\mathbf{v}$, $\mathbf{r}$, and $\mathbf{w}$ are the position, velocity, angular position, and angular velocity of the quadcopter respectively. A simplified transition model for a quadcopter using PD control is:
\begin{align}
\dot{\mathbf{p}} & = \mathbf{v} \\
\dot{\mathbf{v}} & = -g\mathbf{e}_3 + \text{exp}([\mathbf{r}])\mathbf{e}_3u_1/m \\
\dot{\mathbf{r}} & = \mathbf{w} + \frac{1}{2}[\mathbf{r}]\mathbf{w} + (1 - \frac{||\mathbf{r}||}{2\text{tan}(||\mathbf{r}||/2)})\frac{[\mathbf{r}]^2}{||\mathbf{r}||^2}\mathbf{w} \\
\dot{\mathbf{w}} & = \begin{bmatrix} k_1(u_2 - r_x) + k_2w_x \\ k_1(u_3 - r_y) + k_2w_y \\ 0 \end{bmatrix}
\end{align}
where $\mathbf{e}_3 = \left[ \begin{matrix} 0 & 0 & 1 \end{matrix} \right]^T$, $[\mathbf{r}]$ represents the skew-symmetric matrix of $\mathbf{r}$, $||\mathbf{r}||$ represents the magnitude of $\mathbf{r}$, $k_1$ and $k_2$ represent the proportional and derivative gains respectively, and the control $\mathbf{u} = \left[ \begin{matrix} u_1 & u_2 & u_3 \end{matrix} \right]^T$ is comprised of the desired total thrust $u_1$, the desired roll angle $u_2$, the desired pitch angle $u_3$, and assumes yaw remains the same. Note that this uses a PD controller because the integration term I is generally not useful for trajectory following.

Using this model you can, given the current state $\mathbf{x}$, calcualte how the state will change. This of course does not give you the desired angular positions about which you asked. Assuming the user expects the quadcopter to hover once it has reached the specified desired position $\mathbf{p} = [X, Y, Z]^T$ then we need $\mathbf{v} = \mathbf{r} = \mathbf{w} = \left[ \begin{matrix} 0 & 0 & 0 \end{matrix} \right]^T$ for the final state.

However that still does not give you the desired angles to transition between the initial state $\mathbf{x}_i$ and the final state $\mathbf{x}_f = \left[ \begin{matrix} \mathbf{p} & \mathbf{v} & \mathbf{r} & \mathbf{w} \end{matrix} \right]^T$. To get that you need a higher level controller to calculate a trajectory $\pi = (\mathbf{x}[], \mathbf{u}[], \tau)$ with $\mathbf{x} : [0, \tau] \rightarrow \mathcal{X}$, $\mathbf{u} : [0, \tau] \rightarrow \mathcal{U}$ for the state-space $\mathcal{X}$ and control space $\mathcal{U}$ and some travel time $\tau$. This trajectory will give you the desired angles at any given moment.

Unfortunately there are a lot of ways to calculate this trajectory. One possibility is to look at my own work in this area. Specifically my paper titled Kinodynamic RRT*: Asymptotically Optimal Motion Planning for Robots with Linear Dynamics.


问:
If a point in a sphere can be given by longitude and latitude, why isn't a torus (also representing 2 dimensions) its configuration space?
In Modern Robotics by Kevin Lynch et al, there is mention of the difference between topology and sample representation as shown by the figure below.

I am curious as to why the topology of a point is a sphere and not a torus.  Both represent 2 dimensions and you get wrapping in the torus too since the circular portions allow wrapping.

I think it is the topology of 2 revolute joints because they can move by 360 degrees but saw can a point on a sphere.

答:
I am not entirely sure what type of answer you're looking for but one difference is that on a sphere, the two degrees of freedom are somehow more coupled than in a torus.

On a sphere, you can do a half revolution along one degree of freedom, and another half revolution along the other degree of freedom and end up back where you started.

On a torus, this is not the case. You will end up much "farther" than where you started.

To use the robotics examples, a spherical pendulum can return to its rest position after rotating upwards 180 degrees by rotating down 180 degrees in any direction. With a 2D robot arm, rotating one of the arms will never move the other arm, and so rotating arm 1 360 degrees results in a different state than rotating arm 1 180 degrees and then rotating arm 2 180 degrees.

Also see https://en.wikipedia.org/wiki/Hairy_ball_theorem. Smooth and continuous vector fields exist on the torus but not on the sphere. This has consequences for controllers on those spaces.

They are also different using the standard math definition. A sphere cannot be continuously deformed into a torus.


问:
Why are serial manipulators almost always made of single dof joints?
I am currently taking my first robotics class and I found it curious that all serial manipulators have single dof joints. Why not use joints with more dof and reduce the number of components? Will that increase the error?</p>

答:
Conceptually multiple-dof join is a series of 1-dof joints in sequence. Such decomposition allows you to model your control system and do stuff like inverse- and forward kinematics.

For example - if you have something like a 'ball-joint', 2-dof, you will still need two motors to control it, two control signals to calculate (and transmit), each of them might have their own errors, etc. And you will use two encoders to read this joint position. And still you can model such joint as two 1-dof in sequence.


问:
Is there a device for detecting what room you are in of a building?
I have found some inspiration for creating an smartphone application. But one of the main segments of the app requires it to know where the smartphone is located. It doesn't have to be that precies. It needs to know if you are in office A, office B, the coffee corner, ...

Is there any device/solution for this? I am thinking of an device that sends out a bluetooth connection that a smartphone can pickup in a 5m radius. Or maby a type of nfc tag for a greater radius that a smartphone can pick up?

If there is any app/solution for this idea yet, please let me know.

Any answer is highly appreciated!
Thanks for thinking along!

答:
At a corporate commercial scale, there’s devices like the Cisco wireless location appliance https://www.cisco.com/c/en/us/products/wireless/wireless-location-appliance/index.html.

At a hobbyist scale I think the low range beacon strategy you mention could work, but not aware of any products.


问:
Air Resistance Drag Force Calculation For Quadcopter
Hello there. Hope you are all doing well.

I have created a dynamic quadcopter model on Simulink and designed a controller for it. So far, everything is going well. However, since I am going to be implementing my control algorithm on a real quadcopter, I wanted to incorporate some aerodynamic effects as well. I used "Modelling and control of quadcopter" paper by Teppo Luukkonen as reference.

On the paper he formulates the aerodynamic effects as shown below (he mentions that there are many effects which could have been included as well):

In another part of the paper; he assigns 0.25 kg/s for coefficients Ax, Ay, and Az. I just want to know how he calculated this matrix of Ax, Ay, and Az. Is there a formulation? Can it be generalized for any quadcopter? Can you show me a source where I can get this information?

答:
A linear function of velocity is one of the simplest ways to model the drag force.

Finding good values for the three coefficients is usually determined by collecting data from a quadrotor, and picking those values for which this linear model predicts the acceleration closest to that measured from the real data. E.g. minimizing the sum of squares of the error over all the data points in a least squares fashion, this can be done using a large linear system.

This will only work well for modeling drag if the weight and thrust are well known and there are no other forces acting on the quadrotor. Even still, as mentioned, drag is not perfectly linear, and there are other forces due to the impact of air such as blade flapping.

In general, a dynamical model can be learned, using data collected from a real quadrotor, by optimizing a cost function capturing the difference between the model's prediction and the real trajectory, over some chosen set of parameters. In this case, the parameters are the three linear coefficients for velocity, but better models can be built using a more expressive parameterization. For example, a linear function of the quadrotor state vector projected onto some high dimensional feature space.
See https://people.eecs.berkeley.edu/~brecht/papers/07.rah.rec.nips.pdf for this regression method.

In addition to using trajectory prediction error to optimize for unknown model parameters, you can also use trajectory tracking error. The principle is that, if your controller successfully inverts the dynamics model, then a low tracking error should imply low prediction error and vice-versa. That is, if you can predict well, you can control well.

See https://arxiv.org/pdf/1712.02402.pdf for a method that uses trajectory tracking error as a means of optimizing for the drag coefficients (Section VI).

There are many many other papers that model quadrotors and learn models from data; you can use the references in the above paper as a head start.

Hope this helps!


问:
Multiple Rotations via Matrix Multiplication
so I am having an issue understanding why you right multiply for current frame rotation vs left multiply for fixed frame.

Let us say you want to: Rotate about x, then Rotate about current y, then rotate about newer z.

So we have Rx, then Ry'(prime because it is the new y, not original) and then Rz''

It makes sense that it should be  like this

You take a vector P and you rotate it about x to create a new vector p'. That is p'=Rxp.
Then you take this new vector (p') and rotate it about z to create p'', that is:
p''=Rz''p' = Rz''(Rxp)

That means that you left multiply when you are doing successive rotation about the current frame. However it is the opposite, and I can't understand why...

答:
Something to remember about rotation matrices is that they have multiple applications (that each has a different role in rigid-body motion):

representing an orientation relative to a reference frame,

changing the reference frame of a variable (e.g., a vector), or

rotating a vector or a frame without changing its reference frame.

The first half of your question is talking about what happens when you need to produce a chain of successive rotations, which is only applicable when you want to represent the final orientation relative to the original fixed frame. You could then use this relative orientation to change the reference frame of a variable from the fixed frame to the "current" frame or to rotate a vector or frame to align with the "current" frame without changing its frame of reference.

However, the second half of your question is talking about rotating the vector p relative to the same fixed frame twice in succession, so the fact they are both left-multiplied makes sense. You would need to change the frame of reference of the vector in order for the phrase "current" frame to make sense since you haven't defined any frame of reference (or any frame) other than the fixed frame.

Check out section 3.2.1 of the Modern Robotics textbook for a more thorough explanation or have a look at the associated video on YouTube: https://www.youtube.com/watch?v=6KIPusOv5fA.


问:
How does Lidar penetrate jungle canopy for archeological surveys?
I've read articles like these https://www.nationalgeographic.com/history/article/maya-laser-lidar-guatemala-pacunam and wanted to understand what's going on here. My understanding is the lidar would mostly bounce off of the canopy but to create maps like the ones in the article you would need to generate height maps from below the canopy.

Is there some sort of filtering of the point cloud that's going on? Do enough of the pulses reach the ground and back that meaningful data can be collected? I'm just wondering how surveyors able to generate meaningful maps of what what the canopy is hiding and make these amazing discoveries?

Here's an example image:

答: I saw the article a couple of years ago and thought the same. Looks like filtering is the only option. If they flew the drone near the surface, few of the multi-million beams might have hit the ground and I guess they were extracting the lowest points to generate the ground mesh.</p>


答: Google research indicates there that seem to be multiple techniques. Some involving multiple looks from different angles (see "High-resolution foliage penetration with gimbaled lidar"). Some involving multiple lasers where the interference between two beams cancels out the foliage return (see "Lidar system able to penetrate foliage and map obscured environments". This seems to also be called "gated digital holography" (see "Seeing the forest through the trees with a new LiDAR system"). Filtering for flecks of the ground as you suggested seems to be the basis for most of the techniques.

Title: How does Lidar penetrate jungle canopy for archeological surveys?

Body:
I've read articles like these https://www.nationalgeographic.com/history/article/maya-laser-lidar-guatemala-pacunam and wanted to understand what's going on here. My understanding is the lidar would mostly bounce off of the canopy but to create maps like the ones in the article you would need to generate height maps from below the canopy.

Is there some sort of filtering of the point cloud that's going on? Do enough of the pulses reach the ground and back that meaningful data can be collected? I'm just wondering how surveyors able to generate meaningful maps of what what the canopy is hiding and make these amazing discoveries?

Here's an example image:


问:
Confusion over configuration space, workspace and task space
I have been watching Kevin Lynch's videos on Coursera in this series on the basics of robotics. From that I gathered these are the definitions.

configuration space is the space of all configurations of the robot.

workspace is a specification of the configurations that the end-effector of the robot can reach

task space is a space in which the robot's task can be naturally expressed

Is the workspace contained in the configuration space since the end-effector is part of the robot? I understand that the configuration space is for each rigid link of the robot from this answer and the end-effector is a link.

Is it possible to have a task space that is beyond the configuration space?

答:
Two of these terms describe mathematical spaces (Configuration and Task spaces), and one (Workspace) the physical space in which the robot operates. Acknowledging they are related, but they are not the same "type of thing".

I recommend having a look at the QUT Robot academy videos by Peter Corke:

Task and Configuration Space

and Robot Workspace.

Which define configuration space as the mathematical set of all possible joint configurations. Task space the mathematical set of all possible end effector poses. And, workspace as the physical volume of all reachable end point positions".

No I don't believe it's possible to have a task space "beyond" the configuration space.


问:
4 Axis Gimbal Nodal Robot DH Parameters
I'm working on a robot gimbal where there are 4 revolute joints (4th axis for gimbal lock, reducing inertia etc). The robot is nodal such that all axes have the same nodal point and rotating any axes keeps does not change the position of the end effector, only changes its look direction.

Normally for kinematics I define my robots just with trig. This time I'm trying to learn how to define the robot with DH parameters as it seems like every article I've read about 4 axis gimbals has used DH to define the model so I need to understand it.

Below is my kinematics diagram of the robot in its home position and what I'd like to check is that I've correctly defined my frames. I've been a bit confused with finding the common normal as the Xi distance to Zi-1 is zero so I found it hard to visualize that. I think I finally got it correct though.

The grayed out part of this model is just a support bearing. I added
it for my own clarity as that's how the actual robot looks.

This is then the DH parameter table I came up with. I think the only length value that matters is L1. Since the robot is nodal, the length of the links 2 and 3 could be anything and it wouldn't affect it so I put them as zero, but the distance from the base to the robots nodal point does matter.

Link
Type
θ
α
r
d

1
R
0
π/2
0
L1

2
R
π/2
π/2
0
0

3
R
0
-π/2
0
0

Did I define my model frames/DH parameters correctly?

答:
i had corrected the kinematic diagram as per the right hand rule,

and the dh parameters are,

i had updated thekinematic diagram with the 5th frame (for camera mount)

Title: 4 Axis Gimbal Nodal Robot DH Parameters


问:
Implementing PID on a line following bot using turtlebot
I am using ROS melodic,turtlebot 2 on Ubuntu 18.04.

The idea is to create an environment consisting of lines as a path (slightly curved), and to program the turtlebot to follow the lines. Basically, a line following bot.

The idea is to click photos using the camera mounted on the turtlebot,process them to segment out the path which needs to be followed, and then control the bot's orientation, by using PID to control the angular velocity, so that it moves along the path.

I have created a program called take_photo_mod.py, which successfully keeps clicking photos and saving them as image.jpg, at a frequency that can be controlled.

```<code> #!/usr/bin/env python
    from __future__ import print_function
    import sys
    import rospy
    import cv2
    from std_msgs.msg import String
    from sensor_msgs.msg import Image
    from cv_bridge import CvBridge, CvBridgeError

    class TakePhoto:
        def __init__(self):

            self.bridge = CvBridge()
            self.image_received = False
            img_topic = "/camera/rgb/image_raw"
            self.image_sub = rospy.Subscriber(img_topic, Image, self.callback)
            rospy.sleep(1)

        def callback(self, data):

            try:
                cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            except CvBridgeError as e:
                print(e)

            self.image_received = True
            self.image = cv_image

        def take_picture(self, img_title):
            if self.image_received:

                cv2.imwrite(img_title, self.image)
                rospy.loginfo("received")

                return True
            else:
                return False

    if __name__ == '__main__':

        rospy.init_node('take_photo', anonymous=False)
        camera = TakePhoto()

        while not rospy.is_shutdown():

            img_title = rospy.get_param('~image_title', 'image'.jpg')
            if camera.take_picture(img_title):
                rospy.loginfo("Saved image " + img_title)
            else:
                rospy.loginfo("No images received")

            rospy.sleep(1)
</code>```

As for the next part, I plan to create a program go_forward.py, which incorporates two things:

Part 1. The openCV aspect: processing the image clicked to segment out the path. I have successfully managed to do this:

```<code>import cv2
import numpy as np
import math as m

def contor(lst):
    if len(lst)>1:
        m = list()
        for i in lst:
            for j in i:
                m.append(j[0])
        m = np.array(m)
    else:
        m=np.array([[w//2,h-150],[w//2,h-40]])
    print("m=",m)
    m = m[m[:, 1].argsort()]
    return m

def aver(i, p):
    a = p[i]
    y = int(np.mean([a[:, 0]]))
    x = int(np.mean([a[:, 1]]))
    return (y, x)

def extract(frame):
    theta=0
    phi=0
    print("h,w =",h,w)
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lb = np.array([0, 0, 0])
    ub = np.array([255, 50, 160])
    mask = cv2.inRange(hsv, lb, ub)
    res = cv2.bitwise_and(frame, frame, mask=mask)

     res2 = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
        ret, thres = cv2.threshold(res2, 100, 255, cv2.THRESH_BINARY)
        countours, hierarchy = cv2.findContours(thres, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
        cv2.drawContours(frame, countours, -1, (255, 0, 255), 3)

        points = contor(countours)
        i = np.where((points[:, 1] > h - 80) & (points[:, 1] < h))
        #print('i1', len(i[0]))
        if len(i[0]) > 0:
            end = aver(i, points) #coordinates (x2,y2)
        else:
            end = (w // 2, h)
        i = np.where((points[:, 1] > h - 190) & (points[:, 1] < h - 110))
        #print('i2', len(i[0]))
        if len(i[0]) > 0:
            strt = aver(i, points) #coordinates (x1,y1)
        else:
            strt = (w // 2, h - 200)

        frame=cv2.line(frame,strt,end,(255,0,0),9)
        cv2.imshow("aa",frame)
        cv2.waitKey()
        cv2.destroyAllWindows()

        if (strt[0] - end[0]) == 0:
            phi=m.pi/2 #angle with horizontal
            theta=0 #angle with vertical
        else:
            slope = (strt[1] - end[1]) / (end[0] - strt[0])
            phi=m.atan2(slope) #angle with horizontal
            theta= (m.pi/2) - phi #angle with vertical

        return theta

    frame=cv2.imread("image.jpg",1)
    h, w, _ = frame.shape
    i,d=0,0
    error=extract(frame)
    print(error)
</code>```

image.jpg ( which I was using for testing the openCV aspect. When take_photo_mod.py runs , image.jpg will keep changing at the frequency with which the bot will take photos)

This draws a line that approximately gives the direction along the path. Further, this calculates the angle with the angle with the vertical.

Part 2. PID aspect: This is the part with which I am struggling. The camera is mounted on the bot, so whatever we are calculating on the image is in the bot's frame,and the orientation of the bot in the bot's frame is along the vertical. Hence the angle which we calculate with the vertical acts as the "error" term: the angle by which the bot should turn to be along the path. Now, I think it's the angular velocity which will serve as the control signal.

I already have a script that just makes the bot go forward till its terminated.It had  angular velocity fixed at 0. So,the program "goforward.py" will basically be the combination of the openCV aspect ,and tweaking the "simply go forward" script to use PID to set it's angular velocity (instead of it being fixed at 0). Then, the two programs(nodes) take_photo_mod.py and goforward.py will be launched together using a launch file.

What I tried for goforward.py:

```<code>#!/usr/bin/env python

import rospy
import cv2
import numpy as np
import math as m
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist

#openCV aspect

def contor(lst):
    if len(lst)>1:
        m = list()
        for i in lst:
            for j in i:
                m.append(j[0])
        m = np.array(m)
    else:
        m=np.array([[w//2,h-150],[w//2,h-40]])
    #print("m=",m)
    m = m[m[:, 1].argsort()]
    return m

def aver(i, p):
    a = p[i]
    y = int(np.mean([a[:, 0]]))
    x = int(np.mean([a[:, 1]]))
    return (y, x)

def extract(frame):

    h, w, _ = frame.shape
    #print(h,w)
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lb = np.array([0, 0, 0])
    ub = np.array([255, 50, 160])
    mask = cv2.inRange(hsv, lb, ub)
    res = cv2.bitwise_and(frame, frame, mask=mask)

    res2 = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
    ret, thres = cv2.threshold(res2, 100, 255, cv2.THRESH_BINARY)
    countours, hierarchy = cv2.findContours(thres, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    cv2.drawContours(frame, countours, -1, (255, 0, 255), 3)

    points = contor(countours)
    i = np.where((points[:, 1] > h - 80) & (points[:, 1] < h))

    if len(i[0]) > 0:
        end = aver(i, points)
    else:
        end = (w // 2, h)
    i = np.where((points[:, 1] > h - 190) & (points[:, 1] < h - 110))

    if len(i[0]) > 0:
        strt = aver(i, points)
    else:
        strt = (w // 2, h - 200)

    if (strt[0]-end[0]) == 0:
        phi=m.pi/2 #angle with horizontal
        theta = 0 #angle with vertical
    else:
        slope = (strt[1] - end[1]) / (end[0] - strt[0])
        phi=m.atan2(slope) #angle with horizontal
        theta= (m.pi/2) - phi #angle with vertical

    return theta

#PID

def PID (kp,ki,kd,dt,error):

#making the bot move

class GoForward():

    def _init_(self):
        rospy.init_node('GoForward', anonymous=False)
        rospy.loginfo("To stop TurtleBot CTRL + C")
        rospy.on_shutdown(self.shutdown)
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
        r = rospy.Rate(10)
        move_cmd = Twist()
        move_cmd.linear.x = 0.2
        move_cmd.angular.z = 0

        while not rospy.is_shutdown():
            frame=cv2.imread("image.jpg",1)
            error=extract(frame)
            w=0
            kp=0.15
            ki=0.02
            kd=0.02
            dt=0.01
            w= PID(kp,ki,kd,dt,error)
            velocity_control=0.5 #when the bot is turning, its velocity should be slowed down.
            move_cmd.linear.x = 0.2-(veclocity_control *abs(w))
            move_cmd.angular.z = -w  #-sign because we are controlling angular velocity in -z direction (clockwise),if the error is positive, and vice versa
            self.cmd_vel.publish(move_cmd)
            r.sleep()

    def shutdown(self):
        rospy.loginfo("Stop TurtleBot")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

if _name_ == '_main_':
    try:
        GoForward()
    except:
        rospy.loginfo("GoForward node terminated.")
</code>```

The problem I'm having is to implement the PID part properly. How exactly should I approach writing the PID (...) function?

Something I tried: (pseudocode):

```<code>def pid_controller (kp,ki,kd,dt,error):

    e_prev = error
    e_sum = 0

while  :

    e=?
    e_sum = e_sum + e *dt
    dedt = ( e - e_prev ) / dt
    w = kp * e + ki * e_sum + kd* dedt
    e_prev = e

return w
</code>```

I've got it in bits and pieces but I still don't know how the full thing should work exactly.... for instance when should the loop terminate? What exactly should I have in place of e and e_prev?

答:
You're defining

```<code>def pid_controller (kp,ki,kd,dt,error)
</code>```

... the error is your e. The alternative would be to define

```<code>def pid_controller (kp,ki,kd,dt,theta_reference,theta_feedback)
</code>```

and have

```<code>error = theta_reference - theta_feedback
e = error
</code>```

Body:
You're defining

def pid_controller (kp,ki,kd,dt,error)

... the error is your e. The alternative would be to define

def pid_controller (kp,ki,kd,dt,theta_reference,theta_feedback)

and have

error = theta_reference - theta_feedback
e = error


问:
UKF for a serie of observations with covariance
I have some doubts about how to implement a UKF-like algorithm when I only have motion observations and no control inputs.

Assume I have a robot with state $s_t = (x_t, y_t, \theta_t)$ and the corresponding 3x3 covariance matrix $P_t$. I obtain a serie of $k$ measurements $z_i = (\Delta x_i, \Delta y_i, \Delta \theta_i)$ for $i = t \: ... \: t+k-1$, each with covariance $Z_i$. I need to compute the covariance of the resulting state $s_{t+k}$.

Since the transformation from $s_t$ to $s_{t+k}$ is non-linear, I need to approximate the posterior distribution of $s_{t+k}$ with a Gaussian and I am trying to use a UKF-like approach.

My problem is that I do not exactly understand how I should use both the covariance of the state $s_t$ and that of the measurements $z_t \: ... \: z_{t+k-1}$. I already have the nonlinear function $g$ such that $s_{t+1} = g(s_t, z_t)$ and I compute all the intermediate states $s_t \: ... \: s_{t+k}$. But I do not have a control input (generally called $u_t$) so is the prediction step just to compute sigma points from $P_t$? And in the update step how do I take into consideration the measurement covariance $Z_t$?

答:
It seems that since your "measurements" are just deltas in the state, they should really be considered your control inputs. Intuitively, measurements are supposed to reduce the variance in your state estimate, and controls inputs will increase it. So to me it sounds like you have a problem with only control inputs (deltas to the state) and no measurements. Based on the info, there is no way to compute deltas to the state from the state itself, so it's not clear what the measurement function is.

So with that in mind, in this case, you can propagate your covariance forward using the propagation (prediction) equations, taking into account the covariance of each "control input". Note that the process noise in the KF prediction step can vary at every timestep. But keep in mind that your covariance will grow indefinitely, unless you add some measurements. If you can only observe a noisy version of your velocity, your position estimate will drift indefinitely.


问:
process has died , exit code -11
I installed this framework and followed the instructions, but when I run it, I get the following error:

ros melodic
ubuntu 18.04

```<code>... logging to /home/mobiiin/.ros/log/5a6f3b96-10a2-11ec-b5bc-1002b5cd5c12/roslaunch mobiiin-ubu-22317.log

Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://mobiiin-ubu:40883/

SUMMARY
========

PARAMETERS
 * /dxslam/config: realsense_d435.yaml
 * /dxslam/depth_topic: /camera/depth/image
 * /dxslam/feature_topic: camera/color/feat...
 * /dxslam/image_topic: /camera/rgb/image...
 * /dxslam/pub_tf_child_frame:
 * /dxslam/pub_tf_parent_frame: map
 * /dxslam/reference_frame: vslam_origin
 * /dxslam/vocabulary: DXSLAM.fbow
 * /rosdistro: melodic
 * /rosversion: 1.14.11

NODES
  /
    dxslam (dxslam_ros/dxslam)
    map_vslam_tf (tf/static_transform_publisher)

auto-starting new master
process[master]: started with pid [22334]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 5a6f3b96-10a2-11ec-b5bc-1002b5cd5c12
process[rosout-1]: started with pid [22354]
started core service [/rosout]
process[map_vslam_tf-2]: started with pid [22365]
process[dxslam-3]: started with pid [22382]
File does not exist: DXSLAM.fbow
File does not exist: realsense_d435.yaml
Vocabulary: /home/mobiiin/git/dxslam/Vocabulary/DXSLAM.fbow
Config: /home/mobiiin/catkin_ws/src/dxslam_ros/config/realsense_d435.yaml
[dxslam-3] process has died [pid 22382, exit code -11, cmd /home/mobiiin/catkin_ws/devel/lib/dxslam_ros/dxslam __name:=dxslam __log:=/home/mobiiin/.ros/log/5a6f3b96-10a2-11ec-b5bc-1002b5cd5c12/dxslam-3.log].
log file: /home/mobiiin/.ros/log/5a6f3b96-10a2-11ec-b5bc-1002b5cd5c12/dxslam-3*.log
</code>```

this is the file that fails while running. it doesn't go further than line 218.

and it has the correct path to vocabulary and yaml file

any help is appreciated

p.s.: there isn't any log file whatsoever, I don't know why it mentions the log file at the end

heres a part of the code where it reports it cant find the files then it goes forward:
I think there's a possibility that this is not actually the issue

```<code>auto exist = [](std::string filename) {
    std::ifstream fs(filename);
    if (!fs.good()) std::cerr << "File does not exist: " << filename << std::endl;
    return fs.good();
};

auto directory = [](std::string path) {
    size_t pos = path.find_last_of("\\/");
    return (std::string::npos == pos) ? "" : path.substr(0, pos);
};

// find the specified filenames in the same folder of the default vocabulary/config
if (!exist(p_vocabulary_)) {
    p_vocabulary_ = directory(default_vocabulary) + "/" + p_vocabulary_;
    if (!exist(p_vocabulary_)) return false;
}
if (!exist(p_config_)) {
    p_config_ = directory(default_config) + '/' + p_config_;
    if (!exist(p_config_)) return false;
}

std::cout << "Vocabulary: " << p_vocabulary_ << "\n";
std::cout << "Config: " << p_config_ << "\n";
</code>```

P.S: I've found that this line is the one causing the problem:
could it be an opencv issue?

```<code>cv::FileStorage config_fs(p_config_.c_str(), cv::FileStorage::READ);
</code>```

答:
turns out the problem was my opencv 4.2 interfering with opencv 3.2 which is installed by default through ros melodic.

After taking Alex's seggestion and installing GDB.
I ran the launch file with GDB and it was in fact a segmentation fault. While tracing back the issue I saw couple of lines pointing to opencv3.2.so and others pointing to opencv4.2.so. after uninstalling the opencv and rebuilding the framework and ros package the issue got resolved. Thanks again to @Alex

Title: process has died , exit code -11


问:
Designing own robot for Gazebo simulation?
For my project I am trying to model and control a quadrotor with actuating arms. As I don't have a physical model with me, I want to simulate and control the robot using ROS and Gazebo.  </p>

<p>At the moment I have created a CAD design (servos are used to rotate the arms). I have read about URDF and know that I can define stuff such as sensors, velocity, inertia etc. The question here is as to how I can link the urdf with my cad model. How can I make the propellers and servos move to make the robot fly? </p>

答:
Modelling a quadcopter is a complex process. I would highly recommend starting with an existing model which is fully operational and then modifying it or extending it to match your design. You're going to need a lot more components than just converting your CAD model to URDF to get the simulation going.

There's a large variety of open source examples you can start with in the PX4 project. I recommend checking out the User Guide or the Developer Guide as starting points.


问:
Getting started with motion planning
I have recently finished the coursera Motion Planning course and I am looking for a project to do, using ROS, OpenRave, Gazebo and similar tools. My project would be in the area of motion planning for a mobile robot. The objective is to take a turtlebot from point A to point B in an environment with dynamic obstacles.

I am looking for suggestions regarding the software architecture and how to connect the above mentioned tools together.

答:
If you're interested in doing generic motion planning the best place to start is with the MoveIt Project

If you specifically want to do ground navigation, the ROS navigation stack has been a standard starting point. However if you're getting started now I'd recommend getting involved with nav2 which is an iteration on the design which is being developed for ROS 2.

Title: Getting started with motion planning


问:
Which motors are used in boston spot robot
I want to know which motors or smart servos are used in boston dynamics spot robot. Where can i read its spicification and is it possible to buy the motor? </p>

答:
I had the chance to play with Spot this afternoon and it is not hydraulic.

I bet for a stepper motor with cycloidal gears and position encoders. But I not really sure which kind of motor. They didn't want me to tear it down...