问:
Computing the Jacobian matrix for Inverse Kinematics
When computing the Jacobian matrix for solving an Inverse Kinematic analytically, I read from many places that I could use this formula to create each of the columns of a joint in the Jacobian matrix:

$$\mathbf{J}_{i}=\frac{\partial \mathbf{e}}{\partial \phi_{i}}=\left[\begin{array}{c}{\left[\mathbf{a}_{i}^{\prime} \times\left(\mathbf{e}_{p o s}-\mathbf{r}_{i}^{\prime}\right)\right]^{T}} \\ {\left[\mathbf{a}_{i}^{\prime}\right]^{T}}\end{array}\right]$$

Such that $a'$ is the rotation axis in world space, $r'$ is the pivot point in world space, and $e_{pos}$ is the position of the end effector in world space.

However, I don't understand how this can work when the joints have more than one DOFs. Take the following as an example:

The $\theta$ are the rotational DOF, the $e$ is the end effector, the $g$ is the goal of the end effector, the $P_1$, $P_2$ and $P_3$ are the joints.

First, if I were to compute the Jacobian matrix based on the formula above for the diagram, I will get something like this:

$$J=\begin{bmatrix}
((0,0,1)\times \vec { e } )_{ x } & ((0,0,1)\times (\vec { e } -\vec { P_{ 1 } } ))_{ x } & ((0,0,1)\times (\vec { e } -\vec { P_{ 2 } } ))_{ x } \\ ((0,0,1)\times \vec { e } )_{ y } & ((0,0,1)\times (\vec { e } -\vec { P_{ 1 } } ))_{ y } & ((0,0,1)\times (\vec { e } -\vec { P_{ 2 } } ))_{ y } \\ ((0,0,1)\times \vec { e } )_{ z } & ((0,0,1)\times (\vec { e } -\vec { P_{ 1 } } ))_{ z } & ((0,0,1)\times (\vec { e } -\vec { P_{ 2 } } ))_{ z } \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 1 & 1 & 1
\end{bmatrix} $$

This is assumed that all the rotation axes are $(0,0,1)$ and all of them only have one rotational DOF. So, I believe each column is for one DOF, in this case, the $\theta_\#$.

Now, here's the problem: What if all the joints have full 6 DOFs? Say now, for every joint, I have rotational DOFs in all axes, $\theta_x$, $\theta_y$ and $\theta_z$, and also translational DOFs in all axes, $t_x$, $t_y$ and $t_z$.

To make my question clearer, suppose if I were to "forcefully" apply the formula above to all the DOFs of all the joints, then I probably will get a Jacobian matrix like this:

(click for full size)

But this is incredibly weird because all the 6 columns of the DOF for every joint is repeating the same thing.

How can I use the same formula to build the Jacobian matrix with all the DOFs? How would the Jacobian matrix look like in this case?

答:
One way is to find the Jacobian using Perturbation method.

Basically, you compute forward kinematic matrix and find each entry through numeric differentiation

for each f(θ1+Δθ, θ2, θ3...)
subtract f(θ1, θ2, θ3)
divide by Δθ

θ1, θ2+Δθ, θ3...
subtract f(θ1, θ2, θ3)
divide by Δθ

and so on


问:
Motion planning algorithms with applications beyond robotics in animation
I am reading the introduction (chapter 1) of Principles of Robot Motion Theory, Algorithms, and Implementations by Choset et al. and they mention that some of the work in robotics has been applied to digital actors, animation etc and it isn't elaborated as to what these algorithms are and what problems they are solving in the other fields. What are they?

Algorithms developed for motion planning or sensor interpretation are not just for
robots anymore. In the entertainment industry, motion planning has found a wide
variety of applications in the generation of motion for digital actors, opening the way
to exciting scenarios in video games, animation, and virtual environments

答: One example I see is animating human character on video game. On Unity game engine demo, they show there is human character that stand on big rock. The rock roll left and right while character maintain to stay on the top of rock. To achieve this, character has to control the center of mass itself by moving joint on his leg. That is by performing inverse kinematic.</p>

ContentLicense: CC BY-SA 4.0


问:
Can a Madgwick IMU filter be used to determinate the tilt angle with respect to the horizontal plane
I am using a 3-axis accelerometer and a 3-axis gyroscope to estimate the tilte angle between the X axis of the IMU sensor and the ground (horizontal plane). My robot is stationary at power up, so I can determine the initial tilt angle just by using the data acquired from the accelerometer. However, when he starts moving I can not do that anymore. That is why I need a sensor fusion algorithm.

Can I use the Madgwick filter which does not include a magnetometer? Note that my robot can rotate around any of the three Cartesian coordinate system axis while he operates in free space. I still need time to understand the filter myself, so I do not know the answer yet.

Thank you for your time.

答:
Unless your robot is experiencing sustained accelerations on the order of 1g, ignoring linear accelerations to compute the tilt angle should work well. Besides, typically the weight on the accelerometer measurement is much lower than that on the gyroscope measurement in a standard complementary filter so any linear accelerations should only add a small bias. How is your robot moving at startup? What sort of accelerations are you seeing?

If on startup, the robot is at an arbitrary orientation AND experiencing excessive linear accelerations (not velocities), then it may be tricky as the initial orientation computed may be quite wrong. In that case, if there is any way to know the linear motion in advance, e.g. if the motion is a result of control inputs, then that known acceleration can be subtracted before the accelerometer readings are used in the filter.

A particularly tricky case may be when the robot starts in free fall. If it doesn't hit terminal velocity to experience a drag force, and without any other forces acting on it, I don't see a way to measure the tilt relative to the ground with an accelerometer and gyroscope. The accelerometer will measure all zeros and give no tilt information. However, the drag force in most situations may be enough to give some bearing, but you are not guaranteed that the force is parallel to the force of gravity.

If one wishes to estimate the tilt of a robot in a parabolic free fall with air resistance. Then I can imagine some algorithm that attempts to estimate the tilt using the direction of the drag force and some estimated progress along a parabolic arc. It sounds complicated though.

I'm not familiar with the Madgwick filter specifically, but to me it sounds like some variant of a complementary filter.


问:
Determine road curvature radius using non GPS sensors
I’m trying to solve for the “curvature” of a road, and wondering if this can be done with a standard mobile phone.

If I have a mobile phone fixed onto my vehicles windscreen, using only standard mobile phone sensors (accelerometer/gyro) and NO GPS, is it possible to determine the “curvature” of the road that the car is travelling on?! If so, how?

My current thought process is that I could use the combined readings from a gyro (ω) and accelerometer (a) and solve for R (Radius of curve) as below:

R = a/ω²

As R tends to infinity the road has no curvature (straight)

Thanks

答:
The acceleration from the IMU is quite noisy. You may be able to detect the acceleration when turning, but the result won't be very accurate and would likely only work well on very tight turns. Another issue would be that linear accelerations of the car would be detected as turns.

If the phone is fixed to the windscreen, then it likely has a good view of the road and VO / SLAM algorithms should work well. As a bonus, you would also get the entire trajectory, positions and all. For this specific purpose, you can more easily just look at the curvature of the lane lines seen in the camera.


问:
Are Artificial Intelligence and Robotics Different?
I need help in differentiating between AI and Robotics. Are AI and Robotics two different fields or is robotics a subject in AI?

I want to pursue a career in AI and Robotics. So I need your valuable suggestion. I searched the web and also some universities that I want to apply and I cannot find any such thing that I am searching for.

答:
AI means different things to different people and encapsulates a large number of sub-domains: vision, natural language processing, knowledge representation and engineering, machine learning, etc.  Many, if not all, areas of AI have applications to robotics; however, doing AI research does not mean that you directly, or in any way, work with robots.  Working with robots does not necessarily mean that you do artificial intelligence research, either.

Off the top of my head, here are a few good programs (my opinion, which was based off of my research and interests): Georgia Tech, MIT, CMU, Stanford, Brown, Johns Hopkins, etc.

NB:
 It may be different for a master's degree, but if you're looking to do research, you shouldn't be judging schools based off the general reputation of a particular sector of the CS department.  Instead, you should be looking for specific professors with whom you would like to conduct research.  Being at a well known school is wonderful, but if the professors aren't doing what you're interested in, then you're doing yourself a disservice.

ContentLicense: CC BY-SA 3.0

OwnerDisplayName: Steve P.


问:
perplexity about ch8.2.1 the linear dynamics, in the book <modern robotics>
From previous chapters, I think I have understand the twist,  it is about the angular velocity and the linear velocity, the difference is that linear velocity is not one fixed point in the body but is the one in the body in coincidence with the origin instantly.

I also notice the difference Stationary/inertial reference frame

first quetion

in chapter 8.2 ,is  $v_b$ the velocity of the mass center of the body? after all the inertia frame is in coincidence with the mass center instantly.

Let $r_i =(x_i, y_i, z_i)$ be the fixed location of mass $i$ in a body frame {b}, where the origin of this frame is the unique point such that $\sum_{i} m_i r_i =0$. This point is known as the center of mass.

second question

what is the relationship between eq8.22 $f_b = m(\dot v_b + [\omega_b]v_b)$  and $f_b = m(\dot v_c)$ from classical method ,$v_c$ is the linear velocity of the mass center

third question

I have read the Roy Featherstone book, the spatial acceleration is defined as $[\dot w, \ddot r - w \times \dot r]^T$ , this garantees that spatial acceleration is zero even when the body is rotating at a constant speed therefore some points are undergoing centripetal force.
However  in the modern robotics I find that the derivative of the twist is not accordance with the the spatial acceleration

答:
To answer your first question: $v_b$ is the linear velocity of the centre of mass of the body relative to a fixed inertial frame coincident with the body frame with its origin at the centre of mass.

For your second question: Equation 8.22 takes into account the contributions of all linear and rotary motions on the overall force on the body, while the equation you've included from the classical method ($f_b=m(\dot v_c)$) assumes you are both measuring in a fixed inertial frame and that the body is not rotating.

Finally, the third question: there are a few key differences in the notation used in the Modern Robotics textbook and in Featherstone's work, one of which produces equations for spatial acceleration that appear to disagree with each other.

However, having used both methods both in my research and in teaching courses on robotics for many years, I can assure you that as long as you stick with one standard of notation you will end up with the same numbers in the end. In other words, $\dot v_b + [\omega_b]v_b = \ddot r - \omega \times \dot r$.


问:
what is difference between twist and classcial velocity
In ch 8.2.1 Classical Formulation in the book Modern Robotics by Frank C. Park

