问:
gear motor with encoder and Arduino Uno
I'm working on a project where using a gear motor with an encoder and an Arduino Uno. I have not done Arduino in a long time and am struggling getting it to do what I want. If you look at the wiring guide for the motor below here is what I have currently wired up:

1 - M1 (-) wired to the ground of the voltage source, 2-GND Encoder wired to the GND pin on the Arduino, 3-Encoder A phase wired to Pin9, 4-Encoder B phase wired to Pin10, 5-3V3 Encoder + wired to 3V3 pin on the Arduino, 6-M1 motor (+) wired to the hot voltage source.

When I supply a voltage source the motor begins to spin. I want the motor to start not moving even when the power is supplied. Then I want to be able to set the angle that the position of the motor will turn to that spot and have it stop moving. I will then turn off the power supply and the motor will stay in that position until power is supplied again a new position is programed into the code. This motor can rotate in both directions. Please help even suggestions what commands to use will be great.

答: I completely agree with the previous answers,but for what you are trying to do a stepper motor would be a better option.</p>

Title: gear motor with encoder and Arduino Uno


问:
KDL Inverse kinematic and initial guess
I am using KDL Inverse Kinematic Solver (LMA) for a kinematic chain of 8 DOF. The problem is that the convergence to solution depends on the initial values. I run the algorithm with many initial values (ca 100 times) to increase the success rate. To generate the initial values I generate database consisting of thousands of joint value and cartesian pose pairs (using forward kinematics) in the neighborhood of my expected workspace. Before running IK, I search the database to get poses closest to the desired cartesian pose and use the paired joint space values as the initial guess.
In this way I find a solution for almost all cases, however it is very time consuming.

My questions:

Is it a good approach at all to generate such database of (joint space value, cartesian pose) pairs and perform the search?

Are there other approaches to mitigate the initial value problem?

Thanks.

答:
Unfortunately, (I think), you are doing the right thing.  But maybe there are some games to play...

The nature of IK is that it will give you a set of solutions.  (Or maybe in the KDL case it will give you a single solution, based on an initial configuration.  But anyways the concept is the same.)

In general, for a perfectly constrained problem, the number of IK solutions grows exponentially with the number of joints.  And if you are under-constrained, then the number of IK solutions is infinite.  But these infinite solutions form "islands" in configuration space.  Take for example this planar 2 link arm:

The two IK solutions (or islands of solutions) are: elbow up, and elbow down.  You wouldn't need to feed KDL thousands of initial configurations to get these two solutions.  You probably only need to give it an initial configuration close enough to each of these islands.

Optimization 1.  Know before-hand what kind of general arm configuration you want.

Sure, this arm configuration is valid for grabbing this flashlight:

But it is awkward and probably not good for the next sequence of actions.  You probably want a more "natural" looking arm configuration like this:

Optimization 2.  Once you get your single IK solution, it might be possible to use null-space techniques to move the redundant DoF into a better configuration?

ContentLicense: CC BY-SA 4.0

Title: KDL Inverse kinematic and initial guess


问:
What Properties of a Robot Arm Would Make its C-Space a Genus-2 Torus?
Once upon a time, I saw a webpage that had a complete list (with ellipses, of course) of robot arms that have each surface (closed [compact and without boundary], 2-dimensional manifolds) as their c-space (Well, maybe it was just the orientable ones; it was a long time ago). For instance, a spherical pendulum with a rigid rod as the link has $S^2$ as its c-space, and a two-link RR robot arm has the genus-1 torus $T^2$ as its c-space (see Figure 1 below). Would someone know what properties of a robot arm would make its C-Space a genus-2 torus (see Figure 2 below)?

Thanks much in advance, I am keenly interested in the solution to this question.

Figure 1

Figure 2

EDIT: I think Figure 3, or perhaps a similar four-bar linkage, may have been a robot arm with the genus-2 torus as its c-space.

Figure 3

答:
The C-space of a robot is tied to its actuators - helical (revolute plus linear) and prismatic (linear) actuators have C-space $\mathbb{R}$ while revolute joints have C-space $S^{1}$. The C-space then of any serial (and probably parallel I'm just not as familiar with these structure) chains is simply the Cartesian product of each individual joint. So, an RRP robot would have C-space $S^{1} \times S^{1} \times \mathbb{R}$.

To answer your question about a genus-2 torus as a robot's C-space, this seems very unlikely to be possible without a good deal of hand-waving magic. As helical and prismatic joints are both non-compact, we would have to rely on revolute joints. It should be noted that even spherical joints can be (and to my best knowledge are) modeled as combinations of revolute joints. Of course an unactuated spherical joint could be seen as having a C-space of $S^{2}$, but once motors are added I think it should be safe to assume the C-space is some number of Cartesian products of $S^{1}$.

So, how would we obtain:

$$ (S^{1} \times S^{1}) \ \# \ (S^1{} \times S^{1}) $$

where $\#$ is the connected sum? Considering this is the gluing of two torus together over some "slices" we have cut into each torus - we could view this as the C-space having some smoothly changing actuation features - where once some threshold has been crossed, we have reached a C-space in some ways disjoint from our previous C-space. I think perhaps what could accomplish this would be three revolute joints, such that joints 1 actuates the robot when joint 2 has some configuration parameter $-\frac{\pi}{4} \lt \theta \lt \frac{\pi}{4}$, they both actuate the robot when $\frac{\pi}{4} \lt \theta \lt \frac{\pi}{2} $ or $ -\frac{\pi}{2} \lt \theta \lt -\frac{\pi}{4} $, and finally joint 3 actuates the robot when $-\pi \lt \theta \lt -\frac{\pi}{2}$ and $\frac{\pi}{2} \lt \theta \lt \pi$ (these are not really disconnected - just an unfortunate side effect of using real numbers to represent $S^{1}$) (also I did not set equalities properly because I am lazy). As a final note, this construction is not completely accurate as far as I can tell. It appears joint 1 and/or joint 3 would have to be in certain configurations for the proposed "crossover" to happen. Also, I'm not entirely sure what the link structure would be either. There's likely many other issues with this - but it's a start.

So, perhaps such a robot is possible, but it is very, very unlikely that one exists already - plus the fact that the mechanical design of such a robot would be... difficult to say the least.

I did not notice you were asking for more than exclusively genus-2 torus C-space robots. Any sort of arm structure will have a genus-1 torus C-space. Any robot driven directly, or through a gear train, by motors will have a genus-1 torus C-space. It seems like these would satisfy what you are most interested in - so I would avoid anything with linear or helical actuators.

The C-space of figure 3 is a 2-dimensional compact manifold. We know it is 2 dimensional as there is a general formula for open chain manipulators - dimension of manifold is equal to $n - m$ when not at singular configurations (this will not affect every point on the manifold, but will increase the dimension of the manifold above $n-m$ at specific points). This is done by considering the closed chain manipulator as an open chain manipulator and using our knowledge of the inverse kinematics solutions for fixed end-effector locations.


问:
Using an IMU to determine the axis about which it is being rotated
I've got a 6DOF IMU and have, using some trig and and a few references such as ADI's app note: https://www.analog.com/en/app-notes/an-1057.html successfully implemented some Arduino/C code to calculate pitch, roll, and "tilt" angle for any given attitude.

Now, I would like to be able to detect about which axis the IMU is being rotated (in its own reference frame). So, picture grabbing one corner of the board on which the IMU is mounted and just lifting it up. This will rotate the IMU, and if I lift straight up, it should rotate in a plane, about that plane's perpendicular.

I would like to find the projection of that perpendicular into the XY plane of the IMU.

I should probably be able to figure this out, but am a bit stuck on it. Then also, all of the search terms I'm using don't readily distinguish the typical pitch and yaw examples of which there are many!

Thanks!

答:
If you look at a Bode plot you can design a simple low pass filter by using something like this:
$v_n = c_1 \:v_{n-1} + c_2 \: v_{n,raw}$. This is called a ``difference equation'' and fairly simple to implement in Arduino code. $v_{n,raw}$ is your currently measured velocity, $v_n$ is the velocity resulting from the low pass filter and $v_{n-1}$ is the previous time step's velocity which resulted from the low pass filter. $c_1$ and $c_2$ are coefficients which you have to choose, and this will change your Bode plot. The coefficients need to sum up to 1. You can simply use MATLAB and look up the transfer function of a low pass filter in continuous time, it should look something like $G(s)=\frac{\omega_{cutoff}}{s + \omega_{cutoff}}$. Knowing your sample time of your IMU recording allows you to apply the c2d-command in MATLAB on $G(s)$ and voila you have your discrete transfer function and you have your coefficients $c_1$ and $c_2$. So it all depends on your cutoff frequency.

From my experience: If you use an MPU6050 the velocity should drift too much. There is a nice documentation for an Arduino library by Romain JL Fetick. The library is called MPU6050_light if I remember correctly.


问:
Minimize Waypoints Why Still Maintaining Distance From Objects for Motion Planning?
I'm working on a project where I want to give instruction to blindfolded participants on how to walk around an obstacle course. I'll map out my room and convert it to a grid map. However, given that I want to minimize the amount of instruction(turns) to give to each person, what algorithm can I use that minimize the amount of waypoints generated, as well as gives some room from obstacles, kind of like in in a potential field algorithm?

My biggest challenge right now is how to convert the path into a series of waypoints that are less granular than the original path. Any ideas?

答: In addition to Dijkstra a really simple to understand and implement algorithm is APF, which stands for Artificial Potential Field. The main idea behind it is that you "build" a potential field in the space so that the robot/participant is attracted by the goal and repelled by the obstacles. The total potential field is the sum of attractive and repulsive potentials. Applying a gradient decent on top of that would find an optimal solution for your problem. The problems with this algo is that you might get stuck to local minimals, and it's performance is poor in dynamic environment.</p>


问:
Monte-Carlo Localization
I'm implementing Monte-Carlo localization for my robot that is given a map of the enviroment and its starting location and orientation. Mine approach is as follows:

Uniformly create 500 particles around the given position

Then at each step:

motion update all the particles with odometry (my current approach is newX=oldX+ odometryX(1+standardGaussianRandom), etc.)

assign weight to each particle using sonar data (formula is for each sensor probability*=gaussianPDF(realReading) where gaussian has the mean predictedReading)

return the particle with biggest probability as the location at this step

then 9/10 of new particles are resampled from the old ones according to weights and 1/10 is uniformly sampled around the predicted position

Now, I wrote a simulator for the robot's enviroment and here is how this localization behaves:
http://www.youtube.com/watch?v=q7q3cqktwZI

I'm very afraid that for a longer period of time the robot may get lost. If add particles to a wider area, the robot gets lost even easier.

I expect a better performance. Any advice?

答:
I just answered the question on StackOverflow, where it already been asked. Here is the link:

https://stackoverflow.com/questions/21249131/monte-carlo-localization-for-mobile-robot

ContentLicense: CC BY-SA 3.0

Title: Monte-Carlo Localization


问:
Difference between stripboard and breadboard
I'm new around here. I've observed individuals using stripboard.
Could you please explain the difference between a breadboard and a stripboard?
Are  stripboards internally connected ? I've  stripboard with tracks cut into them in certain applications. I'm not sure why this is carried out.</p>

答:
I googled it, and stripboard looks like breadboard, but it's permanent.

Breadboards have spring-loaded clips that hold the leads pushed in, so you can push components in and make electrical connections without soldering. There are no such spring-loaded clips with stripboards, meaning you'd have to solder everything in place.

:EDIT:

Here's a picture of a breadboard. You can see the holes; again there are spring-loaded clips that hold the components once you push them in. In this picture, the "rails" are connected, so the first column is one bus, the second column is one bus, the second-to-last column is one bus, and the last column is one bus. For the interior section, rows a/b/c/d/e are connected and rows f/g/h/i/j are connected.

Here's a picture of a stripboard. Here each row is electrically connected.

Here's a picture of a stripboard that's arranged the same way as a breadboard. You can see the four rails and lots of rows, again separated a/b/c/d/e and f/g/h/i/j.

And finally, maybe what you're asking about, is perfboard, which has metal vias that can be soldered to, but there are no electrical connections between any holes. This would require you to solder jumpers... lots of jumpers lol. The "easiest" way to do this would be to leave the leads long on the through-hole devices and solder jumpers to the leads, or you could lay down the leads and make solder bridges between vias,  but really the easier thing would be to use stripboard.


问:
Ubuntu ARM lacking /sys/devices/cape-bone-iio
I'm trying to pull analog input from a beaglebone black using this tutorial. However when I go to /sys/devices there is no cape-bone-iio. I have spoken with several other programmers and one of them suggested that the cape-bone does not work with the newer versions of Linux. However downgrading could have negative impact on the rest of the project. Is there any other solution?

答:
I found the files for the analog inputs on the Beaglebone Black here:

/sys/devices/platform/ocp/44e0d000.tscadc/TI-am335x-adc.0.auto/iio:device0/in_voltage#_raw
(where # is the pin number, 0 through 7)

Didn't downgrade my kernel. No enabling or setup needed. Those files are just continuously populated with the voltage readings from those pins. Values are 12 bit (0 to 4095, which corresponds to 0.0 volts to 1.8 volts).

Or at least this is how my Beaglebone Black works.


问:
Trajectory tracking for a robotic arm
I want a robotic arm to do trajectory tracking in the joint space. Suppose the waypoints are
$$
[t(0),q_1(0),q_2(0),q_3(0)],\\
\cdots,\\
 [t(n-1),q_1(n-1),q_2(n-1),q_3(n-1)]
$$
I already know that I can use these waypoints to get a smooth trajectory $[q_1(t),q_2(t),q_3(t)]$ for each joint through interpolation, but how to control the motor (current control or torque control is supported) to track the smooth trajectory properly? More specifically, how to make sure that these motors could keep in sync?

Title: Trajectory tracking for a robotic arm

Body:
For simplicity let's consider a 2R planar manipulator. This method can be expanded to any robot as long as you can define error-proof dynamics. Equations of motions for this robot can be defined by the following equations.

EoM 1 :

<img src="https://latex.codecogs.com/svg.image?u1%20=%20%5Ctheta_%7B%5Cddot%7B1%7D%20%7D%20%5C,%7B%5Cleft(m_2%20%5C,%7Bl_1%20%7D%5E2%20+2%5C,m_2%20%5C,%5Ccos%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,l_1%20%5C,r_2%20+m_1%20%5C,%7Br_1%20%7D%5E2%20+m_2%20%5C,%7Br_2%20%7D%5E2%20+I_1%20+I_2%20%5Cright)%7D-%5Ctheta_%7B%5Cdot%7B2%7D%20%7D%20%5C,%7B%5Cleft(l_1%20%5C,m_2%20%5C,r_2%20%5C,%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)+l_1%20%5C,m_2%20%5C,r_2%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,%7B%5Cleft(%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20+%5Ctheta_%7B%5Cdot%7B2%7D%20%7D%20%5Cright)%7D%5Cright)%7D-u_1%20+%5Ctheta_%7B%5Cddot%7B2%7D%20%7D%20%5C,%7B%5Cleft(m_2%20%5C,%7Br_2%20%7D%5E2%20+l_1%20%5C,m_2%20%5C,%5Ccos%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,r_2%20+I_2%20%5Cright)%7D-g%5C,m_2%20%5C,r_2%20%5C,%5Csin%20%5Cleft(%5Ctheta_1%20+%5Ctheta_2%20%5Cright)-g%5C,l_1%20%5C,m_2%20%5C,%5Csin%20%5Cleft(%5Ctheta_1%20%5Cright)-g%5C,m_1%20%5C,r_1%20%5C,%5Csin%20%5Cleft(%5Ctheta_1%20%5Cright)" alt="EoM 1" />

EoM 2:

<img src="https://latex.codecogs.com/svg.image?u2%20=%20%5Ctheta_%7B%5Cddot%7B2%7D%20%7D%20%5C,%7B%5Cleft(m_2%20%5C,%7Br_2%20%7D%5E2%20+I_2%20%5Cright)%7D-u_2%20+%5Ctheta_%7B%5Cddot%7B1%7D%20%7D%20%5C,%7B%5Cleft(m_2%20%5C,%7Br_2%20%7D%5E2%20+l_1%20%5C,m_2%20%5C,%5Ccos%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,r_2%20+I_2%20%5Cright)%7D-g%5C,m_2%20%5C,r_2%20%5C,%5Csin%20%5Cleft(%5Ctheta_1%20+%5Ctheta_2%20%5Cright)+l_1%20%5C,m_2%20%5C,r_2%20%5C,%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,%7B%5Cleft(%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20+%5Ctheta_%7B%5Cdot%7B2%7D%20%7D%20%5Cright)%7D-l_1%20%5C,m_2%20%5C,r_2%20%5C,%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20%5C,%5Ctheta_%7B%5Cdot%7B2%7D%20%7D%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)" alt="EoM 2" />

Here, u1 and u2 are torques required for joints 1 and 2 respectively.

The rest of the parameters are shown in the Image.

Note that, these equations can be derived using the lagrangian method.

These EoMs can be written in manipulator form.

The corresponding matrices are :

<img src="https://latex.codecogs.com/svg.image?M%20=%20%5Cleft(%5Cbegin%7Barray%7D%7Bcc%7Dm_2%20%5C,%7Bl_1%20%7D%5E2%20+m_1%20%5C,%7Br_1%20%7D%5E2%20+m_2%20%5C,%7Br_2%20%7D%5E2%20+2%5C,l_2%20%5C,m_2%20%5C,%5Ccos%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,r_2%20+I_1%20+I_2%20%20&%20m_2%20%5C,%7Br_2%20%7D%5E2%20+l_1%20%5C,m_2%20%5C,%5Ccos%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,r_2%20+I_2%20%5C%5Cm_2%20%5C,%7Br_2%20%7D%5E2%20+l_1%20%5C,m_2%20%5C,%5Ccos%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,r_2%20+I_2%20%20&%20m_2%20%5C,%7Br_2%20%7D%5E2%20+I_2%20%5Cend%7Barray%7D%5Cright)" alt="M" />

<img src="https://latex.codecogs.com/svg.image?C%20=%20%5Cleft(%5Cbegin%7Barray%7D%7Bcc%7D0%20&%20l_1%20%5C,m_2%20%5C,r_2%20%5C,%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)+l_1%20%5C,m_2%20%5C,r_2%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,%7B%5Cleft(%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20+%5Ctheta_%7B%5Cdot%7B2%7D%20%7D%20%5Cright)%7D%5C%5Cl_1%20%5C,m_2%20%5C,r_2%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)%5C,%7B%5Cleft(%5Ctheta_%7B%5Cdot%7B1%7D%20%7D%20+%5Ctheta_%7B%5Cdot%7B2%7D%20%7D%20%5Cright)%7D-l_1%20%5C,m_2%20%5C,r_2%20%5C,%5Ctheta_%7B%5Cdot%7B2%7D%20%7D%20%5C,%5Csin%20%5Cleft(%5Ctheta_2%20%5Cright)%20&%200%5Cend%7Barray%7D%5Cright)" alt="C" />

The desired torques to track required trajectory can be computed by :

This equation is obtained by feedback linearization . You can read more about it here.  In this equation, K matrix contains PD gains. Desired theta and desired angular velocity can be computed using either cubic
or quentic polynomial.

If you have a starting angle and ending angle and theie velocities at two instants , the cubic polynomial to track the trajectory would be :

You can compute the coefficients of these cubic polynomials based on your desired trajectory. If you want jerk free motion , you can add a fifth order term to your polynomial.

You can try different PD gains or you can compute them using eigen value placement. For this robot K matrix is 2X4. You need to use following state and input matrices for eigen value placement.

Results of this implementation looks as shown below.


问:
Kinect for Xbox: SDK selection
My application is basically about sound source localization and visual servoing. I selected Kinect as the main hardware.

I already know the basic differences between Kinect for Windows and Kinect for Xbox. I cannot access to windows version from my country (no reseller here in Turkey), but the xbox version is there at the stores. I am not sure about problem specific software selection.

I found out that the latest Kinect SDK supports sound source localization (and beamforming) using the built-in microphone array. Can I use that SDK within the xbox version? Or is there another SDK for xbox, having the same support? I am not sure because I also read that OpenNI does not provide the best audio API.

I will also apply some processing on image & depth outputs, so I will be using OpenCV. I also want to use Qt for threading, GUI etc. So, another question: Is it possible to use the microsoft official kinect SDK within another IDE, not Visual Studio?

答:
There is some discussion about this on stackoverflow. Please check out this link:

https://stackoverflow.com/questions/9936272/using-xbox-360-kinect-with-kinect-for-windows-sdk

Title: Kinect for Xbox: SDK selection


问:
What type of actuators are needed for a high accuracy, low load weight robotic arm?
I am trying to build a robotic arm that needs to lift a very low amount of weight (between 0.01-0.05kg) at a time, but it needs to be very precise (+/- 1mm accuracy ideally). The robot really only needs to have 3 DOF and the arc is roughly 0.5 meters. Additionally, the robot doesn't need to be very fast, only around 0.2-0.5 m/s is necessary.

What type of actuator would be the best suited for this kind of task, and what other factors would I have to consider? Do I need to spend more money for high quality servos, or can the task be done with hobby/RC servos with some modifications?

答:
Ask yourself if you really need accuracy or precision.  Because I think precision is easier to achieve than accuracy for most arms.

I am not sure I would be as worried about the actuator as the gear-train and sensing.  Here are some back of the envelope calculations to illustrate:

At 0.5 meters, to achieve an error of 1 mm, that is $asin(1/500) = 0.1146^\circ$.  (And there will probably be an elbow halfway down the arm with similar backlash, compounding the error).  You probably want your gear train to have a backlash less than that or else you will have a hard time controlling the arm.

To sense this error, your joint encoder will need a resolution of $360^\circ/0.1146^\circ = 3141.6$ ticks/rev.  And you will really want these joint encoders after the gear train to be able to measure and account of the above mentioned backlash.

Thankfully, your payloads are light, so arm sag is probably not a concern.


问:
Articulated Body Algorithm with gear ratio
I am reading up on the proposed articulated body algorithm by Featherstone, which goes like:

Pass 1:
\begin{equation}
\nu_0 = 0\\
\nu_i = {^i}X_{\lambda(i)}\nu_{\lambda(i)}+s_i\dot{q}_i\\
c_i = \nu_i \times s_i\dot{q}_i\\
p_i=\nu_i \times^* I_i\nu_i
\end{equation}
Pass 2:
\begin{equation}
I^A_i=I_i+\sum_{j\in\mu(i)} {^i}X_{j}^* I{^a}{_j} ^{j}X_{i}\\
p_i^A = p_i + \sum_{j\in\mu(i)}{^i}X_{j}^*p_j^a\\
h_i = I^A_i s_i\\
d_i = s_i^T h_i\\
u_i = \tau_i - s_i^T p_i^A\\
I_i^a = I_i^A - h_ih_i^T/d_i\\
p_i^a = p_i^A + I_i^a c_i + h_iu_i/d_i
\end{equation}
Pass 3:
\begin{equation}
a_0 = -a_g\\
a_i^` = {^i}X_{\lambda(i)}a_{\lambda(i)} + c_i\\
\ddot{q}_i = \left(u_i-h_i^Ta_i^`\right)/d_i\\
a_i = a_i^`+s_i\ddot{q}_i
\end{equation}

The algorithm seems just to work if I assume for a robotic arm a direct drive. Is there a way to add gear ratios with the mass and inertia of rotors to this algorithm? Any help pointing to a reference is greatly appreciated.

答:
In Featherstone's book "Rigid Body Dynamics Algorithms", there is a section of Chapter 9 (specifically, 9.6) dedicated to explaining how to incorporate gears into a given dynamic model.

At a high level, you need to incorporate the gear ratios into the dynamics via a similar process to how a kinematic loop would be handled, by introducing additional joint variables for the additional moving parts and then mapping them onto the standard (typical) joint variables in the algorithm above.

The example shared in the book describes a simple geared system where there are geared motors driving the angular positions of two links, which can be described as the following set of four moving components (assuming small gears drive large gears at each joint and the stator of the first motor is fixed):

The first link, the first large gear, and the stator of the second motor;

The first small gear and the rotor of the first motor;

The second link and the second large gear; and

The second small gear and the rotor of the second motor.

This means that joints 1 and 3 are the typical joints you would expect, while joints 2 and 4 are rotations within the motors driving the gears, and leads to a relationship between the joint positions in $q$ and the gear ratios $n_1$ and $n_2$. Specifically, if we define independent variables as $y_1 = q_1$ and $y_2 = q_3$, then we can define a function $\gamma$ to map from $y$ to $q$ as $\gamma = [\begin{matrix} y_1 & n_1 y_1 & y_2 & n_2 y_2 \end{matrix}]^T$.

Once we've done that, we can define the matrix $G = {\partial \gamma}/{\partial y}$ and the vector $g = \dot G \dot y$ to get to the following dynamic equation of motion (where $H$ and $C$ are the coefficients of the equation of motion without gearing):

$$G^T H G \ddot y + G^T (C+Hg) = G^T \tau$$.

Note: In this formulation, $\tau_1 = \tau_3 = 0$ while the motor torques will appear in $\tau_2$ and $\tau_4$. Using this equation, you can then apply the inverse dynamics equations as needed, such as the ABA you've outlined above.


问:
Roomba iRobot Create 2 Open Interface, is there a way to turn off vacuum motor while the robot is docking?
I am able to control every desired aspect of my robot, except for vacuum motor which seems to be turned on for some reason every time I send Seek Dock (code 143) command. Is there a way to turn it off? I was trying to send Motors (138) command with argument 0 after sending Seek Dock, but docking also means changing OI mode to Passive and Motors are not accepted in passive.

I would also like to understand the rationale behind having vacuum motor turned on during docking.

答:
The vacuum is turned on while docking because during normal cleaning missions the robot avoids getting too close to the dock in an effort not to bump it or move it.  So the area close to the dock doesn't get cleaned and dirt and debris can still build up on the dock.  We want to clean the dock because dirty contacts can cause problems with charging.

Disclaimer: I work at iRobot where I am developing the next generation of consumer robots. However, my postings on this site are my own and don't necessarily represent iRobot's positions, strategies, or opinions.


问:
odometry on 3wd omnidirectional mobile robot
I found formulas for the speeds of the robot in several directions Vx, Vy and w, when I try to find the coordinates of the robot from these speed.I want to define the global coordinates of the robot.

formulas i got from here https://github.com/michele-bertoni/omni3/blob/main/omni3.cpp#:~:text=this%2D%3E-,displacement%5BFORWARD%5D,-%3D%20T30R%20*

wR, wL, wB this angular speed wheel right, wheel left and wheel back.

```<code>public void forwardKinematics() {
    this.Vx = Math.tan(Math.toRadians(30)) * (this.wR - this.wL);
    this.Vy = Constants.WHEEL_RADIUS / 3 * (this.wR - 2 * this.wB + this.wL);
    this.w = (Constants.WHEEL_RADIUS / (3 * Constants.ROBOT_RADIUS)) * (this.wR + this.wB + this.wL);
}
</code>```

my odometry code

wheelSpeed[0] = Vx

wheelSpeed[1.]  = Vy

dt = 0.02

```<code>  x =  wheelSpeed[0] * 0.02,
  y =  wheelSpeed[1] * 0.02,
  w =  angle.minus(prevAngel).getRadians()
</code>```

I have tried these equations:

But for some reason I got local coordinates. i want to get global coordinates to implement driving by coordinates.

答:
It sounds like all you want is dead reckoning for a holonomic robot.

Here is our setup:

Here, $X_W$, $Y_W$, and $\theta_W$ are our World frame.  $X_L$ and $Y_L$ are our Local frame.

Where ever you "turn on" the robot, this will be the world origin.  You will have to keep track of the world pose.  And you will keep adding to it with incremental updates.

You are correct that you need to get the displacement since the last time step:

$\Delta X_L = V_X * \Delta t$

$\Delta Y_L = V_Y * \Delta t$

$\Delta \theta = \omega * \Delta t$

And one more that will be handy:

$H_L = \sqrt{{\Delta X_L}^2 + {\Delta Y_L}^2}$

Let's zoom in on the incremental displacement;

Now you just have a little trigonometry to get the incremental displacements in the world frame:

$\Delta {X_W} = H_L \sin(\frac{\pi}{2}-\tan(\frac{\Delta Y_L}{\Delta X_L})-\theta_W)$

$\Delta {Y_W} = H_L \cos(\frac{\pi}{2}-\tan(\frac{\Delta Y_L}{\Delta X_L})-\theta_W)$

Then do the update:

$X_{W_{new}} = X_{W_{old}} + \Delta X_W$

$Y_{W_{new}} = Y_{W_{old}} + \Delta Y_W$

$\theta_{W_{new}} = \theta_{W_{old}} + \Delta \theta$

And you are done!

But some things to keep in mind regarding dead reckoning like this:

it is going to diverge from reality probably very quickly.  there are things like wheel slip, errors in your measured wheel diameter, uneven terrain, etc.

you should measure actual wheel rotations with encoders.  rarely will your motors rotate at exactly the prescribed velocities.

dead reckoning is OK for short periods of time, and is a reasonable local assumption, but you usually want to supplement it with some "global" sensing.


问:
Is it better to have batteries distributed at the rotors or the center of the multicopter?
I've seen 3 approaches to mounting batteries on a multicopter:

All the batteries rigidly mounted near the center of the airframe

All the batteries in a bag hanging under the center of the airframe

Each rotor has its share of the batteries rigidly mounted near/under it. (For example, a quadcopter with 1/4 of all the batteries mounted underneath each motor).

Which design is the best, and why?
If there is no one best design, what are the advantages/tradeoffs between the designs?
Is there some other design I'm overlooking that is better in some way?

(This question focuses on multirotor flying machines.
For ground vehicles, see " Is it better to have weight distributed over the wheels or the center of the robot? ").

答: Center, you want the moment of inertia to be as small as possible.</p>


问:
How can I start programming proto X quad
I have bought a really small proto X quad (it has a joystick which navigates the device) and I am looking for a way to send a signal to this thing from my computer.

So can anyone point me how can I turn on one of the propellers of this quad using my laptop (I have a decent knowledge in python/matlab/C# but hardware is a completely new world to me).

答:
I can think of 2 ways:

(1) Open it up, get rid of off the control board in the quad and only keep the motors and electronic speed controller. There should one per motor, but they might be integrated in the main board in which case you'd have to get some yourselves. Then get anarduino (a programmable microcontroller) to control the motors. The communication with a laptop can then be done via an RF signals (using e.g. xbees) and use another arduino as a transmitter plugged into your laptop to control it.

There a lots of tutorials and blogs online about this kind of projects, with open source code available etc.

(2) Probably the easiest option: Reverse engineering the RF signals between the quad and the remote control and replicate them from your laptop. To do that, you'll have to know the frequency that's being used (probably written at the back of the remote), buy a receiver + transmitter for that frequency. Use the receiver to "listen" to what the signals are when using the remote. Then you can try to recreate those signals in software and use the transmitter to send those across to the quad.

Sounds difficult, but it's probably the easiest option. See this magpi article - page 8 that describes how to do this - This was for a totally different project but the method still stands.


问:
Smooth step function Simulink
I'm trying to implement a smoothed step function in Simulink.

I tried to put a filter after the step function:

The result is not what I wanted.
How can I smooth even around 1 the function?
My deal is to not have a dramatic change on behavior.

A possibile idea is to have something like:

Thanks in advance

答:
So you could use the ramp block, but that only has a turn-on time and a slope; there's no limiting it once it's turned on.

What I prefer to use instead is the repeating sequence block, which lets you define an arbitrary output/time pairs.

If your simulation runs for 10 seconds, and you want a ramp that outputs zero at t=0, one at t=1, and holds that value for the full simulation, then you would give time values of:

```<code>[0, 1, 10]
</code>```

and you would give output values of:

```<code>[0, 1, 1]
</code>```

which would build you that ramp. Then, if you want to smooth the corners on that ramp, you can pass the output through the same filter you used for the step input.

Title: Smooth step function Simulink

Body:
So you could use the ramp block, but that only has a turn-on time and a slope; there's no limiting it once it's turned on.

What I prefer to use instead is the repeating sequence block, which lets you define an arbitrary output/time pairs.

If your simulation runs for 10 seconds, and you want a ramp that outputs zero at t=0, one at t=1, and holds that value for the full simulation, then you would give time values of:

[0, 1, 10]

and you would give output values of:

[0, 1, 1]

which would build you that ramp. Then, if you want to smooth the corners on that ramp, you can pass the output through the same filter you used for the step input.


答:
There are a variety of functions that can give you an "S" curve like you want.  Check out the Sigmoid function.

I usually use something like this:

$f(x) = \frac{x}{\sqrt{1+x^2}}$

And it can be adjusted for varying amounts of "steepness".

$f(x) = \frac{x}{\left(1 + |x|^{k}\right)^{1/k}}$

Here is one in the range you specified:

$f(x) = \frac{1}{(1.0 + e^{-10x+5)}}$

Title: Smooth step function Simulink

Body:
I'm trying to implement a smoothed step function in Simulink.

I tried to put a filter after the step function:

The result is not what I wanted.
How can I smooth even around 1 the function?
My deal is to not have a dramatic change on behavior.

A possibile idea is to have something like:

Thanks in advance


问:
how to stop the joint motion( clear the force)
I have set a force on the shoulder of simple_arm and it seems that It is going to move forever until I stop it somehow.

How can I clear/stop the force that I have set by Joint::SetForce() on a joint? In general how can we stop the motion of a model in programming.
Is there any function to stop the model?

Originally posted by nkoenig on Gazebo Answers with karma: 7661 on 2012-09-19 23:07:26.404522 UTC
Post score: 0

---JSON_START_ROS2022_SE_MIGRATION {'score': 0, 'name': 'nkoenig', 'user_id': 5, 'timestamp': '2012-09-19 23:07:26.404522 UTC'}---

答:
Where in you'r code have you applied the force ?
are you absolutely sure it is not in the onUpdate() or similar never ending call ?

What is the purpose?

have you got friction in the joint ?
according to newtons laws of motion, the arm will continue to rotate forever, unless something provides a reaction force, like friction.

if you apply the force in a small duration of time, the turnrate of the arm will after that moment in time become constant. With frictions it will decellerate, with repeated force the arm will accellerate.
with repeated force and friction the arm will reach a steady state where you'r applied force equals the force from friction, and from here the speed will eventually become constant.

solutionset:

if you are repeatingly applying force, stop that after some time and let the friction stop the arm (use gazebos simulation time..)

learn control theory, (and basic physics) and let a controller that calculate the desired force to have the arm move to a position or speed or both be implemented.  (You should do this to control the HW anyway)

stop using forces, cheat and use the animation interface for gazebo

Originally posted by nkoenig with karma: 7661 on 2012-09-19 23:09:50.954117 UTC
This answer was ACCEPTED on the original site
Post score: 0

---JSON_START_ROS2022_SE_MIGRATION {'accepted': True, 'score': 0, 'name': 'nkoenig', 'user_id': 5, 'timestamp': '2012-09-19 23:09:50.954117 UTC'}---


问:
Link collision avoidance for redundant manipulator
I am trying to have my robot arm follow a trajectory while avoiding collisions between obstacles and the robot links. The way I go about it is to find the closest point on the robot arm, and treat the links leading up to this point as a separate robot. Then for this separate robot I find joint velocities that make the robot move straight away from the obstacle. Lastly I project these joint velocities onto the nullspace of the complete robot's Jacobian $J$.

This works well most of the times, but sometimes I experience that the jacobian for the separate robot $J_0$ becomes ill conditioned - and produces huge joint velocities. This usually happens when the closest point sits on one of the first links, then the separate robot is often close to a singularity.

I am trying to improve the condition number of $J_0$ by using the "damped jacobian" $$J_0 = (J_0 + \lambda I)$$
However unless the closest point on the robot sits on the sixth link $J_0$ will not be square and you cannot add the identity matrix in this case. I guess there are ways to damp these subjacobians, but I am unsure how.

In essence I am looking for a way to condition variable sized jacobians. Or some alternative which makes the robot play nice close to singularities.

An example of where it goes wrong, can be seen in the image below. The robot is following the blue dashed trajectory, while trying to avoid the brown rod. In this specific case the purple vector indicates the closest point on obstacle and link at one timestep. In the next timestep the closest link has changed (orange arrow). The jacobian for the manipulator leading up to the tip of the orange arrow has a condition number of 170 - compared to the condition number of the one leading up to the purple arrow tip, which is only 3.

For reference. This is the resolved rate motion scheme I am using:
\begin{align}
 &\dot q_0 = J_0^+(q) \dot x_0 \\
 &\dot q = J^+(q) \dot x + NN^+\dot q_0 \\
&q = q + \dot q \cdot dt
\end{align}

$\dot x_0$ is the desired velocity of the endpoint of the separate robot

$\dot x$ is the desired velocity of the end point for the full robot

$A^+$ is the pseudo inverse of A.

$N$ is the nullspace of $J$

答:
Using "damped least squares" is usually the way to make the jacobian behave better around singularities.  But keep in mind it is still a singularity, and things will always be weird there.

The formulation for damped least squares that I have seen is:

$\dot{q}=J^T(JJ^T+\lambda^2I)^{-1}\dot{x}$

Also, you might want to use a different motion planner.  TrajOpt is a good one that easily handles keeping the arm out of obstacles.


问:
Why do quadcopters use brushless motors?
I've been thinking about starting a quadcopter project, maybe building it from scratch. One of the main barriers-to-entry for me is the motors: it seems like most quadcopters use brushless motors. I have some experience with DC motors and using PWM signals to regulate speed, but no experience with brushless motors. As I understand it, brushless motors are more expensive than the typical DC motor I would use on a land robot, and they also require Electronic Speed Controllers (ESCs), which seem to make them (from my perspective) even more expensive and more complicated to use.

So, my question: what is it about brushless motors that make them useful in a quadcopter? Is it more torque, less weight, something to do with efficiency? And would it be significantly harder (or even possible) to achieve lift using DC motors instead?

答:
For the smallest (and cheapest) of multicopters, DC brushed motors are well in use: http://www.rcgroups.com/forums/showthread.php?t=2009496
They are coreless motors1 and have very little inertia, can accelerate very quickly and are perfectly suited for high frquency PWM (we drive them with up to 32kHz). You can achieve more than 3:1 power:weight ratio on a sub 50g AUW airframe. Driving those motors is a lot simpler than a brushless motor, you basically just need a transistor and a capacitor.

That applies to micro copters, brushless technology didn't catch up in that weight class (yet). Probably just a question of time. The first micro brushless systems are available, but they cost many times more than coreless motors and don't offer more performance (yet).

For larger quads: no question, brushless is king. But for the micro and nano-quads, its awesome to have these cheap motors to crash with confidence ;)

1 - http://www.pacificelectronicscorp.com/dccoreless.html


问:
PID control for robotic manipulator servoing
I'm trying to enable robotic manipulator servoing using MoveIt servo implementation.
However, I'm having some questions regarding reference following and error compensation.

The thing is, I roughly know pose of the object I need to manipulate, and as closer I get, my pose estimation is better, therefore I should converge to the wanted pose. Pipeline I'm having currently is:

Use RRT planning to plan and execute trajectory to pre-approach pose

Use robotic arm servoing to slowly move arm to pre-manipulation pose

Manipulate object (not important for know)

In my implementation, I'm using ROS ActionServers for different modes of control. The thing is 1. part of the pipeline works fine, and the pose estimation as soon as I'm close enough is really good. Servoing in MoveIt is realized by using 4 PIDs (3 for position/xyz, and 1 for orientation/quaternion). Error is generated as (e = p_wanted - p_current) and after going through PIDs we get linear and angular velocities of the end effector. Thing is, p_wanted is always the same, and p_current changes during servo execution. As far as I know, error should minimize, but in some cases, after certain time, servoing "diverges" and error begins increasing instead of decreasing.

I'm wondering how could I prevent that jump, and how can I tune PID parameters good enough to be able to minimize large errors (10 cm) as well as small errors (1 cm). Is it even possible? Would I get better preformance if I would concentrate on continuous reference change, compensation of 1cm of error at the time?

If more context is needed, or something isn't clear enough, let me know.

Thank you for your help :)

答:
How exactly are you calculating the error?  If you are combining position error and angular error, then that might make the error blow up.  (Especially if the orientation error is calculated incorrectly).  Calculating angular error can be tricky.

In the past when I've done tasks like this, I usually keep the end-effector's orientation constant and do translation only to slowly get the arm into the pre-manipulation pose.  See this question for how to do that: How to get pure end-effector translation through Jacobian?

But if you do need to follow a trajectory with orientation included, see this question: Jacobian-based trajectory following


问:
Solution to Inverse Kinematics of a 6 DOF Articulated robot with a wrist joint
I'm working on getting a solution to the inverse kinematics of a 6DOF Articulated robot with a wrist joint.

I derived the robot's forward kinematics firstly, using these angles
$$θ_1 = 60.102$$
$$θ_2 = 65.220$$
$$θ_3 = 40.390$$
$$θ_4 = 70.215$$
$$θ_5 = 40.002$$
$$θ_6 = 30.211$$

Using these joint parameters, I found a total transformation matrix

$a_1 = 0.18;~~a_2 = 0.6;~~a_3 = 0.12;~~a_4 = 0;~~a_5 = 0;~~a_6 = 0;$

$d_1 = 0.4;~~d_2 = 0;~~d_3 = 0;~~d_4 = 0.62;~~d_5 = 0;~~d_6 = 0.115;$

$α_1 = 90;~~α_2 = 0;~~α_3 = -90;~~α_4 = 90;~~α_5 = -90;~~α_6 = 0;$

This is the resulting total transformation matrix:

$$-0.920890~~~0.342692~~~~0.185808~~~~-0.077297$$
$$-0.010244~~~0.455209~~~~-0.890326~~~-0.273985$$
$$-0.389689~~~-0.821795~~~-0.415687~~~0.845690$$
$$0.000000~~~~0.000000~~~~0.000000~~~~1.000000$$

Now, I'm trying to find an analytical Inverse Kinematic solution that will output the original joint angles I used for the robot's forward kinematics using the total transformation matrix I found.

I used this formula to find the first three angles by decoupling the robot. The output of $\theta_1$ is correct but the output of $\theta_2$ & $\theta_3$ are wrong.

$$θ_1 = atan2(y_c,x_c) + π$$
$$θ_3 = atan2(s3,c3)$$
$$θ_2 = atan2((z_c-d_1),\sqrt(x_c^2+y_c^2)) - atan2(a_3s_3,a_2+a_3c_3)$$

KEY

$$x_c = P_x - (d_6*r_{13})$$
$$y_c = P_y - (d_6*r_{23})$$
$$z_c = P_z - (d_6*r_{33})$$
$$s_3 = sine theta 3$$
$$s_3 = sqrt(1-c_3^2)$$
$$c_3 = cosine theta 3$$
$$c_3 = \frac {(x_c^2 + y_c^2 + (z_c-d_1)^2 - a_2^2 - a_3^2)}{(2*a_2*a_3)}$$
$$a_2,a_3,d_1 == robot link parameters, defined earlier$$

Does someone know of another IK analytical method I can use that will get me my original joint angles?

答:
You don't say which DH parameter conventions you are using, so it will be hard for users to help you.  And there are actually three different DH parameter conventions.

But what might be going on is that they are both valid solutions, but with different configurations.  A 6 DoF arm can have many different joint configurations which lead to the same end-effector pose.  Here is a simple 2D example:


答:
Angular error is calculated using dot product of quaternion and quaternion inverse as shown here :)

Basically, I've been using MoveIt servo.

And I've managed to grasp it following this article.

First of all, kudos to all the MoveIt contributors because they made it easy as possible.

Second of all, I had multiple problems regarding servoing which are in (priority order):

badly tuned PID controllers for individual joints

trying to servo manipulator at the edge of reachable space

servoing 6DoFs with 6DoF manipulator (simply doesn't work well enough for larger Euclidean distances)

Some advices:

tune PID controllers of each joint, and do it well. Make sure your robot is controllable

Do not servo for large Euclidean distances

Servo less DoFs than your manipulator has

Rest of the discussion.

Thank you for your answer. Resources are super helpful :)

Cheers.

Title: PID control for robotic manipulator servoing

Body:
I'm trying to enable robotic manipulator servoing using MoveIt servo implementation.
However, I'm having some questions regarding reference following and error compensation.

The thing is, I roughly know pose of the object I need to manipulate, and as closer I get, my pose estimation is better, therefore I should converge to the wanted pose. Pipeline I'm having currently is:

Use RRT planning to plan and execute trajectory to pre-approach pose

Use robotic arm servoing to slowly move arm to pre-manipulation pose

Manipulate object (not important for know)

In my implementation, I'm using ROS ActionServers for different modes of control. The thing is 1. part of the pipeline works fine, and the pose estimation as soon as I'm close enough is really good. Servoing in MoveIt is realized by using 4 PIDs (3 for position/xyz, and 1 for orientation/quaternion). Error is generated as (e = p_wanted - p_current) and after going through PIDs we get linear and angular velocities of the end effector. Thing is, p_wanted is always the same, and p_current changes during servo execution. As far as I know, error should minimize, but in some cases, after certain time, servoing "diverges" and error begins increasing instead of decreasing.

I'm wondering how could I prevent that jump, and how can I tune PID parameters good enough to be able to minimize large errors (10 cm) as well as small errors (1 cm). Is it even possible? Would I get better preformance if I would concentrate on continuous reference change, compensation of 1cm of error at the time?

If more context is needed, or something isn't clear enough, let me know.

Thank you for your help :)


问:
Finding error in euler angle from quaternion
I'm currently working on creating a ROS controller package for franka arm, which I choose to implement a feedback linearization controller that works as following:

Assume we have a planned trajectory $x_d$, planned velocity $\dot x_d$, and planned acceleration $\ddot x_d$. All of which are 6x1 matrices with upper three rows corresponding to position, and bottom three rows corresponding to orientation of the end effector.

In order to find the desired joint torque $\tau$, three most important quantities that we need to solve for are desired joint acceleration $\ddot q_d$, joint position and velocity errors $\Delta q$ and $\Delta\dot q$  respectively.

Whereas we can solve for them by the following steps:
$$\begin{align}
    \Delta q = J^+(q)(x_d-x)
\end{align}$$
$$\begin{align}
    \Delta\dot q = J^+(q)(\dot x_d-\dot x)
\end{align}$$
$$\begin{align}
    \ddot q_d = J^+(q)(\ddot x_d - \dot J(q)\dot q)
\end{align}$$
whereas for franka arm, the Jacobian matrix $J$ and its time derivative $\dot J$ are 6x7 matrices, and Jacobian psedoinverse $J^+$ is a 7x6 matrix.

To calculate joint torque, we have:
$$\begin{align}
    \tau = M(\ddot q_d + K_p\Delta q+K_d\Delta \dot q) + \tau_G
\end{align}$$

In terms of implementation, I had trouble calculating the bottow three rows of $x_d,\dot x_d,$ and $\ddot x_d$, which are the Euler position,velocity, and acceleration of the orientation of end effector respectively. Since the hardware interface of the robot only has an attribute to end effector orientation in quaternion, and the trajectory is in terms of Euler angle, I found this approach online:

```<code>if (Quaternion.coeffs().dot(orientation_curr_.coeffs()) < 0.0) {
            orientation_curr_.coeffs() << -orientation_curr_.coeffs();
        }
    Eigen::Quaterniond error_quaternion(orientation_curr_.inverse() * Quaternion);
    error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
    error.tail(3) << curr_transform.linear() * error.tail(3);
</code>```

whereas Quaternion is the desired orientation transforming from Euler angle by:

```<code>Quaternion = Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())* Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(ya, Eigen::Vector3d::UnitZ());
</code>```

My question is can somebody please explain to me the mathematical intuition behind the code finding the error? Thank you so much in advance!

答:
In general, Euler angles are one of the worst representations for orientation when you need to do math.  Quaternions and axis-angle representation are closely related I believe.  This is a good site for understanding quaternions: https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm  And it also has pages on conversions: https://www.euclideanspace.com/maths/geometry/rotations/conversions/index.htm

But the answers in this thread might be of use to you too: Jacobian-based trajectory following

And if you want a really deep dive into quaternions, this is a good reference: http://www.neil.dantam.name/note/dantam-quaternion.pdf


问:
Roomba S9 - no data over serial connection
Unlike other Roomba models, for the s9 there is little to no information available for setting up and using a serial connection to a Windows PC.

I've managed to get to the point where Putty serial can connect to the device on COM5 at 115200 baud. But with the Roomba appearing to be awake, no text comes across at all on the session, even when the device is put into diagnostics mode or rebooted.

Could this be a driver issue? Or, and indication that the motherboard is toast?

Tks

答:
Unfortunately, the Roomba S9+ doesn't support the Open Interface spec.  So you can't control the robot through the USB port. And neither do any of the other latest generation of Roombas (i.e. the "i", and "j" series)

Only certain models of the 500, 600, 700, 800, and maybe "e" series robots have the serial port hardware and code support to make it "hackable". (And the existence of the port doesn't mean that the code supports it).  This feature was phased out a while ago.  So the older the robot, the more likely it will be to support the OI spec.

The proper way to control a Roomba is to use a "Create".  i.e. either a Create (antique), Create2 (recently retired), or Create3.

Disclaimer: I work at iRobot where I am developing the next generation of consumer robots. However, my postings on this site are my own and don't necessarily represent iRobot's positions, strategies, or opinions.


问:
Roomba s9 -- no communication
I connected my Roomba s9+ USB port to my Windows 10 PC, which cannot detect it (nothing under Ports):

I messed around a bit and updated drivers, and now Windows 10 detects Roomba as:

I don't know what to do with this. Putty serial connection to this port receives nothing.

What should I do to troubleshoot the issue?

Title: Roomba s9 -- no communication

Body:
Unfortunately, the Roomba S9+ doesn't support the Open Interface spec.  So you can't control the robot through the USB port. And neither do any of the other latest generation of Roombas (i.e. the "i", and "j" series)

Only certain models of the 500, 600, 700, 800, and maybe "e" series robots have the serial port hardware and code support to make it "hackable". (And the existence of the port doesn't mean that the code supports it).  This feature was phased out a while ago.  So the older the robot, the more likely it will be to support the OI spec.

The proper way to control a Roomba is to use a "Create".  i.e. either a Create (antique), Create2 (recently retired), or Create3.

Disclaimer: I work at iRobot where I am developing the next generation of consumer robots. However, my postings on this site are my own and don't necessarily represent iRobot's positions, strategies, or opinions.


问:
Simulate GPS IMU With Quadcopter Swarm?
I have multiple drones work in swarm formation, i made the quadcopter model and the swarm one. Until now i have the swarm moving in a formation leader-follower and track a predefined trajectory based on PID Controller. What i need help is how to add GPS, IMU sensors model to my model?</p>

答:
It depends on what you are trying to achieve.

If you want to see how your path following / formation tracking etc. works under non-ideal conditions, then you can simply give it a position that is slightly wrong.  Perhaps some kind of Gaussian around the actual location.  It is kind of pointless to be in a simulation environment with perfect localization, then simulate multiple noisy sensors, then use a complicated algorithm to fuse the sensors to try to get back out good localization.  Why not just simulate the output of all that mess?  i.e. "good" not "perfect" localization.

To reiterate, it is totally reasonable to not want to use perfect localization in your simulator.  But there are easier ways to go about it.

If you want to make your simulation more closely match your real hardware, I would highly recommend looking at the noise, error, and other characteristics of your actual sensors, then adding those to the simulator.  Every sensor, platform, and application is slightly different, so it will be difficult for anyone else to provide this data for you.

But in general, from what I've seen, GPS usually has some constant offset from the actual position, then in a little while, it will jump to some other offset.  Acclerometers usually have a scale error, or axis misalignment.  And gyros have drift.

If you want to test out your kalman filter or whatever sensor fusion algorithm you have, then that is probably better served in a different question.

Title: Simulate GPS IMU With Quadcopter Swarm?


问:
Rotate while strafing with mecanum wheels
I have a robot with a drivetrain that is comprised of 4 mecanum wheels. These wheels are able to move vertically and horizontally, as well as rotate. The robot is square (18in x 18in) with the front 2 wheels in line, back 2 wheels in line, left 2 wheels in line, and right 2 wheels in line.

My question is would it be possible to strafe (move horizontally) while rotating? Is this feasible with mecanum wheels? If so, what would be the required relative speeds of each wheel?

Any help is appreciated, thanks!

答:
Yes.  A robot with 4 fixed mecanum wheels is able to translate and rotate simultaneously.  There are many links online which provide tutorials on how to do this:

https://www.instructables.com/Mecanum-wheel-robot-bluetooth-controlled/

https://www.roboteq.com/applications/all-blogs/5-driving-mecanum-wheels-omnidirectional-robots

https://howtomechatronics.com/projects/arduino-mecanum-wheels-robot/

The specific wheel velocities will change depending on your wheel configuration.  But here is one example:

And here is a more general vector-way of thinking about the wheel velocities.

If you want to translate and rotate at the same time, you can simply add the two control inputs.  For example, if you want to translate to the right, while also rotating counter-clockwise, you would do this:

```<code>// Move Right Translation
front_left_translation  = +trans_speed
front_right_translation = -trans_speed
back_left_translation   = -trans_speed
back_right_translation  = +trans_speed