I know some remarks about this formula.The origin of coordinate system is coincidence with the mass center of the rigid body. $w_b$ is the angular velocity of the body. It is noticeable that $v_b$ is not the velocity of one particular fixed point in the body, but is the velocity of one selected point in the body and the point is in coincidence with the origin of the system instantaneously. Based on these, we can use the formula compute arbitray point's velocity and acceleration. The point's coordiante in the system is $p_i$. The $p_i$  rightly reprensent one concrete point in the body.

Now question is that , if I take $v_b$ as the fixed point(the mass center) in the body , how can I get  velocity and acceleration of arbitray point in the body? What is the formula ? By this I think I can understand the different between twist and the classical velocity

答:
In these equations from Modern Robotics (by Park and Lynch), the fixed inertial frame ${b}$ is both the reference frame used to define all of the coordinate vectors and has its origin located at the centre of mass (COM) of the body.

The body's angular velocity relative to $\{b\}$ is $\omega_b$, as you say, and is a property of the body which is dependent only on the orientation of the frame and not on the location of its origin. The variable $v_b$ is the linear velocity of the centre of mass of the body as it moves past the (instantaneously fixed, inertial) frame $\{b\}$, and is dependent on both the origin and orientation of the frame.

You have also already answered your own question, as the way to find out the linear velocity and acceleration of any single point $p_i$ (which is rigidly attached to the body) relative to the frame $\{b\}$ is to compute the equations you have shared above:

The linear velocity of the point (i.e., $\dot p_i$) is simply $\dot p_i = v_b + \omega_b \times p_i$, which combines the linear velocity of the COM ($v_b$) and the linear velocity of the point caused by the rotational velocity of the body ($\omega_b$) acting on the perpendicular distances the point is away from the COM in each direction ($\times p_i$).

The linear acceleration of the point (i.e., $\ddot p_i$) is the derivative of the linear velocity, which for brevity's sake I won't retype but will refer you to the image in your initial question. This equation combines the linear acceleration of the COM ($\dot v_b$), the linear acceleration of the point caused by the rotational acceleration of the body ($\dot \omega_b$) acting on the perpendicular distances the point is away from the COM in each direction ($\times p_i$), and the velocity product $\omega_b \times \dot p_i = \omega_b \times (v_b + \omega_b \times p_i)$ which includes both the centripetal and Coriolis accelerations of the body.


问:
How does the Makeblock threaded slot work?
I've been looking into a Makeblock robotics kit but have found no information on the web that comes from end-users, and one of the main advertised features is not clear to me:  The slot threads shown below are straight, while the screw thread that will mate with them is angled.  Is there just very little contact between screw thread and rail thread vs. regular screw hole threads?  Or would the screw want to rest angled somewhat- and then the head would not be flush with the rim of the rail?  Or would the screw deform the aluminum rail if over-torqued?

This is a close up picture of the slot with screws:

答: It would be interesting if they would have these hard anodized, which would give the surface of the aluminum the same hardness as case-hardened steel. I agree though that the contact area is very small. I do not see the advantage of this over an undercut slot that captures a t-nut underneath, which would still give you the flexibility to tighten anywhere along the length, with full thread engagement, and much higher clamping force.</p>


答:
If you want to understand the difference between robotics vs AI, you can roughly think about it as creatures vs brain (more precisely, nerve system).

First off, not all creatures have a brain. They may have nerves that allow them to act reflexively. They are equivalent to robots with no AI (note: definition of AI is not that precise).