// CCW Rotation
front_left_rotation  = -rot_speed
front_right_rotation = +rot_speed
back_left_rotation   = -rot_speed
back_right_rotation  = +rot_speed

// Output
front_left_control  = front_left_translation  + front_left_rotation
front_right_control = front_right_translation + front_right_rotation
back_left_control   = back_left_translation   + back_left_rotation
back_right_control  = back_right_translation  + back_right_rotation
</code>```

Important These directions are robot relative.

For example, if you executed each of these commands (translation and rotation) in isolation for 1 second each, and it moved the robot 1 meter to the right, then rotated the robot 180 degrees.  When you combined them, the robot would actually move in a circular arc.

The velocities would actually be valid for the initial time zero.  But in the next time step, the robot is now turned slightly, so the translation vector will have to be different.

Body:
Yes.  A robot with 4 fixed mecanum wheels is able to translate and rotate simultaneously.  There are many links online which provide tutorials on how to do this:

https://www.instructables.com/Mecanum-wheel-robot-bluetooth-controlled/

https://www.roboteq.com/applications/all-blogs/5-driving-mecanum-wheels-omnidirectional-robots

https://howtomechatronics.com/projects/arduino-mecanum-wheels-robot/

The specific wheel velocities will change depending on your wheel configuration.  But here is one example:

And here is a more general vector-way of thinking about the wheel velocities.

If you want to translate and rotate at the same time, you can simply add the two control inputs.  For example, if you want to translate to the right, while also rotating counter-clockwise, you would do this:

// Move Right Translation
front_left_translation  = +trans_speed
front_right_translation = -trans_speed
back_left_translation   = -trans_speed
back_right_translation  = +trans_speed

// CCW Rotation
front_left_rotation  = -rot_speed
front_right_rotation = +rot_speed
back_left_rotation   = -rot_speed
back_right_rotation  = +rot_speed

// Output
front_left_control  = front_left_translation  + front_left_rotation
front_right_control = front_right_translation + front_right_rotation
back_left_control   = back_left_translation   + back_left_rotation
back_right_control  = back_right_translation  + back_right_rotation

Important These directions are robot relative.

For example, if you executed each of these commands (translation and rotation) in isolation for 1 second each, and it moved the robot 1 meter to the right, then rotated the robot 180 degrees.  When you combined them, the robot would actually move in a circular arc.

The velocities would actually be valid for the initial time zero.  But in the next time step, the robot is now turned slightly, so the translation vector will have to be different.


问:
What can be the rating and specifications of dc motor used for making a quadcopter?
I want to make a quadcopter for my final year project and I am willing to use DC motors as the four rotors of the quadcopter. Can any one guide me about the ratings for proper motor selection for my job.</p>

答: If you want to build a really cheap but great flying quadcopter, check out coreless micro motors: http://www.rcgroups.com/forums/showthread.php?t=2009496</p>


问:
gazebo with player
I guess gazebo is more often being used with ROS and as standalone, has anyone tried to use gazebo ( version 1.0 and above) with player ? I am cool using gazebo with ROS (Electric) and also as standalone but have never done well with the installation with player. Any first hand experience may be useful. Which version of player to use with gazebo (version 1.0 and above) ? Also, is it advisable to use gazebo with player as player has not been a very active project for nearly the last 2 years (last version release (3.0.2) was in July 2010) ?

I had asked a similar question at gazebo-list@kforge.ros.org, however I hope to reach more people at answers.gazebosim.org.

Original comments:

Comment by Arkapravo on 2012-09-22 00:41:52.641179-05:00:

@nkoenig Yo ! I felt the same ! I will be using ROS - ROS is a big improvement on player.

End of original comments

Originally posted by Arkapravo on Gazebo Answers with karma: 1 on 2012-09-20 06:11:04.609245 UTC

Post score: 0

---JSON_START_ROS2022_SE_MIGRATION {'name': 'Arkapravo', 'score': 0, 'timestamp': '2012-09-20 06:11:04.609245 UTC', 'user_id': 11}---

答:
There is limited support for Player within Gazebo. I believe it consists of position, camera, and laser plugins.

I haven't used Player in many years. But I think the latest version should work with Gazebo.

In short, I would recommend using ROS.

Originally posted by nkoenig with karma: 7661 on 2012-09-21 19:33:46.650602 UTC

This answer was ACCEPTED on the original site

Post score: 0

---JSON_START_ROS2022_SE_MIGRATION {'accepted': True, 'name': 'nkoenig', 'score': 0, 'timestamp': '2012-09-21 19:33:46.650602 UTC', 'user_id': 5}---

Title: gazebo with player


问:
How do I make the coffee cup appear?

Here's a tutorial I went through today:

http://www.ros.org/wiki/simulator_gazebo/Tutorials/SpawningObjectInSimulation

Everything works except the last bit where you add a coffee cup to the scene. The cup shows up in the scene graph, but it's invisible in the 3D view.

Original comments

Comment by nkoenig on 2012-10-02:

Check you ogre log file: ~/.gazebo/ogre.log. Look for error/Error.

Originally posted by Issac on Gazebo Answers with karma: 11 on 2012-09-25

Post score: 1

---JSON_START_ROS2022_SE_MIGRATION {'name': 'Issac', 'score': 1, 'timestamp': '2012-09-25 19:54:33.067230 UTC', 'user_id': 29}---

答:
It appears the coffee cup model uses the old gazebo (pre 1.0) model format and it appears the parser used in fuerte isn't able to understand the syntax of that anymore. The error messages by gazebo on spawning the model at least suggest the mesh definition cannot be read:

```<code>namespace error : Namespace prefix model on physical is not defined
<!-- The trimesh coffee cup --><model:physical name="coffee_cup"><body:trimesh n
                                                                ^