There are some creatures with a brain, but a brain that is quite dumb (in the sense that it can do complex tasks, but it can't learn new ways for it). Those are robots with more complex algorithms, but still no learning, which some consider as no AI and some consider as a specific kind of AI.

The interesting ones are the ones that have brains they use to learn (e.g. humans, cats, dolphins, elephants etc). They are like robots with AI.

In summary, robotics is a whole set of sciences; mathematics, physics, mechanics, electronics, materials, control, geometry, artificial intelligence and many others. However, each of those sciences by itself goes beyond robotics.

If you go after learning AI, you may end up applying it in robotics or not, depending on your own decisions later. If you go after learning robotics, you may end up working on its AI or not, depending on your own decisions later.

My recommendation would therefore be to think of possibilities. For example, if you are interested in AI in robotics now, what would happen if you lose interest in it in the future? If you are generally interested in algorithmic software, choose AI. If you change your mind about robotics, you still have something to focus on. If you are particularly interested in building real things, go with robotics. If you change your mind about AI later, you can still work on robots.

Title: Are Artificial Intelligence and Robotics Different?

Body:
I need help in differentiating between AI and Robotics. Are AI and Robotics two different fields or is robotics a subject in AI?

I want to pursue a career in AI and Robotics. So I need your valuable suggestion. I searched the web and also some universities that I want to apply and I cannot find any such thing that I am searching for.


问:
3D Rigid Body Pose Optimization in flat euclidean space
I am confused about what is the best way to optimize the 3D pose of an object and the best way to parametrize the translation and rotation for optimization in flat euclidean space.

Assume I have a point $\pmb{x^o}$ given in the coordinate system of the Object which pose I want to optimize. I measure the distance between the object and the point through some function $f:\mathbb{R}^3 \to \mathbb{R}$. Goal of the optimization is to minimize this function by optimizing the object pose (translation + orientation).
I can obtain the local spatial gradient of each point in the object coordinate frame $\nabla f \bigg|_{\pmb{x}^o}$.

The pose of the Object is given by the translation vector $\pmb{t}_o^e$ and the rotation matrix $R_o^e$, such that points can be transformed from the object coordinate system into the ego coordinate system through $\pmb{x}^e = R_o^e \pmb{x}^o + \pmb{t}_o^e$ and the the other way around through $\pmb{x}^o = {R_o^e}^\top( \pmb{x}^e - \pmb{t}_o^e)$.

Now for the calculation of the analytic Jacobian w.r.t to the translation $\pmb{t}_o^e$ it is clear to me how it can be obtained by applying the chain rule:
$$
\frac{\partial f(\pmb{x}^o)}{\partial \pmb{t}_o^e} = \nabla f \bigg|_{\pmb{x}^o} \cdot \frac{\partial \pmb{x}^o}{\partial \pmb{t}_o^e} = \nabla f \bigg|_{\pmb{x}^o} \cdot \frac{\partial ({R_o^e}^\top( \pmb{x}^e - \pmb{t}_o^e))}{\partial \pmb{t}_o^e} = \nabla f \bigg|_{\pmb{x}^o} \cdot (- {R_o^e}^\top)
$$
So the result simple means the gradient has to be multiplied with the negative inverse/transposed rotation matrix.

However I am unsure how to do the same for the object orientation and which parametrization to use.
I read that euler angle suffer from their bounds on pitch and roll angles as well as gimbal lock.
I tried using quaternions which are not a minimal parametrization (7 parameters for 6 Degrees-of-Freedom).
This will lead to deriving the rotation matrix after each quaternion parameter, which I have done, but results were not really promising.

I know that the most general and robust solution is using Lie theory and se(3) lie algebra (also known as twist coordinates). However I don't think I can use this here as I don't want to optimize on the manifold but use a Gauss-Newton or BGFS optimizer in flat euclidean space for this problem (the ones offered by scipy.optimize).

Edit:
I noticed this is very broad and its unclear what kind of answer I am looking for. So here my post broken down into some specific questions

What is the recommended parametrization of such a 3D rigid body optimization problem? (e.g. translation+Euler angles, translation+Quaternion, twist coordinates) Follow up is: How is the coordinate system transformation derived after these parameters?

Can twist coordinates be used when I am trying to solve this with the optimization methods provided by pythons scipy package? Do I need to have parameters in the flat euclidean space to apply 'standard' (meaning not 'on manifold') Gauss-Netwon (Non-linear least squares) optimization method with analytic jacobian?

Are there any example where a 3D rigid body optimization is solved using scipy?

I welcome any kind of answer, comment or input.

答:
See https://www.cis.upenn.edu/~cjtaylor/PUBLICATIONS/pdfs/TaylorTR94b.pdf.

You can absolutely use "flat" Euclidean space based optimizers while also optimizing on the manifold, but I agree the default scipy solvers don't give you an easy way to do that. Perhaps you can use pymanopt? See https://www.pymanopt.org/.

Although I wouldn't be scared of implementing a "manifold" based optimizer yourself. In my experience, simple gradient descent works really well when optimizing for an orientation. This involves computing the gradient w.r.t the orientation parameters (shown in the PDF linked above) and then recomputing the orientation after every step. If you run into local minima, random restarts is a (conceptually) simple way to get to a global minimum.

If you really want to use the scipy methods, and if you know your initial orientation is reasonably close to the solution (or use random restarts), I would use a local parameterization starting at your guess, so that any singularities would at least be far away no matter where you start from. This would be similar to the methods described in the PDF, but without the projection back into the manifold.


问:
Doubt regarding the likelihood field in measurement model

I don't quite understand what xztk
and yztk
mean? Are they meant to represent the position of the kth laserscan reading? If yes, could someone please help me figure out how are the values of xztk and yztk being evaluated? Why isn't the angle of the kth laserscan reading (w.r.t. the base_laser) being taken into account?

答:
Yes, my reading is that you're correct that they are indicating the position of the kth range result.

They are taking into account with the the angle of each scan through $\theta_{k,sens}$ which is the angle of the kth scan relative to the sensor's pose.


问:
cpp:585:1: warning: no return statement in function returning non-void [-Wreturn-type]
I am using this Repo

when building the projects I am getting this error

Ros - noetic

```<code>[ 83%] Building CXX object RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/src/trajectory_sampler.cpp.o
/home/guru/my_bots/Udacity_robot/src/RoboND-Kinematics-Project/kuka_arm/src/trajectory_sampler.cpp: In member function ‘bool TrajectorySampler::SetupCollisionObject(const string&, const string&, const Pose&, moveit_msgs::CollisionObject&)’:

make[1]: *** [CMakeFiles/Makefile2:2670: RoboND-Kinematics-Project/kuka_arm/CMakeFiles/trajectory_sampler.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
make[2]: *** [RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeFiles/gazebo_grasp_fix.dir/build.make:76: RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeFiles/gazebo_grasp_fix.dir/src/GazeboGraspGripper.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
make[2]: *** [RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeFiles/gazebo_grasp_fix.dir/build.make:63: RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeFiles/gazebo_grasp_fix.dir/src/GazeboGraspFix.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:706: RoboND-Kinematics-Project/gazebo_grasp_plugin/CMakeFiles/gazebo_grasp_fix.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
Invoking "make -j8 -l8" failed
</code>```

答:
The project appears to have an upstream ticket about the issue.

The warning you cite is a warning not the error that's stopping compile.

At least one of the errors in this is already ticketed on the repository.

https://github.com/udacity/RoboND-Kinematics-Project/issues/24

I would suggest that you follow up over there.

To get better help here. Please ask a question with a self contained minimal example which can be reproduced by whoever is trying to help you.


问:
How do I identify the different wires on RPLidar?
I bought an RPLidar A1M8-R6 from the following site: https://www.dfrobot.com/product-1125.html?tracking=585c2ddea9c83

I am trying to connect RPLidar to Arduino so I could get information on the degrees and distance detected by the Lidar. I was following the instructions from the video on the following site: https://dronebotworkshop.com/getting-started-with-lidar/

My problem is that I am unable to figure out which wires to connect to Arduino. I was trying to follow the video but the colors of the wires in the video and their orientation is not the same in the product I have. Can somebody please help me identify the wires? I'm trying to figure out which one is Ground, 5v, Tx, Rx, Motoctl, Vmoto and vcc. Thank you very much! I'm attaching the picture of the wires here.

答:
Like jsotola mentioned, the way to find information on a thing is to look at its datasheet. From the datasheet (warning: direct download, found on the bottom-right of this page):

So, comparing your picture to the datasheet, it looks like:

Signal     | Color
--------------------
TX           Yellow
RX           Green
VCC_5V       Orange
GND          White
GND_MOTO     Black
CTRL_MOTO    Blue
5V_MOTO      Red
```

You're really going to want to read the datasheet, because that's where you're going to get all the information about baud rate, packet format, parameter settings, voltages, power consumption, etc.

Body:
Like jsotola mentioned, the way to find information on a thing is to look at its datasheet. From the datasheet (warning: direct download, found on the bottom-right of this page):

So, comparing your picture to the datasheet, it looks like:

Signal     | Color
--------------------
TX           Yellow
RX           Green
VCC_5V       Orange
GND          White
GND_MOTO     Black
CTRL_MOTO    Blue
5V_MOTO      Red

You're really going to want to read the datasheet, because that's where you're going to get all the information about baud rate, packet format, parameter settings, voltages, power consumption, etc.


答:
They are different.  They are often used together, but the two are not specifically related.

Artificial Intelligence is a branch of computer science that focuses on solving problems that are traditionally difficult for computers -- tasks that become exponentially or factorially more complex for each incremental increase in the input.  An example of this would be the XKCD movie seating problem, which has 120 options for 5 people but 2,432,902,008,176,640,000 options for 20 people.  Another example would be processing the many thousands of points you'd see in a LiDAR image (like this one) in order to build a plan for navigating through an area.  The field of Artificial Intelligence attempts to find decent shortcuts to getting acceptable solutions to these types of problems; it would take far too long to evaluate each possible solution separately then pick the best one.

Robotic systems are simply mechanical systems that can measure and react to their environment in the pursuit of some goal.  This could be as simple as balancing an inverted pendulum, which requires no AI.  Alternatively, it could be as complicated as recognizing folding towels, which requires AI to sort out things like "which objects are towels", "how do I pick them up", "how do I move in order to fold them", etc.

In short, AI is one of many tools that a robotics engineer might integrate into a robotic system.


问:
To calculate roll,pitch and yaw
How do I calculate roll,pitch and yaw value from the position co ordinates of the drone, that is x,y and z from the gps sensor???
Using the x,y and z co ordinates I have to calculate roll,pitch and yaw. What is the equation for this??</p>

答:
The position coordinates x, y, z are inadequate information to compute the roll pitch and yaw.

x, y, and z are the position of the vehicle in space. roll, pitch, and yaw are the attitude or orientation. They can change independently. Aka you can change the orientation of the vehicle independently from the position of the vehicle.

Title: To calculate roll,pitch and yaw


答: It is not possible to calculate your roll - pitch - yaw angles from linear position information. If you are using a simulated robot with sensors, you can use the gyroscope readings from the IMU sensor, that way you are going to have the angular velocities in three axes. You can then integrate this readings to get angular position. However, because of the integration, the error is going to increase rapidly; therefore you need to combine these readings with some additional sensors (magnetometer, accelerometer) to improve accuracy - you can check Complementary and Kalman Filter for that.</p>

Title: To calculate roll,pitch and yaw

Body:
How do I calculate roll,pitch and yaw value from the position co ordinates of the drone, that is x,y and z from the gps sensor???
Using the x,y and z co ordinates I have to calculate roll,pitch and yaw. What is the equation for this??</p>


问:
Locating nearest device position
I have the following use case:

Given a set of points in a space, I would like to use a smartphone to detect and locate the closest point (Possibly accurate to the at least a meter range)

I am not familiar with this field myself, so I would like to know if you have some input on how to get started with the research.

For example, in your opinion:

What would be the best technology to use? Bluetooth, GPS, NFC

What's the cheapest option to use for the devices? And what would be the overall best option regardless of price?

Thanks in advance for any input, please let me know if I need to clarify something or add more information.

答:
if you read this :
https://en.wikipedia.org/wiki/Near-field_communication

you can see check this :

```<code>Technology       NFC                 Bluetooth       Bluetooth Low Energy
Range           < 20 cm           ≈100 m (class 1)          ≈50 m
</code>```

first you will find out that the NFC ins,t your choice because it,s short Range and, the range of Bluetooth ins,t enough
the only choice from the given options is GPS
but you have two main issue using GPS to locate other devices.

the environment terrain highly affects on position locating when you are using GPS.
environment terrain like when you are in mountainous area or when you are in the location of a city that surrounded by skyscrapers or tall buildings and .....

in GPS modules you have no access to the other GPS modules and instruments who aren't belong to you
but, if the other devices that you are going to access their location are yours, it's easy, you can send the devices locations together and find their relative location & directions

Title: Locating nearest device position

Body:
if you read this :
https://en.wikipedia.org/wiki/Near-field_communication

you can see check this :

Technology       NFC                 Bluetooth       Bluetooth Low Energy
Range           < 20 cm           ≈100 m (class 1)          ≈50 m

first you will find out that the NFC ins,t your choice because it,s short Range and, the range of Bluetooth ins,t enough
the only choice from the given options is GPS
but you have two main issue using GPS to locate other devices.

the environment terrain highly affects on position locating when you are using GPS.
environment terrain like when you are in mountainous area or when you are in the location of a city that surrounded by skyscrapers or tall buildings and .....

in GPS modules you have no access to the other GPS modules and instruments who aren't belong to you
but, if the other devices that you are going to access their location are yours, it's easy, you can send the devices locations together and find their relative location & directions


问:
How to do transformation to get correct linear velocity from linear acceleration IMU data?
I have IMU sensor that gives me the raw data such as orientation, Angular and Linear acceleration. Im using ROS and doing some Gazebo UUV simulation. Furthermore, I want to get linear velocity from the raw IMU data. If I do integration over time there will be accumulated error and will not be accurate with the time when for example the robot makes turns. So If I use

acceleration_x = (msg->linear_acceleration.x + 9.81 * sin(pitch)) * cos(pitch);

acceleration_y = (msg->linear_acceleration.y - 9.81 * sin(roll)) * cos(roll);

So integrating linear acceleration is very bad,

Velocity_x= Velocity_old_x+acceleration_x*dt;

because integrates the acceleration without taking into account any possible rotation of the sensor, which means that the results will probably be terrible if the sensor rotates at all. So I need some ROS package that takes into account all this transformation and gives me the most accurate estimation of the linear velocity. Any Help? Thanks

答:
What you have stumbled upon here is a classic problem in robotics; where am I? Localization is a hard to solve problem. Every sensor will have associated noise and accurately figuring out velocity and position is hard. The most widely used package in ros is robot_localization that will take in arbitrary number of sensor inputs and produce output poses (position and velocity). I'd recommend using the ekf state estimation node.

Unfortunately, if you are stuck with using one IMU, you will have to deal with noise. I'd recommend using additional sources of odometry for better estimates.


问:
How to choose WiFi signal strength detecting sensors
We want to create robot that will localize itself by the signals of wifi routers.
Which sensors should we buy to detect strength of 3 WiFi signal?
Which of following is necessary for us?
http://www.dfrobot.com/index.php?route=product/category&path=45_80
or can be any other more suitable variants?
We are using arduino as a platform.</p>

答:
Any WiFi system should be able to report the signal strength (in dBm), but the question will be how quickly you need it to report values and how accurate a measurement you are looking for.

The main problem with what you're suggesting is that the relationship between signal strength and distance is not necessarily linear.  Antennas produce an interference pattern which can make the strength change depending on your orientation to them.

The second problem is that the signal strength can be affected by the environment: humidity, nearby metal, buildings, and even your body can affect the strength of the signal (if not directly, then through interference).

The only way to really do it is by heavily characterizing the wifi signals in the area you want to operate in and hoping that it doesn't change too much over time.


问:
How do you model a physical robot (diff drive) in python?
I am using PyGame to simulate the motion of a differential drive robot. So far, I have used the standard diff drive kinematics, multiplying by delta time, and I have the motion working as expected.

What I want to do next is build a PID controller to control the robot in trajectory planning, but I feel that I should build a more realistic model (or rather, I would like to). I understand PID control well, but I am a little unsure about how to build the actual robot model and what I should control as I am new to mobile robotics.

From my understanding, on a real robot (diff drive) you send a control signal to each wheel setting it to a certain velocity. The velocity is read by the wheel encoders, position is estimated using that value in the kinematic equation, and the feedback loop (or whatever control style being used) checks the error on the position to the desired position and adjusts the control signal.

As I stated, in my simulation so far I just have the kinematic models to get the robot position, and a set_velocity function to set the velocity of each wheel. I am confused as to how I can incorporate the other aspects of the real-world examples into my simulation, ie setting the velocities and checking them with the encoders (I think I have an idea of how to simulate an encoder in general since I know how to convert ticks to velocity).

Right now, my velocities are simply a class variable of the robot, so I am having trouble understanding how I might do this...

```<code>import pygame
import math
class Robot:
    def __init__(self, image_of_robot, starting_location, width, initial_velocity, max_velocity):
        # Define the width between wheels
        self.width = width

        # Define the intial conditions of the robot and define states
        self._x_position = starting_location[0]
        self._y_position = starting_location[1]
        self._direction_angle = starting_location[2]
        self._left_wheel_velocity = initial_velocity
        self._right_wheel_velocity = initial_velocity

        # Define the limits on the velocity of the robot
        self.max_velocity = max_velocity

        # Create the robot image, the image to be edited, and a bounding rectangle for position
        self.original_image = pygame.image.load(image_of_robot)
        self.rotated_image = self.original_image
        self.bounding_rectangle = self.rotated_image.get_rect(center=(self.x_position, self.y_position))

    # Get the current robot image and bounding rectangle for blit
    def get_image(self):
        return (self.rotated_image, self.bounding_rectangle)

    # Set the velocity of the wheels.
    # Right_wheel = 1 -> right wheel velocity to be set
    # Right_wheel = 0 -> left wheel velocity to be set
    def set_velocity(self, right_wheel=1, value=0):
        if right_wheel:
            if abs(self.right_wheel_velocity + value) <= self.max_velocity:
                self.right_wheel_velocity += value
        else:
            if abs(self.left_wheel_velocity + value) <= self.max_velocity:
                self.left_wheel_velocity += value

    # Update the states (position, direction) of the robot using the equations of motion
    def update_states(self, time):
        self.x_position += ((self.left_wheel_velocity + self.right_wheel_velocity)/2
                            *math.cos(self.direction_angle)*time)

        self.y_position -= ((self.left_wheel_velocity + self.right_wheel_velocity)/2
                            *math.sin(self.direction_angle)*time)

        self.direction_angle += ((self.right_wheel_velocity - self. left_wheel_velocity)
                                / self.width * time)

    # Update the images based on current states
    def update_image(self):
        self.rotated_image = pygame.transform.rotozoom(self.original_image, math.degrees(self.direction_angle),1)
        self.bounding_rectangle = self.rotated_image.get_rect(center=(self.x_position, self.y_position))

    # Update the robot by updating the states and images to reflect current status
    def update(self, time):
        self.update_states(time)
        self.update_image()
</code>```

答:
If you want to have a model that will behave closely to a real-life one, I would definitely believe that you should also have a look at the dynamic modeling of a differential drive robot, as Kinematic equations do not include the forces and torques acting on your robot. In fact during the simulation part of a real-world control problem, you should use the dynamic equations of your plant to have reliable results. As far as I believe, there are some online sources, such as articles, on this one.

If you want to keep it simpler however, you can use your kinematic model as well. It might cause heavy amount of work to have the dynamic equations since it requires you to evaluate some coefficients of your vehicle. Kinematic models yield more realistic results for environments with no disturbance (such as friction).

Also what you think about your input signal is correct, you send velocity commands to each wheel. However, if you want to add more reality you may create a function to map the wheel velocity to a PWM signal pulse-width. Because in real-life you would probably use PWM signals to control your motor (therefore, wheel) speeds.

You can also add some noise to your encoder data, since the sensors do not measure with perfect accuracy, which is a very important phenomena for Controller Design process. A controller which yields good results with perfect sensor readings (on simulation) may perform badly in real-life because of sensor noises.

For what you want to control, it is a bit up to you. You may control the position and orientation of the robot, or you may do position and velocity control. It depends on your application and what you want to achieve.

These are what straightly jumped to my mind when I saw your question. I might add some other suggestions, if anything comes to my mind.


问:
calculation sequences when using RPY-transformation vs Euler-transformation
I have trouble understanding calculation sequences for RPY-transformation and Euler-transformation.

Let's assume that there are 3 (rotation/translation) instructions.

I will notate transformation matrix for instruction 1 as [1],

transformation matrix for instruction 2 as [2],

transformation matrix for instruction 3 as [3]

In this situation when using RPY-method, calculation order will be [3][2][1] -> [3][2 * 1] -> [3 * 2 * 1].

First multiply [2] and [1], and then multiply [3]. Right?

When using Euler-method, I am not sure what is correct order.

[1][2][3] -> [1 * 2][3] -> [1 * 2 * 3]

First multiply [1] and [2], and then multiply [3]

[3][2][1] -> [3 * 2][1] -> [3 * 2 * 1]

First multiply [3] and [2], and then multiply [1]

What is correct sequence when calculation Euler-method? Is it 1) or 2) ?

Also I think the resulting 4x4 matrix should be same whether I use RPY or Euler transformation. Is it correct?

答:
I would like to add to your answer a simple concept i used to understand rotation matrices.
So, first you rotate the x- axis. No problem here.R=[x]

Second, you rotate the already rotated axes(that was rotated around x) around z.
Hence you pre-multiply first matrix by second matrix as it is now changing the previous rotation.
Post multiplying here would mean that the frame was rotated around x now affected the rotation around z which is not true. Hence pre-multiplication. Rot. matrix becomes R'=[z][x].

Now, third you rotate already rotated axes (that was rotated first around x and then around z) around y. As your rotation around y affects the previously rotates axes, so you pre-multiply them with y- rotation matrix. Post multiplying would mean that rotation around x and z affects the rotation around y which is not true as frames were rotated around y in the end (not first). New matrix R"=[y][z][x].
So any new rotation around reference frames affects all rotations done before so should be placed first. The first rotated matrix doesn't affect anyone but themselves should be placed last.

I hope that makes sense.


问:
Robotic vessel navigation using GIS datasets
I am trying to build an autonomous vessel able to plan the best route to a certain waypoint or to follow a pre-defined route.

Currently, I have imported Esri GIS files containing elevation data (coordinates can be extracted from grid size and lower left corner position), and the robot automatically loads in a dynamic vector the correct map using a geofencing algorithm.

The idea is to use elevation data to keep the boat away from shallow water and land, possibly using a D* algorithm to determine the best route.
However, those GIS datasets are discrete and sometimes have a pretty low accuracy, while the GPS on board uses RTK to get a 2cm accuracy.
The problem comes when I try to convert a discrete distribution in a continuous map, especially for elevation data: there are certain areas where altitude goes from -10 (underwater) to 100 from one point to another. Any ideas?

答:
If I understand your question correctly, you are trying to estimate the position of arbitrary points in an environment given scattered 3D points. For example, your robot is at position (x, y, z) where your x and y are known but you need to determine the z given the scattered GIS data.

This is an interpolation problem, and the matlab doc have a pretty comprehensive page on this exact problem for the griddata method. Depending on the interpolation method you use the equation to find the position along z are going to be different.

If you choose to go with a linear interpolation (the robot is on the plane defined by the three closest GIS points) you can use something like this answer that I will copy here for safekeeping:

p1, p2, p3 define a plane. You can represent it by a point and a
normal. For instance, P=p1, N=(p2-P) x (p3-P) (that is, N = cross
product of p1p2 and p1p3).

Now for p4 to be in the same plane, it satisfies the plane equation:

```<code>  (p4-P) · N = 0  %// dot product
⇒ (x4-x1)*N.x + (y4-y1)*N.y + (z4-z1)*N.z = 0
</code>```

Re-arranging:

```<code>z4 = z1 - ((x4-x1)*N.x + (y4-y1)*N.y)/ N.z
</code>```

No linear system to solve, you just need a cross product.

Since you have large distances between your GIS measurements, I would use a Cubic interpolation but again, it is mainly important that your interpolation is closest to the physics of your estimated model.

Body:
If I understand your question correctly, you are trying to estimate the position of arbitrary points in an environment given scattered 3D points. For example, your robot is at position (x, y, z) where your x and y are known but you need to determine the z given the scattered GIS data.

This is an interpolation problem, and the matlab doc have a pretty comprehensive page on this exact problem for the griddata method. Depending on the interpolation method you use the equation to find the position along z are going to be different.

If you choose to go with a linear interpolation (the robot is on the plane defined by the three closest GIS points) you can use something like this answer that I will copy here for safekeeping:

p1, p2, p3 define a plane. You can represent it by a point and a
normal. For instance, P=p1, N=(p2-P) x (p3-P) (that is, N = cross
product of p1p2 and p1p3).

Now for p4 to be in the same plane, it satisfies the plane equation:

  (p4-P) · N = 0  %// dot product
⇒ (x4-x1)*N.x + (y4-y1)*N.y + (z4-z1)*N.z = 0

Re-arranging:

z4 = z1 - ((x4-x1)*N.x + (y4-y1)*N.y)/ N.z

No linear system to solve, you just need a cross product.

Since you have large distances between your GIS measurements, I would use a Cubic interpolation but again, it is mainly important that your interpolation is closest to the physics of your estimated model.


问:
seeking details on ros::SingleSubscriberPublisher class
I recently came across 'ros::SingleSubscriberPubliser' class. The documentation describes it as 'Allows publication of a message to a single subscriber. Only available inside subscriber connection callbacks'.

What does 'subscriber connection callbacks' mean?

Also, when does it make sense to use this class?

答:
The subscriber connection callback is triggered when there's a new subscriber connected to a publisher.

There's documentation of how you can use that function in the Node Handle Documentation

It's not a commonly used feature. I don't know what your use case is but would gess that it is not something that you want to try to leverage.


答:
There is no definitive or concrete definition for what "Euler angles" means. It really varies by field / time period / convention. What's concrete is there are 6 distinct orderings of rotating around three different axes. See https://en.wikipedia.org/wiki/Euler_angles.

Note that rotating intrinsically (angle w.r.t the up-till-now transformed axes) around X, then Y, then Z is the same as rotating extrinsically (angle w.r.t. original axes) around Z, then Y, then X.

Note that what's listed as "Euler angles" in the wikipedia page are actually rotations around axes in the format ABA and "Tait-Bryan angles" are in the "more commonly used" (IMO) format of ABC (three distinct axes).

Title: calculation sequences when using RPY-transformation vs Euler-transformation

Body:
I have trouble understanding calculation sequences for RPY-transformation and Euler-transformation.

Let's assume that there are 3 (rotation/translation) instructions.

I will notate transformation matrix for instruction 1 as [1],

transformation matrix for instruction 2 as [2],

transformation matrix for instruction 3 as [3]

In this situation when using RPY-method, calculation order will be [3][2][1] -> [3][2 * 1] -> [3 * 2 * 1].

First multiply [2] and [1], and then multiply [3]. Right?

When using Euler-method, I am not sure what is correct order.

[1][2][3] -> [1 * 2][3] -> [1 * 2 * 3]

First multiply [1] and [2], and then multiply [3]

[3][2][1] -> [3 * 2][1] -> [3 * 2 * 1]

First multiply [3] and [2], and then multiply [1]

What is correct sequence when calculation Euler-method? Is it 1) or 2) ?

Also I think the resulting 4x4 matrix should be same whether I use RPY or Euler transformation. Is it correct?


问:
Quadcopter Position Measurement (Accelerometer, GPS or Both)?
I previously thought that an accelerometer on a quadcopter is used to find the position by integrating the data got from it. After I read a lot and watched this Youtube video (specifically at time 23:20) about Sensor Fusion on Android Devices, I seem to get its use a little correct. I realized that it's hard to filter out the considerable noise, generated from error integration,  to get useful information about the position. I also realized that it is used along with the gyroscope and magnetometer to for fused information about orientation not linear translation.

For outdoor flight, I thought of the GPS data to get the relative position, but is it so accurate in away that enables position measurement (with good precision)? How do commercial quadcopters measure positions (X,Y and Z)? Is it that GPS data are fused with the accelerometer data?

答:
Generally, for indoor flight, commercial quadcopters do not measure position. Instead, they measure the change in position so as to prevent the quadrotor from moving when it should not. So while accelerometers are not great for maintaining an estimate of the quadrotors position they can be used to stabilize the system, i.e. to determine what commands needed to be executed to maintain their current position.

Drift can still be a problem though. As such the data from the accelerometer is usually combined with other sources of information. The Kalman filter is one approach as the presenter touches on around minute 27. Another is to use a complimentary filter which are nice because they require less processing power. Some of the other sources for data for estimating the state of a quadcopter include the gyroscopes, magnometers, barometric pressure sensors, range finders (usually ultrasonic though I have seen IR sensors used), and optical flow via cameras.

With this additional information you could arguably still use an accelerometer for position estimation. I have not seen this done so I cannot speak to its effectiveness though. I suspect other approaches would be better. One method would be to use a laser range finder to localize the robot. Another would be to use a motion capture system as many research facilities do.

As a side note, beware that GPS gives absolute position $\pm \epsilon$ where $\epsilon$ accounts for error, i.e. give or take some error, as opposed to relative position.


问:
TF is broken when using robot_localization package with only IMU sensor, how to assure that the measured velocity is the one from the robot?
Im using thisrobot_lokalization robot_localization package for the linear velocity. As only sensor input in my case is IMU. I configure the parameters and set the frames as follow

```<code>map_frame: map
odom_frame: odom
base_link_frame: /thrbot/base_link
world_frame: odom
</code>```

The launch file is the following one

```<code><launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
    <rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
    <param name="odom_used" value="false"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu" args="0 0 0 0 0 0  /thrbot/base_link /thrbot/imu_link  100" />

</launch>
</code>```

The robot base_link frame is thrbot/base_link the IMU is thrbot/imu_link

Here is the photo of the TF without using the package . But when using the package my TF is broken as can see from the photo here . Any help how to fix the TF and correctly use the package with IMU as only sensor input?

To fix the TF i change the configure the parameters in ekf_template.yaml as follow

```<code>odom_frame: odom
base_link_frame: /thrbot/base_footprint
world_frame: odom
</code>```

and the launch file to follow

```<code><launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
    <rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
    <param name="frame_id" value="odom"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_odom" args="0 0 0 0 0 0 /odom /thrbot/base_footprint 100" />

</launch>
</code>```

Now the TF looks good as can see . Than following parameters are configure in ekf_template.yaml:

```<code>imu0: /thrbot/imu
imu0_config: [false, false, false,
              true, true, true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_remove_gravitational_acceleration: true
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
</code>```

My question is now as that correct configuration for underwater UUV to get linear velocity?

答:
Based on your configuration, you are trying to create map -> odom -> /thrbot/base_link. But you do not have a map frame. Looking at your oroginal TF, you should instead create world -> odom -> base_footprint.

I think your configuration should be:

```<code>odom_frame: odom
base_link_frame: /thrbot/base_footprint
world_frame: world
</code>```

EDIT: as pointed out,

```<code>world_frame: odom
</code>```

Body:
Based on your configuration, you are trying to create map -> odom -> /thrbot/base_link. But you do not have a map frame. Looking at your oroginal TF, you should instead create world -> odom -> base_footprint.

I think your configuration should be:

odom_frame: odom
base_link_frame: /thrbot/base_footprint
world_frame: world

EDIT: as pointed out,

world_frame: odom


问:
After planning a path, how do I ensure the robot is following the planned path?
After planning a path using a path-planning algorithm, e.g. RRT* or A* how do I ensure the physical robot is actually following the planned path?

After doing some research, I've come across Craig Reynolds' path-following algorithm.

However, given a fixed-size arena how do I know where the location of my robot in the arena and that it is about to traverse out of the path? I would imagine some sort of triangulation/gps is needed to know the location of the robot, am I right?

The robot we are given for this project is a 4 four-wheel steering robot that cannot rotate on the spot (non-differential drive). In addition, we only have infrared sensors and a camera.

Is there any way to make the robot follow the planned path with high accuracy? The planned path has some steering angles.

答: With problems like this, you will need apriori knowledge of your operating area. Since you know that it is a fixed-size arena, you could build a map and use localization within the map to know where you are / if you stray away from your path. There are several visual methods to create such a map. I'd suggest you start with ORB_SLAM.


答:
This problem can be solved by various types of controllers.

Firstly, I would suggest you to create intermediary points in between two consecutive points your Path Planning Algorithm calculates. Try to make the intermediary points frequent. The frequency of these intermediary points will also define the movement characteristic of the robot (agressive, slow but steady...). That, way you are going to have a trajectory for your robot to follow.

Then the most basic Controller type you can try would be a classical PID Controller. A PID controller will iteratively calculate the configuration error of your robot (position, and/or orientation), and then create control signals for your robot by multiplying, differentiating, and integrating this error. I will check my sources and may add some resources/articles for you to look.

Sometimes a PID Controller may not perform well. Given your robot type, you may need to try different Controller Algorithms, maybe you can look at some simpler methods; such as Stanley, Pure-Pursuit Controller.

For the precise detection of the robot location; this a gigantic area, with many different methods and approaches. You can use IMU sensor's accelerometer data to calculate the linear acceleration of the vehicle (be careful; the data you read directly from the accelerometer is not the pure linear acceleration), then double integrate it to have the position. Be warned that result of this operation will be nowhere near accurate. You may search for how to fuse different sensors' reading to improve this data. Or, if you have a small arena for your robot; you may use a LIDAR-like sensor or maybe a camera, which can perform precise localization.

As I mentioned, both localization and control fields have many different methods which vary in computational need. If you are new to this field, I would recommend you to study PID Controllers first.

Title: After planning a path, how do I ensure the robot is following the planned path?

Body:
After planning a path using a path-planning algorithm, e.g. RRT* or A* how do I ensure the physical robot is actually following the planned path?

After doing some research, I've come across Craig Reynolds' path-following algorithm.

However, given a fixed-size arena how do I know where the location of my robot in the arena and that it is about to traverse out of the path? I would imagine some sort of triangulation/gps is needed to know the location of the robot, am I right?

The robot we are given for this project is a 4 four-wheel steering robot that cannot rotate on the spot (non-differential drive). In addition, we only have infrared sensors and a camera.

Is there any way to make the robot follow the planned path with high accuracy? The planned path has some steering angles.


问:
No ROS 'sourceing' anymore after installing rosdep2
My ROS-system was working pretty well.

Then, I installed rosdep by using sudo apt install python-rosdep2, which ended fine without showing any issues. Then I issued

```<code>sudo rosdep init
sudo rosdep update
</code>```

which also ended without showing issues.

But now, my ROS-installation seems to be broken because I can't source my ROS any more!

For example, typing ROS-commands, such as

roscore or rqt

results in:

Sourcing, as usual with source /opt/ros/meldoic/setub.bash makes no difference.

What has happened? How to fix this?

Additional Info:

Typing ros<tab><tab> only shows me the following options anymore:

Where is all the rest gone?

答:
If you're using the ROS distribution melodic, it's likely that installing python-rosdep2 removed much of it from your system if you didn't pay attention to the apt prompts.

If you're using the packages.ros.org sources following the official installation you should be using python-rosdep not python-rosdep2 which comes from the Upstream repositories.

The same issue was found here for python3-rosdep2 and noetic but it's the same conflict.

Your solution will be to switch back to python-rosdep and reinstall your melodic packages.

Note that the answer by @pvl is sill important that you shouldn't use sudo rosdep update.

And I would recommend looking a little closer at what happens when you run apt and say yes/no to the prompts. ended fine without showing any issues likely also told you it was uninstalling most of melodic at the same time. (It was successful and exited cleanly, is not necessarily the same as doing what you wanted.)


问:
Floor Segmentation to Determine Navigable Paths
In my application, my robot has the following physical setup:

Differential drive mechanics with feedback (wheel encoders)

Commercially available webcam mounted with a known transform to the base of the robot (RGB, no depth)

The robot will be navigating through a structured, indoor type environment (think office, home, or university), and I would like to be able to determine the navigable paths through the environment using my vision sensor.

What is the best way to approach the problem of finding safe paths to travel when given a single vision sensor?

Edit:  I think that I am more interested in the vision processing techniques than the actual path-planning mechanics.

答:
Have a look at the literature that is available on this subject.
In principle you can go two different ways: behavior based or sense/plan/act.

for the behavior based approach there are a lot of ways you could achieve your goal, and it also depends on your environment. One very simple and elegant solution that I saw (don't have reference at hand) was to assume pixels that are just in front of you to be ground, and extend the pattern through some sort of flooding algorithm. The projection of this onto the ground plane is traversable. Optical flow could be another possible solution, which takes a little more processing power, but might be more robust on textured environments. Have a look at the literature around, I am sure there a plenty more ways.

more complicated, but more robust would be some sort of structure from motion to get a model of your environment. This is not so trivial with a monocular camera, but possible. You could then generate a traversability map and perform path planning (some sort of A* or D*) and then path following on it.


答:
Is ... GPS data ... fused with the accelerometer data?

Yes, many aircraft use sensor-fusion techniques so both GPS data and accelerometer data effect the estimated X, Y, Z position.
Often they use a Kalman filter to do the data fusion.
(  kalman-filter; Why do I need a Kalman filter? )

Measuring X,Y,Z accurately for each photo is important for assembling photos into an aerial photographic map. (a) (b)

Low-cost GPS receivers typically produce a X,Y,Z position once a second with an accuracy better than 20 meters. That is "good precision" for photographic mapping with a small RC aircraft or finding the destination runway in a manned aircraft. It's pretty much useless for a quadcopter flying in a room or a small field  less than 20 meters on a side.

(Some GPS receivers are available that use DGPS and other techniques to get better accuracy, or generate a position many more times per second, or both).

How do ... quadcopters measure positions (X,Y and Z)?

They usually don't.

My understanding is that most quadcopters aren't doing aerial mapping or fully autonomous motion, so they don't bother calculating XYZ position. (c) (d)

As you already know from that video,
quadcopters use the accelerometers to improve their estimates of the aircraft orientation (its pitch, roll, yaw).
The quadcopter needs to know the orientation in order to stay right-side-up and to avoid spinning uncontrollably.

How do ... quadcopters measure positions (X,Y and Z)?

The people at the GRASP lab at U Penn measure XYZ position of their quadcopters using lots of expensive motion capture cameras bolted to the walls. They produce lots of really impressive-looking videos.

A few cutting-edge research projects are trying to measure XYZ positions using cameras attached to the quadcopters.
("sFly Quadrotors Navigate Outdoors All By Themselves")
( Track a moving object )
("Encouraging Developments In Quadcopters (w. Lots Of Video)")

Before GPS, commercial aircraft used airport radio beacons to do instrument landing navigate.
I see some people talking about related techniques for radio beacon localization for quadcopters.

Ian suggests tagging each landmark with a unique augmented reality glyph.
Some people find it more practical to use a camera on the quadcopter to detect and estimate relative bearing to a simple red rectangular landing pad.
("Autonomous Quadcopter Docking System")

Title: Quadcopter Position Measurement (Accelerometer, GPS or Both)?

Body:
I previously thought that an accelerometer on a quadcopter is used to find the position by integrating the data got from it. After I read a lot and watched this Youtube video (specifically at time 23:20) about Sensor Fusion on Android Devices, I seem to get its use a little correct. I realized that it's hard to filter out the considerable noise, generated from error integration,  to get useful information about the position. I also realized that it is used along with the gyroscope and magnetometer to for fused information about orientation not linear translation.

For outdoor flight, I thought of the GPS data to get the relative position, but is it so accurate in away that enables position measurement (with good precision)? How do commercial quadcopters measure positions (X,Y and Z)? Is it that GPS data are fused with the accelerometer data?


问:
Peter Corke Robotics Matlab toolbox Panda DH/MDH conventions confusion
I am new to this toolbox and DH parameters in general but I noticed that the Peter Corke Robotics Toolbox for Matlab originally contains a model of the Franka Emika Panda robot. This model was made using the m-file script from the following link:

https://github.com/petercorke/robotics-toolbox-matlab/blob/master/models/mdl_panda.m

This script, shown below, states that the model was made using standard DH conventions.
But when you check the model using isdh(panda) and ismdh(panda) it says that the Panda model is defined using modified DH convention and not standard DH. Snippets of these checks are shown at the bottom.

```<code>%MDL_PANDA Create model of Franka-Emika PANDA robot
%
% MDL_PANDA is a script that creates the workspace variable panda which
% describes the kinematic characteristics of a Franka-Emika PANDA manipulator
% using standard DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%   qr         arm along +ve x-axis configuration
%
% Reference::
% - http://www.diag.uniroma1.it/~deluca/rob1_en/WrittenExamsRob1/Robotics1_18.01.11.pdf
% - "Dynamic Identification of the Franka Emika Panda Robot With Retrieval of Feasible Parameters Using Penalty-Based Optimization"
%   C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano and A. De Luca
%   IEEE Robotics and Automation Letters 4(4), pp. 4147-4154, Oct. 2019, doi: 10.1109/LRA.2019.2931248
%
% Notes::
% - SI units of metres are used.
% - Unlike most other mdl_xxx scripts this one is actually a function that
%   behaves like a script and writes to the global workspace.
%
% See also mdl_sawyer, SerialLink.

% MODEL: Franka-Emika, PANDA, 7DOF, standard_DH

% Copyright (C) 1993-2018, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function r = mdl_panda()

    deg = pi/180;
    mm = 1e-3;

    %% Define links (thanks Alex Smith for this code)
    L1 = RevoluteMDH('a',     0.0, 'd', 0.333, 'alpha',   0.0, 'qlim', [-2.8973 2.8973], ...
    'm', 4.970684, 'r', [3.875e-03 2.081e-03 0], 'I', [7.03370e-01  7.06610e-01  9.11700e-03 -1.39000e-04  1.91690e-02  6.77200e-03], 'G', 1);
    L2 = RevoluteMDH('a',     0.0, 'd',   0.0, 'alpha', -pi/2, 'qlim', [-1.7628 1.7628], ...
        'm', 0.646926, 'r', [-3.141e-03 -2.872e-02 3.495e-03], 'I', [7.96200e-03  2.81100e-02  2.59950e-02 -3.92500e-03  7.04000e-04  1.02540e-02], 'G', 1);
    L3 = RevoluteMDH('a',     0.0, 'd', 0.316, 'alpha',  pi/2, 'qlim', [-2.8973 2.8973], ...
        'm', 3.228604, 'r', [ 2.7518e-02 3.9252e-02 -6.6502e-02], 'I', [3.72420e-02  3.61550e-02  1.08300e-02 -4.76100e-03 -1.28050e-02 -1.13960e-02], 'G', 1);
    L4 = RevoluteMDH('a',  0.0825, 'd',   0.0, 'alpha',  pi/2, 'qlim', [-3.0718 -0.0698], ...
        'm', 3.587895, 'r', [-5.317e-02 1.04419e-01 2.7454e-02], 'I', [2.58530e-02  1.95520e-02  2.83230e-02  7.79600e-03  8.64100e-03 -1.33200e-03], 'G', 1);
    L5 = RevoluteMDH('a', -0.0825, 'd', 0.384, 'alpha', -pi/2, 'qlim', [-2.8973 2.8973], ...
        'm', 1.225946, 'r', [-1.1953e-02 4.1065e-02 -3.8437e-02], 'I', [3.55490e-02  2.94740e-02  8.62700e-03 -2.11700e-03  2.29000e-04 -4.03700e-03], 'G', 1);
    L6 = RevoluteMDH('a',     0.0, 'd',   0.0, 'alpha',  pi/2, 'qlim', [-0.0175 3.7525], ...
        'm', 1.666555, 'r', [6.0149e-02 -1.4117e-02 -1.0517e-02], 'I', [1.96400e-03  4.35400e-03  5.43300e-03  1.09000e-04  3.41000e-04 -1.15800e-03], 'G', 1);
    L7 = RevoluteMDH('a',   0.088, 'd',   0.107, 'alpha',  pi/2, 'qlim', [-2.8973 2.8973], ...
        'm', 7.35522e-01, 'r', [1.0517e-02 -4.252e-03 6.1597e-02 ], 'I', [1.25160e-02  1.00270e-02  4.81500e-03 -4.28000e-04 -7.41000e-04 -1.19600e-03], 'G', 1);

    %% Create SerialLink object
    robot = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', 'PANDA', 'manufacturer', 'Franka-Emika', 'tool', transl([0 0 110*mm]));

    qv = [0 0 0 -4 0 0 0]*deg;
    qr = [0 -90 -90 90 0 -90 90]*deg;

    % place the variables into the global workspace
    if nargin == 1
        r = robot;
    elseif nargin == 0
        assignin('caller', 'panda', robot);
        assignin('caller', 'qv', qv); % zero angles
        assignin('caller', 'qr', qr);
    end
end
</code>```

So I am confused regarding these results and would like to know if the Panda model generated by the script above is defined using standard DH parameters or modified DH parameters.

Thanks.

答:
The Panda robot uses a different convention than Spong's convention, which is what most students in American institutes learn DH from. DH, however, is just a convention, not a rule, so there are many ways DH can be interpreted. In this case, Peter is probably referring to Spong when referring to "standard" convention.
Spong uses this 4x4 matrix multiplication order (from left to right):

```<code>rotZ * translateZ * translateX * rotX
</code>```

Panda uses this order:

```<code>rotX * translateX * translateZ * rotZ
</code>```

Do note, however, that the translation matrices in the middle don't need to follow a particular order. Both of those translations can be done in a single matrix call.
I recommend building the required matrices manually as the Spong convention is what Peter's library uses.

Body:
The Panda robot uses a different convention than Spong's convention, which is what most students in American institutes learn DH from. DH, however, is just a convention, not a rule, so there are many ways DH can be interpreted. In this case, Peter is probably referring to Spong when referring to "standard" convention.
Spong uses this 4x4 matrix multiplication order (from left to right):

rotZ * translateZ * translateX * rotX

Panda uses this order:

rotX * translateX * translateZ * rotZ

Do note, however, that the translation matrices in the middle don't need to follow a particular order. Both of those translations can be done in a single matrix call.
I recommend building the required matrices manually as the Spong convention is what Peter's library uses.


问:
Do diagonal mecanum wheels require the rollers to cross the center of the robot
I'm currently in the process of making some changes to my robot to use mecanum wheels. The kinematic models I've seen assume the 45 degree roller from one diagonal wheel to the other, crosses the center of the robot and I've also seen some frame designs which don't bother with this at all. Would there be any major implications if X alignment doesn't cross the center of the robot?

Thanks

答:
From the Wikipedia article, the wheel will generate a propelling force perpendicular to the roller axel.

Note the yellow box isn't the outline of the robot, just a box representing the mounting points of the wheels. So to your question, overlay your robot on the image and analyze whether the pivot points are what you want/desire.


问:
Can inferences about servo torque or applied force be made by measuring servo current drain?
I'm working on a robot arm project for hobbyist / learning purposes. One of my DIY motivations is to be able to easily add additional sensors. I've been thinking about using ACS712 linear current sensors to measure the drain on each servo. (The hobbyist servos will be Gobilda 2000 series).

Googling has found reports of mixed experiences using current to make inferences about servos - some positive, some negative. Those claiming success seem to be using integration methods (either hardware or software) to get an indicative current over time value. This seems to filter out unpredictable instantaneous measurements.

My question is. Is measuring servo current drain a reasonable approach to make inferences about servo torque / force? If so, are there practical tips regarding implementation or limitations?

答:
Yes, measuring motor current will give you a rough indication of torque.  However, there are some caveats.

Any gear train that you have between the load and motor will decrease the usefulness of current measurement.  Some gear trains are better about this than others.  I'm not 100% sure, but I think this is directly related to its backdrivability.  At the extreme case, if you have a worm gear in there (which is not backdrivable at all), then measuring motor current will tell you very little about the load.  By far the best scenario for this is a direct-drive motor.

Be wary of hysteresis.  And getting different values when the load is or is not in the direction of motor motion.  Or when the motor is stopped.  Even very small motor motions can change the value you read.  For example, in [1], the authors dithered the motor to improve motor torque estimation.

Of course direct force/torque measurement is always going to be more accurate.  In almost all cases I would recommend direct measurement.

[1]: Stolt, Andreas & Robertsson, Anders & Johansson, Rolf. (2015). Robotic force estimation using dithering to decrease the low velocity friction uncertainties. Proceedings - IEEE International Conference on Robotics and Automation. 2015. 3896-3902. 10.1109/ICRA.2015.7139742.


问:
How to use .rosinstall files?
I'm trying to build the rotors package {https://github.com/ethz-asl/rotors_simulator}

I followed the steps below.

-i need to install the dependencies using the .rosinstall files in the source of the RotorS package. but i can't figure how to do that.

```<code><span class="math-container">$ mkdir -p ~/catkin_ws/src $</span> cd ~/catkin_ws/src
<span class="math-container">$ catkin_init_workspace  # initialize your catkin workspace
$</span> wstool init
<span class="math-container">$ cd ~/catkin_ws/src
$</span> git clone git@github.com:ethz-asl/rotors_simulator.git
<span class="math-container">$ git clone git@github.com:ethz-asl/mav_comm.git
$</span> cd ~/catkin_ws/
<span class="math-container">$ catkin init  # If you haven't done this before.
$</span> catkin build
</code>```

-I'm not sure how to use the files to install the dependencies. I read the documentation for rosinstall but I'm confused on which file to mention for path and URI. and how to install the packages.

I'm sorry if this is a basic question, I'm new to ros and I don't know all the features, can someone help me with it?

PS: I'm using Ubuntu Focal with ROS noetic.

答:
The usual way is to use: rosinstall . <your_package>.rosinstall

This answer will explain rosinstall clearly https://answers.ros.org/question/9213/how-exactly-does-rosinstall-work/

Title: How to use .rosinstall files?


答:
Position measurement is all about dealing with uncertainty.  Entire books have been written on how to estimate good position from uncertain data coming from several sources, so I won't try to summarize that here.

In general though, what you need will depend a lot on how accurate you need to be.  Getting GPS fixes once per second (5 times per second on some hardware) is great for something like a large boat.  However, 5 fixes per second is pretty bad when you're moving at high speeds in close quarters, like a quadcopter does -- not to mention the (roughly) 20-foot circle of uncertainty in GPS positions.

There are many position, velocity, and acceleration sensors that you can buy.  Only some of them are practical for any given application, and terribly few of them are good enough to be considered real-time ground-truth by themselves (either operating in tightly controlled environments, or costing you hundreds of thousands of dollars).  So, you need to pick a good mix of sensors so that each one can help keep the inaccuracies of the other sensors in check. This is entirely dependent on your robot and its application; there is no one-size-fits-all.

Typically, you'd combine a high-accuracy-low-frequency method like GPS (or other triangulation-based scheme) with a moderate-accuracy-high-frequency method like an accelerometer or gyro.  This keeps estimates from drifting too far while giving some reasonable real-time adjustments.


问:
Can someone explain this yaw angle calculation equation?
Nowdays, I am trying to understand PX4 Autopilot(Drone Flight Control Open Source) attitude control code. But I can’t understand formula below which is in attitude control code description.
Can anyone explain how that formula is induced?</p>
```<code>/*
The axis angle can change the yaw as well (noticeable at higher tilt angles).
This is the formula by how much the yaw changes:|
let a := tilt angle, b := atan(y/x) (direction of maximum tilt)|

**yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))).**
*/
</code>```

答:
This is probably a formula to model what is called "Adverse yaw" in aircraft dynamics. It is a very popular factor - along with Dutch Roll - in terms of fixed-wing aircraft design and control, as it causes the vehicle to make an undesired movement.

In terms of a quadcopter here is what I think: When you perform a "roll" maneuver, the propellers which contribute to the roll angle in the desired direction will have more speed than the others. That way the quadcopter is going to perform the roll. However, these higher speeds will also cause those propellers to produce more drag (recall the thrust and torque equations). Because of that reason, quadcopter may tend to perform yaw movement in the opposite direction according to other angle inputs (combined with pitch angle input).

As the roll angle increases, the adverse yaw may be expected to increase due to higher speed difference - therefore drag - between the propellers.

This formula might be a simplified version of complex aerodynamical interaction between the vehicle and air. They might have approximated it from a real-life data. Maybe if you do some research on "Adverse Yaw", you might find similar equations.

This may not be the complete answer you were looking for, but I hope that it would direct you towards a more clear way to understand the formula/interaction.


问:
MG996R issues with Arduino
I am working on a robot dog, and using 12 MG996R servos. I am powering the servos via a 2S LiPo battery, and I have the battery's negative tied to the Arduino's ground to prevent ground loop issues.

Upon powering up the battery, I immediately noticed that some servos twitched even though there was no program running (the Arduino was turned off at the time). I attempted to move the servos by hand, and while the others still moved freely, these four had a noticeable resistance to turning.

Note that I had not yet used servo.attach(); so the motors should still be able to rotate freely.

In addition, one of the four began smoking. I unplugged the battery quickly. Now, all the motors rotate freely by hand, except the one which smoked is very hard to turn.

Could someone tell me what happened? I double-checked my wiring and everything is good. Positives to positives, negatives to negatives, and signal wires to Arduino pins.

I've run MG996Rs on 2S LiPos in the past with no issues, so I'm not sure what would cause the problem now.

I bought the servos from several different sellers. There are a few from Tower Pro (these three work fine), some from a company called DeeGoo-FPV (The one that smoked came from this company, and three more), and some unlabeled ones (The other three broken ones, plus two working ones.)

EDIT: I opened the burned servo and measured the motor output as shorted. One of the chips was burned as well (probably the one that made the smoke)

答:
tl;dr: use 6 V

Those rascals are rated for 6 volts, not the nominal 7.4 V from 2S LiPos. You could imagine a Shenzhen manufacturer's design/value engineer zealously shaving a penny cost per unit by reducing safety factors and overstressing cheaper components, such that the servos would (mostly) work at 6 V but not necessarily at 7.4 V.

I once tried to run MG996Rs with 2S and, while not smoking out, they acted erratic and crazy. Retesting at 6 V, those same servos behaved correctly.

This all suggests a market opportunity for high-efficiency buck regulator modules to turn 7.4 V into 6.0 V. Better, of course, would be a line of 2S-rated servos for us to use.

Title: MG996R issues with Arduino


问:
Am I checking for a white pixel correctly?
I am currently going through all the pixels in an image to determine what is a white pixel. I then have to decide where to drive the bot. I am also using sensor_msgs/Image.msg that I get from the /camera/rgb/image_raw channel.

However, I can't seem to locate any white image with the code but the RGBa values I set in my model in gazebo all have value 1 as shown in the image below the code .

I logged all my values(more than once) with ROS_INFO_STREAM but no values are 255, let alone 3 consecutive ones.

```<code>void process_image_callback(const sensor_msgs::Image img)
{
  const int white_pixel = 255;

  const int image_slice_width = img.step / 3;

  int j = 0;
  bool found = false;

  for (int i = 0; not found and i < img.height; i++)
  {
    for (j; j < img.step-3; j += 3)
    {
      if (img.data[i*img.step + j] == white_pixel)
      {
      ROS_INFO_STREAM("img.data[i*img.step + (j + 0)]" + std::to_string(img.data[i*img.step + (j + 0)]));
      ROS_INFO_STREAM("img.data[i*img.step + (j + 1)]" + std::to_string(img.data[i*img.step + (j + 1)]));
      ROS_INFO_STREAM("img.data[i*img.step + (j + 2)]" + std::to_string(img.data[i*img.step + (j + 2)]));
      }
      // img.data only has one index
      if (img.data[i*img.step +  j     ] == white_pixel and
          img.data[i*img.step + (j + 1)] == white_pixel and
          img.data[i*img.step + (j + 2)] == white_pixel)
      {
          found = true;
          break;
      }
    }
    ROS_INFO_STREAM("End of j loop");
  }

  if (found)
  {
    // go left, forward or right
  }
  else
  {
    // no white pixel seen so stop the bot
  }
}
</code>```

答:
It looks like there might be a bug in your code as pointed to by @MorganStark47 above in the comment

You may also want to use Gazebo/White as the material versus Gazebo/Grey to get the full brightness. I think the settings your adjusting effect how it interacts with different light sources and may not fully override the texture which a grey will take off some of the brightness.

There's a list here: http://wiki.ros.org/simulator_gazebo/Tutorials/ListOfMaterials


答:
You said:

the RGBa values I set in my model in gazebo all have value 1

and you also said:

but no values are 255

Which I would expect is correct, because you just said you set them all to 1. Did you mean to scale them from 1 to 255 at some point? Casting 1.0 from float to uint8 will give you 1, not 255.

Title: Am I checking for a white pixel correctly?

Body:
I am currently going through all the pixels in an image to determine what is a white pixel. I then have to decide where to drive the bot. I am also using sensor_msgs/Image.msg that I get from the /camera/rgb/image_raw channel.

However, I can't seem to locate any white image with the code but the RGBa values I set in my model in gazebo all have value 1 as shown in the image below the code .

I logged all my values(more than once) with ROS_INFO_STREAM but no values are 255, let alone 3 consecutive ones.

```<code>void process_image_callback(const sensor_msgs::Image img)
{
  const int white_pixel = 255;

  const int image_slice_width = img.step / 3;

  int j = 0;
  bool found = false;

  for (int i = 0; not found and i < img.height; i++)
  {
    for (j; j < img.step-3; j += 3)
    {
      if (img.data[i*img.step + j] == white_pixel)
      {
      ROS_INFO_STREAM("img.data[i*img.step + (j + 0)]" + std::to_string(img.data[i*img.step + (j + 0)]));
      ROS_INFO_STREAM("img.data[i*img.step + (j + 1)]" + std::to_string(img.data[i*img.step + (j + 1)]));
      ROS_INFO_STREAM("img.data[i*img.step + (j + 2)]" + std::to_string(img.data[i*img.step + (j + 2)]));
      }
      // img.data only has one index
      if (img.data[i*img.step +  j     ] == white_pixel and
          img.data[i*img.step + (j + 1)] == white_pixel and
          img.data[i*img.step + (j + 2)] == white_pixel)
      {
          found = true;
          break;
      }
    }
    ROS_INFO_STREAM("End of j loop");
  }

  if (found)
  {
    // go left, forward or right
  }
  else
  {
    // no white pixel seen so stop the bot
  }
}
</code>```


问:
Jacobian for transformation of rotation angles
It is known that with the help of the Jacobi matrix it is possible to transform the angular velocities of the drive links $\boldsymbol{\omega}$ into the angular velocity of the platform $\boldsymbol{\Omega}$ through the Jacobi matrix.

$\boldsymbol{\Omega}=\boldsymbol{J}\boldsymbol{\omega}$

where $\boldsymbol{J}$ - Jacobi matrix.

What matrix is used to transform the corresponding angular positions?

$\boldsymbol{\Theta}=?\boldsymbol{\theta}$

答:
You use Forward Kinematics to calculate the end-effector pose of your robot/platform, given the joint angles.

There are many different methods to calculate the Forward Kinematics, I remember using the Product of Exponentials method, which works well if you have a good visual model/description of your platform. You may use Geometric Methods and functions (trigonometric) if you have a fairly simple platform, such as a SCARA Robot. Also, I have never used it but I believe that the most common approach to Forward Kinematics is something called "Denavit–Hartenberg Parameters". You can make a search on these ones according to your platform and needs.

If you want to know about this because of a position-following application; for example if you want your platform to follow a position trajectory, and desire to calculate the joint angles; you use an iterative approach to achieve that. If that is the case you can have a look one of my previous answers, regarding a similar case by clicking here:


问:
Publisher and subscriber in one ROS node error
I`m doing some underwater UUV Gazebo simulation and would like to use the

```<code>hector_pose_estimation
</code>```

package to fuse IMU and Fluid pressure sensors input for pose estimation.

So my Pressure Sensor has sensor_msgs/FluidPressure but the package wants pressure_height (geometry_msgs/PointStamped) as input. So I created a ROS C++ node to republish sensor_msgs/FluidPressure as the geometry_msgs/PointStamped. The code compiled with no error, but when run it I got error. Firstly, this is the code

```<code>#include "ros/ros.h"
#include "sensor_msgs/FluidPressure.h"
#include <std_msgs/Float64.h>
#include <geometry_msgs/PointStamped.h>

double current_pressure;
double depth;
ros::Publisher pub_ ;
geometry_msgs::PointStamped output;

void pressureCallback(const sensor_msgs::FluidPressure::ConstPtr& msg)
{

     current_pressure= msg->fluid_pressure;

     depth=current_pressure / (997.04*9.8);

     //geometry_msgs::PointStamped::ConstPtr& output;

     output.point.z = depth;
     //positions_pub.publish(output_msg);

     //output.z=depth;
     pub_.publish(output);

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pressure_republisher");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/rexrov2/pressure", 10, pressureCallback);
  ros::Publisher pub_ = n.advertise<geometry_msgs::PointStamped>("/pressure_height", 50);
  ros::spin();
  return 0;
}
</code>```

And when run it this it the ERROR:

```<code>[FATAL] [1633258406.824329181, 173.610000000]: ASSERTION FAILED
    file = /opt/ros/melodic/include/ros/publisher.h
    line = 106
    cond = false
    message =
[FATAL] [1633258406.826511155, 173.612000000]: Call to publish() on an invalid Publisher
[FATAL] [1633258406.826551511, 173.612000000]:
</code>```

Any help?

答: I believe you need to remove the declaration of the global ROS publisher outside the main function.</p>


答:
Try swapping the order in your main function, so your publisher exists before you start the subscriber callback. I also haven't tried running a publisher and subscriber from the same node handle and can't seem to find anything that explicitly says you can or can't do that.

If just swapping the order doesn't work, then try making separate node handles, one for the publisher and one for the subscriber.

Title: Publisher and subscriber in one ROS node error

Body:
I`m doing some underwater UUV Gazebo simulation and would like to use the

```<code>hector_pose_estimation
</code>```

package to fuse IMU and Fluid pressure sensors input for pose estimation.

So my Pressure Sensor has sensor_msgs/FluidPressure but the package wants pressure_height (geometry_msgs/PointStamped) as input. So I created a ROS C++ node to republish sensor_msgs/FluidPressure as the geometry_msgs/PointStamped. The code compiled with no error, but when run it I got error. Firstly, this is the code

```<code>#include "ros/ros.h"
#include "sensor_msgs/FluidPressure.h"
#include <std_msgs/Float64.h>
#include <geometry_msgs/PointStamped.h>

double current_pressure;
double depth;
ros::Publisher pub_ ;
geometry_msgs::PointStamped output;

void pressureCallback(const sensor_msgs::FluidPressure::ConstPtr& msg)
{

     current_pressure= msg->fluid_pressure;

     depth=current_pressure / (997.04*9.8);

     //geometry_msgs::PointStamped::ConstPtr& output;

     output.point.z = depth;
     //positions_pub.publish(output_msg);

     //output.z=depth;
     pub_.publish(output);

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pressure_republisher");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/rexrov2/pressure", 10, pressureCallback);
  ros::Publisher pub_ = n.advertise<geometry_msgs::PointStamped>("/pressure_height", 50);
  ros::spin();
  return 0;
}
</code>```

And when run it this it the ERROR:

```<code>[FATAL] [1633258406.824329181, 173.610000000]: ASSERTION FAILED
    file = /opt/ros/melodic/include/ros/publisher.h
    line = 106
    cond = false
    message =
[FATAL] [1633258406.826511155, 173.612000000]: Call to publish() on an invalid Publisher
[FATAL] [1633258406.826551511, 173.612000000]:
</code>```

Any help?


问:
Drive unicycle kinematics equations through the geometry
The unicycle dynamics in discrete-time is given by:

$$
\begin{array}{l}
{x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \cos ({\theta _{k + 1}})\\
{x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \sin ({\theta _{k + 1}})\\
{\theta _{k + 1}} = {\theta _{k }} + \Delta {\theta _{k + 1}}
\end{array}
$$

However, some researchers have introduced an additional term to the argument of the trigonometric functions above; for example see: Autonomous Land Vehicles: Steps towards Service Robots By Karsten Berns, and Ewald Puttkamer

$$
\begin{array}{l}
{x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \cos ({\theta _{k }} + \frac{{\Delta {\theta _{k + 1}}}}{2})\\
{x_{k + 1}} = {x_k} + \Delta {s_{k + 1}} \times \sin ({\theta _{k }} + \frac{{\Delta {\theta _{k + 1}}}}{2})\\
{\theta _{k + 1}} = {\theta _{k }} + \Delta {\theta _{k + 1}}
\end{array}
$$

I was wondering which one is geometrically correct? In other words, how we can show that $\cos ({\theta _k} + \frac{{\Delta {\theta _{k + 1}}}}{2}) = \cos ({\theta _{k + 1}})$?

According to the following figure, we have

$$
\Delta {x_{k + 1}} = \Delta {s_{k + 1}} \times \sin (\frac{{\Delta {\theta _{k + 1}}}}{2})
$$

This may seem to be a strange result, since we said that

$$
\Delta {x_{k + 1}} = \Delta {s_{k + 1}} \times \cos ({\theta _{k + 1}} + \frac{{\Delta {\theta _{k + 1}}}}{2})
$$

Edit

The solution of unicycle's system of ODEs with MIDPOINT METHOD:

$$
\begin{array}{l}
{t_{k + 1}} = {t_k} + \frac{T}{2}\\
{\theta _{mid}} = {\theta _k} + \omega \frac{T}{2}\\
\begin{array}{*{20}{l}}
{{x_{k + 1}} = {x_k} + v \times \cos ({\theta _{mid}}) \times T = {x_k} + v \times \cos ({\theta _k} + \omega \frac{T}{2}) \times T}\\
{{y_{k + 1}} = {y_k} + v \times \sin ({\theta _k} + \omega \frac{T}{2}) \times T}\\
{{\theta _{k + 1}} = {\theta _k} + \omega T}
\end{array}
\end{array}
$$

答:
tl;dr: don't worry about it

Your system really operates in continuous time, described by differential equations. It's a convenient, even necessary, approximation to use discrete time and difference equations. The dynamics solution would result from integrating the differential equations; the difference equations approximately solve the problem by doing numeric integration.

As it happens, our predecessors over the previous two centuries have left us a rich literature about numeric integration methods, and approaches to difference equations. I spent some time refreshing my undergrad training by browsing Wikipedia for "Numerical methods for ordinary differential equations", "Euler method", "Midpoint method", "Runge-Kutta methods", etc. Recall that in the 18th and 19th centuries, computation was vastly more expensive than now, so there was a big payoff for seeking every last bit of computational efficiency. There is no One Method to Rule Them All.

Some variants evaluate integrand terms in the middle of the discrete interval. I haven't bothered to actually crank through the math, but I expect the geometric formulation you show (with the angle bisector being perpendicular to the chord) winds up equivalent to the midpoint method -- giving rise to $\Delta \theta/2$. That's only one of several methods and variants that may (or may not) be the very best compromise between compute time and accuracy. When our predecessors fiddled the equations to improve convergence (or something), that is equivalent to fiddling the details of the geometry.

Nowadays, computation is cheap enough that we don't need to struggle to find the very best method -- any of them would probably work fine for us. We just need one that is adequate, and if you find you need more accuracy, burn some compute cycles by using a smaller discrete step size.

At the risk of seeming to be an overly pragmatic roboticist, I would suggest "don't worry about it".


问:
Convert sonar scans to Lidar scan file
Has anyone tried to map environments using Sonars or 1D ToF sensors? Since it's very difficult to implement such an algorithm from the scratch, I'm planning to use an existing ROS packages such as GMapping. For that we need to convert the readings of our sensors to a laser scan file.

I have a differential drive robot and 8 sonar sensors mounted on top of the robot in N, NE, E, SE, S, SW, W, NW directions. Using the odometry data I have implemented a Kalman filter to estimate the position of the robot. But as the robot moves the error of the position estimation accumulates.The same applies to when I'm trying to make a map of the environment incorporating the position estimation with the Sonar data. Therefore I need a pose correction method to increase the accuracy of the map. ROS GMapping is a very good package for this, but it is built only for LIDARs

If I can convert my Sonar readings and position estimation to a LIDAR scan file it can be passed to the GMapping package directly. So I'm asking for a method to do that.

Here is what I have done so far. Buffer poses and range measurements for 50 time steps, transform measured points to the world coordinate frame, calculate the mean position for the buffered positions and calculate the distance and angles for the points(I assume this is what LIDAR gives). The goal is to use this data with a existing SLAM packages in order to create a map with less errors.

答:
Assuming you mean a LaserScan ROS message. It's possible to use the geometry of your robot and laser positions to create one ray per sensor in the laser scan. This is basically going to be a manual process to do this. You can see someone else working out the math here

There's a lot of questions on this already: https://answers.ros.org/question/348155/slam-with-ultrasonic/

There's also people doing scanning ultrasonics instead of fixed arrays: http://cobecoballes-robotics.blogspot.com/2018/08/laserscan-ros-arduino.html

I'd also like to qualify that the noise model and discritization of the ultrasonic responses are very different than laser based returns and most of the SLAM algorithms will not give you good performance. There are several warnings to this effect

Title: Convert sonar scans to Lidar scan file