namespace error : Namespace prefix body on trimesh is not defined
h coffee cup --><model:physical name="coffee_cup"><body:trimesh name="cup1_body"
                                                                               ^
namespace error : Namespace prefix geom on trimesh is not defined
izz>0.01</izz><cx>0.0</cx><cy>0.1</cy><cz>0.0</cz><geom:trimesh name="cup1_geom"
                                                                               ^
namespace error : Namespace prefix model on physical is not defined
<!-- The trimesh coffee cup --><model:physical name="coffee_cup"><body:trimesh n
                                                                ^
namespace error : Namespace prefix body on trimesh is not defined
h coffee cup --><model:physical name="coffee_cup"><body:trimesh name="cup1_body"
</code>```

A conversion to SDF would be needed to properly be able to spawn the model.

The differences are readily visible when comparing two simple models:

Coffee cup (not working in fuerte):
Coffee cup

Coke can (working):
Coke Can

There's quite a bit of non-working legacy launch files/models in the gazebo packages and outdated tutorials on ros.org and the described issues are an example of that. Any volunteers willing to improve the situation would be welcome I guess. For the moment, I added a disclaimer to the tutorial notifying readers that spawning the cup does not work in ROS versions more recent than electric.

Original comments

Comment by nkoenig on 2013-01-11:

ROS is not up to date with Gazebo. The tutorials on the ROS website should not be followed, nor should the Gazebo packages in ROS.

Originally posted by Stefan Kohlbrecher with karma: 473 on 2012-10-14

This answer was ACCEPTED on the original site
Post score: 2

---JSON_START_ROS2022_SE_MIGRATION {'accepted': True, 'name': 'Stefan Kohlbrecher', 'score': 2, 'timestamp': '2012-10-14 05:18:50.010346 UTC', 'user_id': 9}---

Title: How do I make the coffee cup appear?

Body:
It appears the coffee cup model uses the old gazebo (pre 1.0) model format and it appears the parser used in fuerte isn't able to understand the syntax of that anymore. The error messages by gazebo on spawning the model at least suggest the mesh definition cannot be read:

namespace error : Namespace prefix model on physical is not defined
<!-- The trimesh coffee cup --><model:physical name="coffee_cup"><body:trimesh n
                                                                ^
namespace error : Namespace prefix body on trimesh is not defined
h coffee cup --><model:physical name="coffee_cup"><body:trimesh name="cup1_body"
                                                                               ^
namespace error : Namespace prefix geom on trimesh is not defined
izz>0.01</izz><cx>0.0</cx><cy>0.1</cy><cz>0.0</cz><geom:trimesh name="cup1_geom"
                                                                               ^
namespace error : Namespace prefix model on physical is not defined
<!-- The trimesh coffee cup --><model:physical name="coffee_cup"><body:trimesh n
                                                                ^
namespace error : Namespace prefix body on trimesh is not defined
h coffee cup --><model:physical name="coffee_cup"><body:trimesh name="cup1_body"

A conversion to SDF would be needed to properly be able to spawn the model.

The differences are readily visible when comparing two simple models:

Coffee cup (not working in fuerte):
Coffee cup

Coke can (working):
Coke Can

There's quite a bit of non-working legacy launch files/models in the gazebo packages and outdated tutorials on ros.org and the described issues are an example of that. Any volunteers willing to improve the situation would be welcome I guess. For the moment, I added a disclaimer to the tutorial notifying readers that spawning the cup does not work in ROS versions more recent than electric.

Original comments

Comment by nkoenig on 2013-01-11:

ROS is not up to date with Gazebo. The tutorials on the ROS website should not be followed, nor should the Gazebo packages in ROS.

Originally posted by Stefan Kohlbrecher with karma: 473 on 2012-10-14

This answer was ACCEPTED on the original site
Post score: 2

---JSON_START_ROS2022_SE_MIGRATION {'accepted': True, 'name': 'Stefan Kohlbrecher', 'score': 2, 'timestamp': '2012-10-14 05:18:50.010346 UTC', 'user_id': 9}---


问:
Turning a mecanum wheeled robot at a specific angle i.e 45 degree
I am implementing inverse kinematics for motion control of a robot with four mecanum wheels. However I am not getting how to turn it at specific heading angle(45 degree)? I am using encoders to check if robot has really made a 45 degree turn. the problem is I am using following equation
= (−1 + 2 − 3 + 4)r/(4(+)) to compute turning angle from four encoders. but its not giving output as expected.

```<code>    volatile long eF1V = 0;
volatile long eF2V= 0;
volatile long eB1V= 0;
volatile long eB2V= 0;

int rpm_set=50;
float angle=0.0;
float radB1=0.0; float radB2=0.0;float radF2=0.0;float radF1=0.0;
float linear_velocityB1=0.0; float linear_velocityB2=0.0; float linear_velocityF1=0.0; float linear_velocityF2=0.0;
int i=70;int rpmB2=0; int rpmB1=0; int rpmF1=0;int rpmF2=0;
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
attachInterrupt(digitalPinToInterrupt(ENC_IN_FL), updateEF1, RISING);
 attachInterrupt(digitalPinToInterrupt(ENC_IN_BL), updateEB1, RISING);
 attachInterrupt(digitalPinToInterrupt(ENC_IN_FR), updateEF2, RISING);
 attachInterrupt(digitalPinToInterrupt(ENC_IN_BR), updateEB2, RISING);

eF1V = 0;
 eF2V= 0;
 eB1V= 0;
 eB2V= 0;

}
void loop() {
   // put your main code here, to run repeatedly:

 while(angle<=0.78)
 {
 turn(); //left motors are tuning in one direction and right in opposite, both at PWM=105
 currentMillis=millis();
   if(currentMillis-previousMillis>interval)
   {
     previousMillis=currentMillis;
    rpmB2=(float)(encoderBRValue*60/ENC_COUNT_REV);   //rpm of rear wheel at right side
    radB2=rpmB2*0.10472; linear_velocityB2=radB2*0.1651;
    rpmB1=(float)(encoderBLValue*60/ENC_COUNT_REV);
   radB1=rpmB1*0.10472; linear_velocityB1=radB1*0.1524;   //m/s
    rpmF2=(float)(encoderFRValue*60/ENC_COUNT_REV);
    radF2=rpmF2*0.10472; linear_velocityF2=radF2*0.1651;
    rpmF1=(float)(eF1V*60/ENC_COUNT_REV);
    radF1=rpmF1*0.10472; linear_velocityF1=radF1*0.1651;
    Serial.print("motorPwmFL=");Serial.print(i);Serial.print('\t');Serial.print("rpmF1=");Serial.println(rpmF1);
 angle=angle+(linear_velocityF2-linear_velocityF1+linear_velocityB2-linear_velocityB1)/(4*(lx+ly));
 Serial.print("angle=");Serial.println(angle);

 eF1V = 0;
  eF2V= 0;
  eB1V= 0;
  eB2V= 0;
   }
}
stop();
}
</code>```

in turn function, left motors are rotated anticlockwise and right motors are rotated clockwise, all motors have same speed given by PWM signal.
getanglefromencoders compute RPM of each wheel using encoder counts and then the above equation is used to calculate angle. but with this logic, my robot does not turn at 45 degree but it keeps rotating.
Can someone please check this and guide me what I am doing wrong?
Thanks

答:
I'm not sure if this is the problem.  (It would be really helpful to see the console output.)  But in general, it is usually better to cast to a float before dividing.

For example, this is potentially problematic:

```<code>rpmB2=(float)(encoderBRValue*60/ENC_COUNT_REV);
</code>```

If encoderBRValue is small, and ENC_COUNT_REV is large then the division might not be greater than 1.  Integer division takes the floor of the result.  i.e. it throws away the remainder.  So it would be better to do this:

```<code>rpmB2=(float)encoderBRValue * 60.0f / ENC_COUNT_REV;
</code>```

And actually, you don't even need the cast if you have 60.0f in there since that will cause the whole expression to be done in float math.

Body:
I'm not sure if this is the problem.  (It would be really helpful to see the console output.)  But in general, it is usually better to cast to a float before dividing.

For example, this is potentially problematic:

rpmB2=(float)(encoderBRValue*60/ENC_COUNT_REV);

If encoderBRValue is small, and ENC_COUNT_REV is large then the division might not be greater than 1.  Integer division takes the floor of the result.  i.e. it throws away the remainder.  So it would be better to do this:

rpmB2=(float)encoderBRValue * 60.0f / ENC_COUNT_REV;

And actually, you don't even need the cast if you have 60.0f in there since that will cause the whole expression to be done in float math.


问:
Differential Drive using shopping cart-style casters
I thought I would be clever and use TWO shopping-cart-style casters (more stable) instead of the ONE usual omnidirectional ball caster. Now that I am running it I am almost positive that the kinematics are totally different and in fact very hard to model.

Can someone confirm or deny this belief, and give any context (links etc.) so I can dig deeper?

答:
Two things:

There is a reason most robots only have a single caster: three points define a plane.  If your robot has 4 wheels, and no suspension, due to manufacturing tolerances it is extremely unlikely that you will be able to get all four wheels to be touching the ground at the same time.  Most likely, one of the wheels will be slightly off the ground.  And hopefully, it isn't one of your drive wheels!  (Maybe this is why there is always one shaky wheel of shopping carts?)  And the problem actually gets worse in uneven terrain.  Imagine if the robot went over a bump such that it lifted one of the drive wheels off the ground.  That is obviously not good.  With three wheels, this won't happen.

The kinematics of how this un-powered caster wheel affects the robot motion is usually not modeled.  Typically, the robot or caster is light enough that it is not severely affected.  And differences between predicted motion and real-life motion are why we use sensors anyway.


问:
IMU Senosr fusion algorithm of gyro and accelerometer during acceleration of vehicle
Hello so we have a car with 10 IMUs mounted at different locations of the car and the idea is the measure the centripetal force at each location of the car and compare it to each other. We also need the gyro data as well. So we need the sensor data to have less errors and drift so that we can compare them easily. As you all know, the gyros drift over time which makes calibration a little bit tough. I decided to apply different sensor fusion algorithm like the complementary filter, but I later realised that this only work when the car is not accelerating. Once the the is accelerating then the complementary filter does not output the right values which makes sense.

Now my question is, is there  any sensor fusion algorithm which works whiles the car is accelerating at a top speed. My sensor has only 6 DoF, ie. gyroscope and accelerometer.

I will be happy if anyone could let me know of any algorithm which works for accelerating objects. Also, I have the possibility to combine the sensor data with GPS signal from the car but I don't know how it is done.

I will be happy with multiple ideas, algorithms, books, papers and journal.

答:
This doesn't directly answer your question because it is not about sensor fusion algorithms, but:

In general, as you said: "gyros drift over time".  The way to compensate for this is:

Properly calibrate your sensor.  This involves making sure the sensor produces the correct output over a range of rotation rates and over a range of temperatures.  The drift should be a random walk.  So will be zero mean unless there is a scale error as the chip warms up.

Periodically zero the gyro.  When you can be certain that your vehicle is not moving, zero out the gyro.  i.e. if the gyro is telling you that the vehicle is rotating when you know it is not, then there is some bias that you should subtract out.

Accelerometers don't drift like gyros, but they do need calibration too.  They should be calibrated for scale and offset in each axis.


问:
Diff-drive robot - Obstacle avoiding - random walk
I have implemented a logic-based algorithm for obstacle avoidance in a differential drive robot with 5 IR sensors.
I want the robot in space, for example, a square room, to move freely and avoid various obstacles and walls.

Below are samples of the code:

```<code># False means no obstacles and True means obstacles in front.
    def left_callback(self, msg):
        self.L_Range = msg.range
        self.action_array[0] = True if self.L_Range <= obstacle_avoider.ACTION_RANGE else False
        self.avoid_obstacle()

    def frontleft_callback(self, msg):
        self.FL_Range = msg.range
        self.action_array[1] = True if self.FL_Range <= obstacle_avoider.ACTION_RANGE else False
        self.avoid_obstacle()

    def front_callback(self, msg):
        self.F_Range = msg.range
        self.action_array[2] = True if self.F_Range <= obstacle_avoider.ACTION_RANGE else False
        self.avoid_obstacle()

    def frontright_callback(self, msg):
        self.FR_Range = msg.range
        self.action_array[3] = True if self.FR_Range <= obstacle_avoider.ACTION_RANGE else False
        self.avoid_obstacle()

    def right_callback(self, msg):
        self.R_Range = msg.range
        self.action_array[4] = True if self.R_Range <= obstacle_avoider.ACTION_RANGE else False
        self.avoid_obstacle()

def avoid_obstacle(self):
        # We have a primary and secondary array, the primary has the three front sensors, and the secondary has the rest two, right and left.
        primary_array = self.action_array[1:4]
        secondary_array = [self.action_array[0], self.action_array[4]]

        if primary_array == [False, False, False]:
            # Just Move Forward
            self.velocity.linear.x = obstacle_avoider.MAX_SPEED
            self.velocity.angular.z = 0

        elif primary_array == [True, False, False]:
            # Move Forward with Right Turn
            self.velocity.linear.x = obstacle_avoider.MAX_SPEED
            self.velocity.angular.z = obstacle_avoider.TURN_SPEED

        elif primary_array == [False, False, True]:
            # Move Forward with Left Turn
            self.velocity.linear.x = obstacle_avoider.MAX_SPEED
            self.velocity.angular.z = -1 * obstacle_avoider.TURN_SPEED

        else:
            # Stop Moving Forward
            self.velocity.linear.x = obstacle_avoider.MIN_SPEED
            if primary_array == [False, True, False]:
                # Compare FL_Range and FR_Range
                if self.FL_Range > self.FR_Range:
                    # Turn Left
                    self.velocity.angular.z = -1 * obstacle_avoider.TURN_SPEED

                else:
                    # Turn Right
                    self.velocity.angular.z = obstacle_avoider.TURN_SPEED

            elif primary_array == [True, True, False]:
                # Turn Right
                self.velocity.angular.z = obstacle_avoider.TURN_SPEED

            elif primary_array == [False, True, True]:
                # Turn Left
                self.velocity.angular.z = -1 * obstacle_avoider.TURN_SPEED

            else:
                # Check Secondary
                if secondary_array == [False, True]:
                    # Turn Left
                    self.velocity.angular.z = -1 * obstacle_avoider.TURN_SPEED

                elif secondary_array == [True, False]:
                    # Turn Right
                    self.velocity.angular.z = obstacle_avoider.TURN_SPEED

                else:
                    # Compare L_Range and R_Range
                    if self.L_Range > self.R_Range:
                        # Turn Left
                        self.velocity.angular.z = -1 * obstacle_avoider.TURN_SPEED

                    else:
                        # Turn Right
                        self.velocity.angular.z = obstacle_avoider.TURN_SPEED
</code>```

I want to ask what a random walk is and how it can be used in this particular robot.

Thanks in advance!

答:
A random walk isn't an algorithm.  It is a description of a random process.

If you want to inject some randomness into your robot code, here are a few ideas:

Drive straight, but when you hit an obstacle, turn in a random direction.  This is kind of how old Roomba code worked.  Given enough time, the robot will cover the entire area.  (Maybe there is a proof of this somewhere in the random walk literature, but I'm not going to dig it up).  Here is a time-lapse photo of a Roomba with a color changing LED on it:

Drive randomly.  There are a few different ways to do this.

The naive implementation is to just put some random velocities on the wheels.  This kind of works, but will probably produce jumpy or shakey behavior.

```<code>def get_vels():
    left_vel = RANDOM_SCALE * (np.random.random() - 0.5) + SPEED
    right_vel = RANDOM_SCALE * (np.random.random() - 0.5) + SPEED
    return (left_vel, right_vel)
</code>```

A slightly better way is to get a random speed delta:

```<code>left_vel = DEFAULT_VEL
right_vel = DEFAULT_VEL
def get_vels():
    global left_vel
    global right_vel

    left_vel += RANDOM_SCALE * (np.random.random() - 0.5)
    right_vel += RANDOM_SCALE * (np.random.random() - 0.5)

    # cap wheel velocities
    if left_vel < MIN_VEL:
        left_vel = MIN_VEL
    if right_vel < MIN_VEL:
        right_vel = MIN_VEL
    if left_vel > MAX_VEL:
        left_vel = MAX_VEL
    if right_vel > MAX_VEL:
        right_vel = MAX_VEL

    return (left_vel, right_vel)
</code>```

This is still not ideal though because the robot may slow down and speed up.  (For example if both wheels have minimum or maximum velocities at the same time).

This is a simulation I ran twice with the same parameters, for the same number of iterations.  You can see that in the first the robot drove very far and straight, and in the second the robot covered very little distance.

Lastly, the method I prefer is to get a random vector to drive in, then convert that to left / right wheel commands.

```<code>theta = np.pi/2.0
def get_vels():
    global theta

    theta += RANDOM_SCALE * (np.random.random() - 0.5)

    # cap turn radius
    if theta > np.pi * 3.0 / 4.0:
        theta = np.pi * 3.0 / 4.0
    if theta < np.pi / 4.0:
        theta = np.pi / 4.0

    left_vel = SPEED_SCALE * math.sin(theta) + math.cos(theta) / TURN_SCALE
    right_vel = SPEED_SCALE * math.sin(theta) - math.cos(theta) / TURN_SCALE

    return (left_vel, right_vel)
</code>```

Body:
A random walk isn't an algorithm.  It is a description of a random process.

If you want to inject some randomness into your robot code, here are a few ideas:

Drive straight, but when you hit an obstacle, turn in a random direction.  This is kind of how old Roomba code worked.  Given enough time, the robot will cover the entire area.  (Maybe there is a proof of this somewhere in the random walk literature, but I'm not going to dig it up).  Here is a time-lapse photo of a Roomba with a color changing LED on it:

Drive randomly.  There are a few different ways to do this.

The naive implementation is to just put some random velocities on the wheels.  This kind of works, but will probably produce jumpy or shakey behavior.

def get_vels():
    left_vel = RANDOM_SCALE * (np.random.random() - 0.5) + SPEED
    right_vel = RANDOM_SCALE * (np.random.random() - 0.5) + SPEED
    return (left_vel, right_vel)

A slightly better way is to get a random speed delta:

left_vel = DEFAULT_VEL
right_vel = DEFAULT_VEL
def get_vels():
    global left_vel
    global right_vel

    left_vel += RANDOM_SCALE * (np.random.random() - 0.5)
    right_vel += RANDOM_SCALE * (np.random.random() - 0.5)

    # cap wheel velocities
    if left_vel < MIN_VEL:
        left_vel = MIN_VEL
    if right_vel < MIN_VEL:
        right_vel = MIN_VEL
    if left_vel > MAX_VEL:
        left_vel = MAX_VEL
    if right_vel > MAX_VEL:
        right_vel = MAX_VEL

    return (left_vel, right_vel)

This is still not ideal though because the robot may slow down and speed up.  (For example if both wheels have minimum or maximum velocities at the same time).

This is a simulation I ran twice with the same parameters, for the same number of iterations.  You can see that in the first the robot drove very far and straight, and in the second the robot covered very little distance.

Lastly, the method I prefer is to get a random vector to drive in, then convert that to left / right wheel commands.

theta = np.pi/2.0
def get_vels():
    global theta

    theta += RANDOM_SCALE * (np.random.random() - 0.5)

    # cap turn radius
    if theta > np.pi * 3.0 / 4.0:
        theta = np.pi * 3.0 / 4.0
    if theta < np.pi / 4.0:
        theta = np.pi / 4.0

    left_vel = SPEED_SCALE * math.sin(theta) + math.cos(theta) / TURN_SCALE
    right_vel = SPEED_SCALE * math.sin(theta) - math.cos(theta) / TURN_SCALE

    return (left_vel, right_vel)


问:
How to get from lidar coordinates to gps coordinates
I build a map from lidar slam lego-loam, which is a lidar odometer, and then I want to use this map by gps localization.

I know the map will have errors in some places, so the question is how do I get the ground truth of the lidar slam?
If the map and track are required to be overlapped, do I need to add GPS to lidar slam?

答:
This seems to be a georeferencing problem.  Where you have some map (your Lidar map), and want to register it (or align it) with another map (in this case GPS).

I would say, yes.  The best solution would just to be gather GPS data while you gather the Lidar map.

Without that, you probably have to do the alignment by hand.  You might be able to find some GIS data for your area of interest.


问:
How do I convert a set of coordinate and rotation values
I'm an electrical engineer who needs to use a robot for their first time. I have a UR5 that I'm controlling through the rtde interface via Python. It seems that what I'm trying to do must be something a million other people have done already but I've spent the morning reading and I'm very confused.

My robot has a "base" plane but it has a method to create a plane aligned with my workpiece that I've used to create a coordinate system from which it's easy to control the robot by writing values in the format [X, Y, Z, RX, RY, RZ]. The problem is that I can not read values from the robot in my custom plane, only in the "base" plane. It seems like it must be a common action to read the coordinates, rotate them by [RX, RY, RZ], then translate them by [X, Y, Z]. The help I've found on the web either does one or the other and often includes a lot of mathematical derivation. When I've tried to apply the solutions I don't get vectors of the same length after I've translated them so it feels like I must have done something wrong.

Is there any resource that can explain how to solve my problem simply? It feels like this must be a very common thing for people who use robots to do.

答:
What you are looking for here is homogeneous transformation matrices. The purpose of these matrices is to seamlessly integrate rotation and translations into one matrix that acts as a change of coordinates. Consider some frame $0$ that has an origin (a zero) and three mutually perpendicular axes and some other frame $n$ with the same properties but not necessarily the same values. A homogeneous transformation matrix then is a matrix of the form:

$${}_{0}\mathbf{T}_{n} =   \left[ {\begin{array}{cc}
    {}_{0}\mathbf{R}_{n} & {}_{0}\mathbf{x}_{n} \\
    \mathbf{0} & 1 \\
  \end{array} } \right] \in \mathbb{R}^{4 \times 4} $$

Where ${}_{0}\mathbf{R}_{n} \in \text{SO}(3)$ is a $3\times3$ rotation matrix that rotates the coordinates of frame $n$ into frame $0$ and ${}_{0}\mathbf{x}_{n} \in \mathbb{R}^{3}$ is the distance from the origin of frame $0$ to the origin of frame $n$. So, any vector $\mathbf{v}_{n}$ in frame $n$ can easily be expressed in frame $0$ as:

$$ \mathbf{v}_{0} = {}_{0}\mathbf{T}_{n}\mathbf{v}_{n} $$

Of course, you might notice that ${}_{0}\mathbf{T}_{n}$ has an incompatible size with respect to $\mathbf{v} \in \mathbb{R}^{3}$ - to solve this we simply append a $1$ to the end of $\mathbf{v}$, giving us $\mathbf{v} = [v_1\ v_2\ v_3\ 1]^{T}$.

Now that we have our formulation of frames in place, let's consider how we might transfer our control from frame $0$ (our base frame) to frame $c$, our "custom" frame. We know we can transform a vector from one frame to another via homogeneous transformation matrices, so, in a rush to the punchline - we'll say our final goal is some coordinate transformation ${}_{c}\mathbf{T}_{n}$, where $n$ denotes the end-effector frame. Currently, we have the ability to "move" frame $n$ into frame $0$ - so we need to find a way to insert our $c$ frame into our model. Let's try establishing some connection between frames that don't change relative to any of our model parameters (i.e. robot joint values). The obvious choice here is frame $0$. Let's establish some coordinate transform ${}_{c}\mathbf{T}_{0}$ - these should be the only measurements you will need to take.

Moving the frame $0$ origin to frame $c$ is as simple as measuring the distance from frame $0$ (your robot's base I assume) to the workpiece center (or whatever you are looking to define the workpiece origin by). The rotation part is a little more complicated. Perhaps first you could determine the $\text{x}$, $\text{y}$, and $\text{z}$ axes directions of your base frame. Then, determine what you would like your workpiece coordinate axes to look like (just make sure they are orthogonal to one another and satisfy the right-hand rule). From here, we want to make a rotation matrix ${}_{c}\mathbf{R}_{0}$ to map frame $0$ orientations to frame $c$ orientations. An intuitive way to think about rotation matrices is:

$$ {}_{i}\mathbf{R}_{j} = [{}_{i}\mathbf{e}_{j}\ {}_{i}\mathbf{u}_{j}\ {}_{i}\mathbf{w}_{j}]$$

where ${}_{i}\mathbf{e}_{j} \in \mathbb{R}^{3}$ is a column vector and represents the $\text{x}$-axis of frame $j$ represented in frame $i$ - and the same for ${}_{i}\mathbf{u}_{j}$ and ${}_{i}\mathbf{w}_{j}$ with the $\text{y}$ and $\text{z}$ axes respectively. At the end of the day, there's plenty of ways to form this matrix - so there will be many internet resources on how to do this if you find yourself lost.

So, we have ${}_{c}\mathbf{R}_{0}$ and ${}_{c}\mathbf{x}_{0}$ - let's say:

$$ {}_{c}\mathbf{T}_{0} =  \left[ {\begin{array}{cc}
    {}_{c}\mathbf{R}_{0} & {}_{c}\mathbf{x}_{0} \\
    \mathbf{0} & 1 \\
  \end{array} } \right] $$

and now we can try moving frame $n$ to frame $c$ via ${}_{c}\mathbf{T}_{0} \cdot {}_{0}\mathbf{T}_{n}$. This gives us:

$$ {}_{c}\mathbf{T}_{0} \cdot {}_{0}\mathbf{T}_{n} = \left[ {\begin{array}{cccc} {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{e}_{n}  & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{u}_{n}  & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{w}_{n}  & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{x}_{n} + {}_{c}\mathbf{x}_{0} \\
    0 & 0 & 0 & 1 \\
  \end{array} } \right] $$

where

$$ {}_{c}\mathbf{R}_{n} = \left[ {\begin{array}{cccc} {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{e}_{n}  & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{u}_{n}  & {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{w}_{n} \\ \end{array} } \right] $$

and

$$ {}_{c}\mathbf{x}_{n} = {}_{c}\mathbf{R}_{0} \cdot {}_{0}\mathbf{x}_{n} + {}_{c}\mathbf{x}_{0} $$

Thus ${}_{c}\mathbf{T}_{n} = {}_{c}\mathbf{T}_{0} \cdot {}_{0}\mathbf{T}_{n}$, and there we have the coordinate transformation we are looking for. Now to actually use this transformation, what I would recommend would be multiplying ${}_{c}\mathbf{T}_{n}$ directly with the $[x\ y\ z]$ vector you mention (appended with a $1$ of course) to get the properly transformed end-effector location w.r.t. the "custom" frame. To get the orientation, I would recommend converting your $[r_{x} \ r_{y} \ r_{y}]$ vector to a rotation matrix - this will depend on what these values actually physically represent (here). Once in rotation matrix form, find the rotation in frame $c$ as:

$$ {}_{c}\mathbf{R}_{n} = {}_{c}\mathbf{R}_{0} \cdot \text{rot_mat}(\mathbf{r}_{x}, \mathbf{r}_{y}, \mathbf{r}_{z}) $$

where $\text{rot_mat}(\cdot)$ represents a function to convert your orientation variables to a rotation matrix. After you do these conversions, you should have all you need to perform your tasks as you are wanting to.


问:
Opposite direction of rotation calculated by Gyro and Accelerometer
I have just started learning how to fuse the data measured by the gyro and accelerometer to estimate the attitude of an IMU in Matlab.  I have no problem estimating or understanding the Euler angles calculated from at set of gyro data. But when I calculate the Euler angles from the accelerometer data, the rotation always seem to go in the opposite direction to that of the gyro's.

(note:the order of rotation is X(Phi) -> Y(Theta) -> Z(Psi))

To troubleshoot, I ran a very simple experiment where I first fixed the x-axis parallel to the ground with the y-axis pointing straight down. Then I gradually rotated the object around the x-axis positively (CCW) by roughly ~+40°.  All the signs and direction of rotation follow the right-hand-rule and right-hand-curl rule.

For clarity I attach the following drawing to associate sections of the accelerometer data to the state of rotation (note: +x-axis points out of the page):

The accelerometer data looks correct. ay stays pointing downward for the first few seconds and logs a value of -1g before rotating.  As the object frame rotates around the x-axis (positively or CCW), ay decreases in magnitude as az increases its.

I expect the Phi-delta measured by gyro should start with 0 deg and end with ~+40 deg because the rotation is positive (CCW), if we ignore the noise, drift, and accumulation of error due to numerical integration - the calculated graph pretty much agrees.

Then with the accelerometer, I expect the Phi to go from -90 deg to -45 deg. The slope should be positive because, again, the rotation is identical and is positive (CCW). Phi-delta is positive, whereas the Phi values should remain negative because the frame position is still CW or negative away from the horizontal orientation.  But when I calculate the Phi, I get the complete opposite:

I am not sure what is wrong in my thought process and hope someone can point out or correct that for me.

答:
Welcome to Robotics, KMC!

One thing I noticed is that your equation gives $\phi=\arctan2(-a_y,-a_z)$, but you're calculating it as $\phi=\arcsin(-a_y/\cos(\theta))$. If your sensor is oriented as depicted then I would expect $a_x$ to be around zero, which would make $\theta = \arcsin(a_x)$ also around zero. This, then, would make $\cos(\theta)$ around 1, which would leave your calculation as basically $\phi \approx \arcsin(-a_y)$.

Since your y values are ranging from about -1 to about -0.75, I would then expect your values as-calculated to be around $\arcsin(-(-1)) = +90^\circ$ to about $\arcsin(-(-0.75)) = 48.6^\circ$, which seems to be pretty close to what you're getting in your plots.

Just for sanity, because your starting values are easy, I took your other formula, $\phi = \arctan2(-a_y, -a_z)$ and evaluated it at the start. You've got $a_y \approx -1$ and $a_z \approx 0$, which gives $\phi = \arctan2(-(-1),-0)$, or $\phi \approx 90^\circ$. This seems odd, because in your drawing we can see that the rotation should be -90, like you mention in your post.

So then I went back to your equations in matrix form:

$$
\left[\array{
a_x \\
a_y \\
a_z \\
}\right] = \left[\array{\sin(\theta) \\
-\cos(\theta)\sin(\phi) \\
-\cos(\theta)\cos(\phi)}\right]
$$

If I divide row 2 by row 3 then I get the following:

$$
\frac{a_y}{a_z} = \frac{-\cos(\theta)\sin(\phi)}{-\cos(\theta)\cos(\phi)}
$$

The $-\cos(\theta)$ cancels on top and bottom, leaving

$$
\frac{a_y}{a_z} = \frac{\sin(\phi)}{\cos(\phi)}
$$

Which reduces to
$$
\frac{a_y}{a_z} = \tan(\phi)
$$

and then solve for $\phi$ with $\arctan$:

$$
\phi = \arctan2(a_y, a_z) \\
$$

I'm not sure where the negative signs came from in your $\phi=\arctan2(-a_y,-a_z)$ but if you do this without the negative signs then I think your numbers come out correct. I wouldn't continue to use the version that relies only on $a_y$ and $a_x$ because $a_x$ is not really a valid input for the $\phi$ calculation.

I'd also recommend (as always) using the Madgwick filter for anything to do with IMU handling. It looks like your units are $\times10^6$, too, so I'd double-check that you're in proper engineering units before trying to push them through the filter.