问:
How to publish odometry from 3 wheeled omnidirectional robot?
I'm new to ROS and robotics and I'm trying to implement the ROS navigation stack in a 3 wheeled omnidirectional robot. I already made the map using RTABMap but now I'm stuck in the 'Publishing Odometry Information over ROS' step. Currently the robot ouputs odometry data in geometry_msgs/Pose2D (http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Pose2D.html) format.

Here are some of the codes compiled in STM32:

```<code>//Calculate Odometry
void calculate_odometry(void)
{
    short int odometry0_speed = odometry0;
    odometry0 = 0;
    short int odometry1_speed = odometry1;
    odometry1 = 0;

    float buffer_x[2];
    float buffer_y[2];

    buffer_x[0] = odometry0_speed * cosf(gyro_radian + 0.785398);
    buffer_x[1] = odometry1_speed * cosf(gyro_radian + 2.356190);

    buffer_y[0] = odometry0_speed * sinf(gyro_radian + 0.785398);
    buffer_y[1] = odometry1_speed * sinf(gyro_radian + 2.356190);

    x_buffer_position += (buffer_x[0] + buffer_x[1]) * odometry_to_cm;
    y_buffer_position -= (buffer_y[0] + buffer_y[1]) * odometry_to_cm;

    x_position = x_buffer_position - x_offset_position;
    y_position = y_buffer_position - y_offset_position;
}

//RECEIVE data FROM gyro
void DMA1_Stream1_IRQHandler(void)
{
    if (DMA_GetITStatus(DMA1_Stream1, DMA_IT_TCIF1))
    {
        if (gyro_status == RESET)
        {
            gyro_status = SET;
            buzzer(22, 22);
        }

        memcpy(&gyro_buffer, gyro_receive + 3, 4);

        gyro_angle = (gyro_offset - gyro_buffer);
        gyro_radian = (gyro_offset - gyro_buffer) * 0.01745329252;

        while (gyro_angle > 180)
            gyro_angle -= 360;
        while (gyro_angle < -180)
            gyro_angle += 360;

        while (gyro_radian > 3.14159265359)
            gyro_radian -= 6.28318530718;
        while (gyro_radian < -3.14159265359)
            gyro_radian += 6.28318530718;

        USART_DMACmd(USART3, USART_DMAReq_Rx, DISABLE);
        USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);

        DMA_ClearITPendingBit(DMA1_Stream1, DMA_IT_TCIF1);
    }
}

//ROS SUBSCRIBER CALLBACK
void cllbck_sub_velocity(const geometry_msgs::Twist &msg)
{
    x_velocity = msg.linear.x;
    y_velocity = msg.linear.y;
    angular_velocity = msg.angular.z;

    motor_timer = 0;
}
</code>```

The gyro data is from GY-521 + Arduino.

From these code I should be able to get

```<code>x -> x_position          dx -> x_velocity
y -> y_position          dy -> y_velocity
A -> gyro_angle          ω -> angular_velocity
</code>```

right?

And then how can I publish the data in nav_msgs/Odometry (http://docs.ros.org/kinetic/api/nav_msgs/html/msg/Odometry.html) format? Do I have all the data I need correctly?

Should I use the code in http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom, what need to be changed or included in the code?

Or can I do autonomous navigation by only using visual odometry from RTABMap?

答:
It looks like you have most of what you need to output the Odometry message. The tutorial at http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom is probably the best practical resource.

The odometry message is a full 3D message. So you can simply fill in the other dimensions as zero (height, roll, and pitch) assuming that's accurate for your vehicle.

The other element of the Odometry message is the velocity expressed as a Twist. This is just a name for a 6dof velocity vector (x, y, z, roll, pitch, yaw). So I see again that you have the (vx, vy, v$w$) So you will again fill in the other fields as zero velocity for (z, pitch, yaw).

The last part is that both pose and velocity terms optionally have a covariance matrix. To start off you can not fill it in. For the next step level you can manually estimate the covariance and just plug the same ones in every time that you publish the message. How to estimate the covariance matrices is a whole additional question that I'd suggest that you look for tutorials on. Depending on what application you're planning to use it for the accuracy of the covariance might be important or less important.


问:
Stepper Motor power supplies
Being new to the world of stepper motors, I have a Longs Stepper Motor 23HS9430 bought from eBay it's a 3.0 A 5 V motor.

Although I have been reading about the supply voltage for the motor, the controller I have, again from eBay, has a supply voltage of 9-42 V DC.

What would be the best controller and supply voltage to run the stepper? The application is a antenna controller with a worm drive gearbox connected to the hardware that moves the antennas.

答: The power supply voltage of hybrid stepping motor driver is generally in a wide range (for example, the power supply voltage of mc425 is 12 ~ 36VDC). The power supply voltage is usually selected according to the working speed and response requirements of the motor If the working speed of the motor is high or the response is required to be fast, the voltage value is also high, but note that the ripple of the power supply voltage cannot exceed the maximum input voltage of the driver, otherwise the driver may be damaged</p>

ContentLicense: CC BY-SA 4.0

Title: Stepper Motor power supplies


问:
Assistance in finding correct LIDAR for short-range object detection task
I am working on a task that involves finding a distance of a moving object going a static linear object, e.g a long pole. After trying some approaches (mostly involving stereo vision), I have come up with the idea of building an array of 1D Lidars set apart in fixed distances from each other and from the static object that could help achieve this task: (array in red, pole is gray, pardon the drawing :) )

The moving object goes over the static object, and "cuts" the beams of several of the 1D Lidars in our array.

We thus know the object position with a precision of the space between Lidars in our 1D array.

I have searched for various lidars to achieve this purpose, and because of the precision being tied to spacing between sensors, the following constraints arise:

Something small is necessary for this purpose - diameter of max 2cm, assuming this is still a circle-shaped sensor.

Since it's tracking a moving object, a scanning frequency of, say, >30hz would be good.

The range is up to 5 meters.

The beam is very directional and focused, so that there is no crosstalk/interference between elements of lidar array.

Precision does not matter, I just need to know if there is an object present or not.

The closest candidate that fulfills the requirements by size is actually an ultrasonic sensor, however that has issues with focus and interference, plus needs a receiver on the other side, thus does not fit this task.

I also searched for pre-built lidar arrays, but generally they were either too large or too slow. If anyone knows of a sensor that fulfills this purpose or can guide me to who might offer one, I would appreciate it. Thank you.

EDIT: Clarifications for project:

This is a research project for sports with a flexible budget, so costs are not much of a limit. The area of interest is within a range of 1 to 5 meters, and the tracked object is a human. Our desire is to precisely measure the object with a max disparity of up to 2cm to a height of up to 40cm, but it may be less. The attached pic illustrates our case - an object goes over the virtual "tube" (a region of interest, essentially) center with an unknown distance, which is the distance we would like to measure.

答:
Go back and look at the camera idea again, but this time put the camera at the end of the "tube" (where you sketched the lidar) looking along the tube just like the lidar does. It will see everything the lidar would, except better. Cameras already have a dense, high performance array of sensors (at the image plane).

A simple webcam would meet your criteria of size and precision, and fancier cams can offer a higher frame rate if you need it. Methods of camera calibration, motion detection, and blob tracking are mature and available for free (such as OpenCV).

This might be a solution to the problem, although it doesn't exactly answer the question as phrased. If xen20 likes this, he or she can edit the question and text to admit this answer (and make it not a shopping question). Then I can edit this answer to remove this meta paragraph.


问:
What thread to use for a load-bearing telescoping lift
I'm trying to find a thread that has good fatigue resistance and strength for a telescoping lift. The lift will lift a weight of around 2-3 kg. I do not intend to spool it as I read that thread with good fatigue resistance are usually not good for spooling, although if there is a kind of thread that is also good with spooling then please let me know. I also want the thread to be relatively lightweight, although that is not a big priority.

I have been looking at steel thread wires but I was wondering if there are any alternatives. Thanks!

答:
I've used Spectra® braided fishing line for a cable driven robot hand.  (I believe there are many similar products by other brands).  I think it would probably work for your application too.

Steer clear of monofilament fishing line.  They probably have too much stretch.

This new type of braided fishing line is super strong and abrasion resistant.  Being a fishing line, it spools nicely and is lightweight. It is also very slick.  I know from experience that you will probably have to research and try out a few different types of knots to find one that wont simply slip out.


问:
Degrees of Freedom
What is the Degree of Freedom for a k-link planar serial chain for k = 2, 3, 6?
Using Grüber Formula, I get k-link chain has k Degrees of Freedom. But the maximum DoF in a plane can be 3. I am not sure what I am missing.</p>

答:
In robotics, the Degrees of Freedom (DoF) typically refers to the dimension of the Joint Space. In this case, DoF = k.

In the planar case, 3 is the maximum dimension of the Task Space, instead.
When k > 3, we basically deal with a redundant planar manipulator.

Here's a quite illustrative series of slides on the topic of Kinematic Redundancy.

Title: Degrees of Freedom


问:
How to obtain the Jacobian matrix for this 2Planar without differentiation
Take a look at the below picture, I would like to derive the Jacobian matrix without  differentiation. In this Modern Robotics book, the screw theory is used. I've derived the forward kinematic using PoE formula which stated here:

$$
\begin{align}
M &=\begin{bmatrix} 1&0&0& L_1+L_2\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix} \\
\mathcal{S}_2 &= [0,0,1,L_1,0,0]^T \\
\mathcal{S}_1 &= [0,0,1,0,0,0]^T \\
e^{[\mathcal{S}_2]\theta_2} &= \begin{bmatrix} c_{\theta_2} &-s_{\theta_2} &0&-L_1(c_{\theta_2}-1) \\s_{\theta_2} &c_{\theta_2} &0&-L_1s_{\theta_2}\\0&0&1&0\\0&0&0&1 \end{bmatrix} \\
e^{[\mathcal{S}_1]\theta_1} &= \begin{bmatrix} c_{\theta_1} &-s_{\theta_1} &0&0 \\s_{\theta_1} &c_{\theta_1} &0&0\\0&0&1&0\\0&0&0&1 \end{bmatrix} \\
T_2^0 &= e^{[\mathcal{S}_1]\theta_1} e^{[\mathcal{S}_2]\theta_2}M \\
&= \begin{bmatrix} c_{\theta_1+\theta_2}&-s_{\theta_1+\theta_2}&0&L_2c_{\theta_1+\theta_2}+L_1c_{\theta_1} \\
s_{\theta_1+\theta_2}&c_{\theta_1+\theta_2}&0&L_2s_{\theta_1+\theta_2}+L_1s_{\theta_1} \\0&0&1&0\\0&0&0&1\end{bmatrix}
\end{align}
$$
In the book, the authors differentiate the last column of $T_2^0$ to obtain this:
$$
\begin{align}
\dot{x} &= -L_1\dot{\theta}_1 s_{\theta_1} - L_2(\dot{\theta}_1+\dot{\theta}_2)s_{\theta_1+\theta_2} \\
\dot{y} &= L_1\dot{\theta}_1 c_{\theta_1} + L_2(\dot{\theta}_1+\dot{\theta}_2)c_{\theta_1+\theta_2}
\end{align}
$$
Or more compactly as
$$
\begin{align}
\begin{bmatrix} \dot{x}\\\dot{y}\end{bmatrix} &=
\begin{bmatrix} (-L_1s_{\theta_1}-L_2s_{\theta_1+\theta_2})&(-L_2s_{\theta_1+\theta_2})\\(L_1c_{\theta_1}+L_2c_{\theta_1+\theta_2})&(L_2c_{\theta_1+\theta_2})\end{bmatrix}
\begin{bmatrix} \dot{\theta}_1\\\dot{\theta}_2\end{bmatrix} \\
\dot{\mathbb{x}} &= [J_1(\theta) \ J_2(\theta)]\dot{\theta} \\
\dot{\mathbb{x}} &= J(\theta) \dot{\theta}
\end{align}
$$
where $J(\theta)$ the Jacobian matrix. The authors claim the Jacobian matrix can be obtained without differentiation using this formula:

I couldn't reach to the same result with the above formula. For the first column, $J_{1s}=\mathcal{S}_1$ which is not the same result obtained by differentiation. Any suggestions?

答:
It seems there are two types of Jacobian matrix which are the geometric and analytic ones. They are not the same but related. The one I've provided in my question is the geometric Jacobian expressed in the spatial frame. The same one expressed in the body frame is

$$
\begin{align}
\mathcal{B}_1 &= [0,0,1,0,L_1+L_2,0]^T \\
\mathcal{B}_2 &= [0,0,1,0,L_2,0]^T \\
\mathcal{V}_b &= \text{Ad}_{e^{-[\mathcal{B}_2]\theta_2}}(\mathcal{B}_1) \dot{\theta}_1 + \mathcal{B}_2\dot{\theta}_2 \\
e^{-[\mathcal{B}_2]\theta_2} &= \begin{bmatrix} c_{\theta_2} &s_{\theta_2} &0&L_2(c_{\theta_2}-1) \\-s_{\theta_2} &c_{\theta_2} &0&-L_2s_{\theta_2}\\0&0&1&0\\0&0&0&1 \end{bmatrix} \\
%
R_{e^{-[\mathcal{B}_2]\theta_2}} &= \begin{bmatrix} c_{\theta_2} &-s_{\theta_2} &0 \\s_{\theta_2} &c_{\theta_2} &0\\0&0&1 \end{bmatrix} \\
%
p_{e^{-[\mathcal{B}_2]\theta_2}} &= [L_2(c_{\theta_2}-1), \ L_2s_{\theta_2} \ ,0]^{T}\\
%
%
\text{Ad}_{e^{-[\mathcal{B}_2]\theta_2}} &= \begin{bmatrix} R_{e^{-[\mathcal{B}_2]\theta_2}}& 0\\ [p_{e^{-[\mathcal{B}_2]\theta_2}}]R_{e^{-[\mathcal{B}_2]\theta_2}} & R_{e^{-[\mathcal{B}_2]\theta_2}} \end{bmatrix}\\
%%
J_{b_1} &= \text{Ad}_{e^{-[\mathcal{B}_2]\theta_2}}(\mathcal{B}_1) \\
J_{b_2} &= \mathcal{B}_2 \\
J_{b}(\theta) &=[ J_{b_1}, \ J_{b_2}]
\end{align}
$$
Converting the geometric Jacobian to the analytic one is straightforward.
$$
\begin{align}
J_v(\theta) &= J_b(\theta)_{(4:6,:)} \\
&= \begin{bmatrix}  L_1s_{\theta_2} & 0 \\ L_2+L_1c_{\theta_2}& L_2\\0&0\end{bmatrix} \\
R_{sb} &= \begin{bmatrix} c_{(\theta_1+\theta_2)}&-s_{(\theta_1+\theta_2)}&0\\s_{(\theta_1+\theta_2)} &c_{(\theta_1+\theta_2)}&0\\
0&0&1\end{bmatrix} \\
J_a(\theta) &= R_{sb} J_v(\theta)
\end{align}
$$
where $J_a(\theta)$ is the analytic Jacobian matrix.


问:
System for determining occupied seats in an auditorium
I need an app that can do live monitoring of whether each seat in an auditorium is occupied,  so visitors can load the app and see where to sit.

The auditorium has a relatively flat ceiling 4m high, and the seats are .5m wide.
The hardware cost per seat needs to be $5.

I'm looking for all solutions.  Web cams, preasure sensors, sonars, lasers, arduino, pi, intel edison, anything.

Obviously there cannot be wires that people could trip over.  Sensors on the ceiling could have wired networking.  Sensors on the seat or floor would need to have wireless communication.  sensors on the ceiling would need to consider occlusion by people sitting in the seats (think, if there is an empty spot between 2 people, can the sensor see it as empty)

In the end, the data needs to be collected as a simple list of which chairs are occupied/open

Possible solutions:

rasberry pi's on the ceiling every 8 seats with a camera.

pressure sensors under chair legs wired to pi's gpio

Drones flying around the auditorium :)

Any ideas?

Update (more constraints):

auditorium size is 400 seats

Installation costs should average 10 chairs per hour(400/10 = 40 hours)

as the picture shows, chairs are cushioned

regular maintenance should take no longer than 30 min. per 2-hour event(eg, batteries)

hardware should last 100 sessions

for auditorium cleaning, it should be possible to "disconnect" and "reconnect" the chairs with 4 hours of labor.

答:
Computer vision would work, but have some error margins like people putting a bag on the other chair. It may be uncomfortable to the people too to, although the cameras could be hidden.

Pressure sensors could suffers from a high weight bag too, where computer vision is more affected by area.

It's a fixed installation with a cost for that. When you can, use wires! There's then no batteries to worry, no signal problems, no RF pollution, and goes on.

As you have a relatively high number of seats, just use a small uC from Microchip, Atmel, or what you prefer, that should cost near 1,60 and a differential driver (RS-485 compliant), and use it to read lets say 4 seats sensors. Pass a bus of RS-485 (UTP with 2 pair), and poll the sensors.

EDIT (as OP said that has no way to pass wires in the floor)

If you go to computer visionnote 2 and you said a chair occupied by some object will in fact by considered occupied (and not the say the people to put the bag on the ground and allow space for another people), computer vision could do the job. Just think its not that cheap as you may think.

Lets take a 200,00 camera (it should be higher than this, need to check prices and options on your country), thought 20 seats, so 10,00/seat would be more than you can use. You could get a larger area depending on the height of the camera relative to the chairs, but high angle lens does not seems a good option as there will me much distortion.

Wiring would have near more or less the cost of the other option. As the cameras require each cable from the hub/switch to the camera, a bus would not be feasible like an RS-485 bus.

Note 1: You mean nothing of infrastructure for wiring, if there's no way to pass wires, then yes, you should go wireless.

Note 2: For computer vision the better is to use RAW cameras (without image compression) and if it's unavoidable to use compression, at least H.264. (An lossless compression would give the same uncompressed image, but that's not easy to find).
Good optics are very important too, so good focus. As you have a controlled light environment, sensor size could be less important, but not forgettable. Computer vision is not that easy, that's why some projects fails. Just putting any camera with any lens would not to the work for most of the scenarios.

ContentLicense: CC BY-SA 3.0


问:
How large and complex a robotic systems from a software point of view?
So, I have this misconception, I think, that software engineering in robotics entails a deep reinforcement learning model and that is it.

So, if you work in this field as a software engineer how large and complex are robotic systems from a software point of view?

Compared to, for example, a compiler, which are large and complex, how complex are the systems that robotic software engineers work on?

Could you also describe the high level architecture of a robotic system?

I have Googled this question but I only get descriptions of the position. I have not found anything related the size and complexity and architecture of a robotic system.

答:
Perhaps surprisingly, you don't need large, complex software systems to produce complex behavior.  The book Vehicles: Experiments in Synthetic Psychology (aka Braitenberg Vehicles) does a good job explaining this concept.

As Chuck said, the definition of "robot" is vague and can incorporate lots of things.  But lets take for example the Roomba (arguably one of the most prevalent consumer robots).  Before it had systematic navigation, you could argue that the code was pretty simple:

Drive straight

If you bump into something, then turn randomly

If your battery is low and you see the dock, then dock yourself

Repeat

Of course, this is a gross oversimplification.  But the code for these early Roombas (and many other consumer robots) fit into cheap and small microcontrollers.

The robotics software engineers did not set up some giant reinforcement learning problem to "teach" the Roomba how to clean.  Instead, they set up a finite state machine, subsumption architecture, behavior tree, or some other "standard" software architecture.  Because at the end of the day, a robot is just a machine programmed to do a task.


问:
Control method advice for Markov (?) process
I'm experimenting with a device for tuning piano strings. It attaches to the piano's tuning pins and turns them slightly by applying a torque impulse. It is a semi-automatic process, handled by a human operator.

Operator presses a piano key, control system records the tone and calculates the frequency error. (The set frequency is known.)

The control system calculates the required size of the torque impulse.

The device applies the torque impulse.

The operator presses the key again. If pitch error is still large, repeat from 1.

The change in frequency caused by a torque impulse is a function of the impulse size and the current frequency of the string.

I'm now going to design a control system for this. So far I've found that it probably is a Markov process, since it is not time dependent and not depends on previous states. But what could be a suitable control method for this application?

答:
What a great idea! Here are some things I'd look out for, based on my experience tuning the pianos I have had over the years. (Not that I'm a pro, just trying to postpone calling in the pro.)

The tuning pins stick in place (obviously, otherwise they would immediately untwist!). A tuner must apply some torque to a pin, just to get it unstuck, before actually tuning the string. The necessary torque "used up" to get unstuck will vary from pin to pin, even on the same note, even on the same pin (if I pull it too sharp, back off, and try again).

Neither is there any reliable relation between delta-torque (actually delta-pin-angle) and delta-pitch, given the variability from string to string due to age, stiffness, even string manufacturer.

There are general trends, of course. Treble strings are under more tension than fat floppy bass strings. Shorter strings need tinier increments than longer ones. Wurlitzers and Yamahas and Steinways all behave differently. If it's way flat you need to bring it up more.

With all that variability, I wouldn't expect to be able to compute and apply a single impulse that would both unstick a pin and stretch its string to bring it into tune. Instead, I would expect a series of smaller impulses to ease it into place. More like a BZZZzzz or a squeeeeek instead of a quick double BANGbang. But listening and predicting would certainly speed the operation, even if it's not all at once.

The controller would want to listen to the string while those smaller impulses are in progress, sampling the pitch continuously, even from a single key strike, as it draws up the pitch. (Of course it would listen for the key strike, not wait for a trigger push). Doing just one sample and one impulse per key strike seems tedious.

I'm not even sure it works to reach a pitch exactly and expect it stay there. Does it unwind a teeny bit before the pin's static friction can "bite"? If so, the tuner has to go the same teeny bit sharp to compensate.

You know (or soon will) how complicated the relationship is between physical frequency and perceptual pitch, and how zero crossings of the waveform hardly anything to do with either (because of how the higher harmonics are non-harmonically sharp -- the string stiffness shortens the effective length). A related thing I have never quite understood is whether you want all strings for a given note to be in exact unison, or whether a certain richness or "fatness" comes from faint detuning.

If you'll forgive some brainstorming: It would be interesting for an upsell product in your product line to automatically manage muting the strings that you're not tuning, the thing the humans use the felts for. Also it would be interesting if the controller, besides listening to the strings, could excite them (speaker or electromagnetics, as well as mic). That wouldn't supplant the keyboard operation, but could complement it. Finally, the top-of-the-line model would move from pin to pin all by itself. If built into the piano, finally a product deserving the name "Auto-Tune"!


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

答:
Honestly, the easiest way to remove waypoints is to simply skip every 1 or 2 or ## waypoints in your Dijkstra or A* (or similarly navigated) map. You do this by looking at the list of nodes to travel to and literally ignoring specifically chosen nodes. Not only do you have to create a path plan for your robot (or people), you need to create a separate algorithm on top of the navigation algorithm that takes care of removing waypoints.

This can be done in a smarter way by looking at the 2-D gradients. In other words, if the grid were an image, you would pass the image through an edge detector filter and try to place straight lines or curves over the edges with fitting algorithms (splines come to mind here).

The problem here is that you need to start from a high precision map, and then remove points to create a low precision map until you start to intersect obstacles or other experiment constraints. The parameters of precision aka. the resolution of the map (like an image) are experiment specific, and are therefore outside the scope of robotics stack exchange. In other words, the "simplification" of the map is an entirely separate problem to path planning.

Note: I am sure there are algorithms that do this automatically, I simply don't know them. Algos like RRT can take advantage of simplification, but those aren't grid based like the question suggests as a requirement, but I may be reading too far into the question.


问:
World and Map Frame for a real robot
I am trying to develop a robot for mapping purposes. Currently, I have a Velodyne VLP16 lidar and an IMU. I am having a bit of a confusion in stating the transforms of the system. For the lidar and IMU, the transforms are supposed to be static and I have defined them using the following:

```<code>static_transform_publisher x y z qx qy qz qw frame_id child_frame_id  period_in_ms
</code>```

How do I define and publish world and amp frame? How do I define the dynamic transforms between world frame and base link?

答:
As you've figured, static transforms are valid for fixed offsets such as sensor positions. They are the minimal solution the more complete recommended solution is to setup a robot model. There's tutorials on that here.

More directly towards your question on how to define the map to base link. That is generally referred to as localization. You will need to run a localization algorithm to keep track of where your robot is in the map. The most common ones are either done in a predefined map, like amcl or by a simultaneous localization and mapping (SLAM) algorithm.

There are a lot of tutorials for getting setup, but I recommend the core series here: http://wiki.ros.org/navigation/Tutorials

Since you didn't clarify in your question if you're using ROS 2 there's a good entry point here: https://navigation.ros.org/ with a first time robot setup guide.

Title: World and Map Frame for a real robot


答:
You are looking for an easy cheap solution.  I doubt there is one.   Each easy solution can be fooled. A camera is going to detect a jacket of bag on a seat, a weight or pressure sensors will get tripped many times.  What if a person "takes" a seat them walk around.  How does your system know a seat is reserved.

I think this system will need to use multiple sensors and some "rules of thumb" to make guesses.   You will have to accept that the results will not be perfect.

Star  with pressure sensors.  You can wire an entire row together from seat to seat without creating any wire s on the floor.  Then you have boxes mounted on the seats nearest the isle to handle that row.  These boxes will need power.  You will need to run wire down the ales to each isle box.

I would back this up with another system perhaps based on vision.

Then you can also count people and cross check the totals from the count, pressure and vision systems.

OK, one last question?  Why do you care about the cost of the seat sensors.  That will be trivial compared to the engineering and installation costs.

Title: System for determining occupied seats in an auditorium

Body:
I need an app that can do live monitoring of whether each seat in an auditorium is occupied,  so visitors can load the app and see where to sit.

The auditorium has a relatively flat ceiling 4m high, and the seats are .5m wide.
The hardware cost per seat needs to be $5.

I'm looking for all solutions.  Web cams, preasure sensors, sonars, lasers, arduino, pi, intel edison, anything.

Obviously there cannot be wires that people could trip over.  Sensors on the ceiling could have wired networking.  Sensors on the seat or floor would need to have wireless communication.  sensors on the ceiling would need to consider occlusion by people sitting in the seats (think, if there is an empty spot between 2 people, can the sensor see it as empty)

In the end, the data needs to be collected as a simple list of which chairs are occupied/open

Possible solutions:

rasberry pi's on the ceiling every 8 seats with a camera.

pressure sensors under chair legs wired to pi's gpio

Drones flying around the auditorium :)

Any ideas?

Update (more constraints):

auditorium size is 400 seats

Installation costs should average 10 chairs per hour(400/10 = 40 hours)

as the picture shows, chairs are cushioned

regular maintenance should take no longer than 30 min. per 2-hour event(eg, batteries)

hardware should last 100 sessions

for auditorium cleaning, it should be possible to "disconnect" and "reconnect" the chairs with 4 hours of labor.


问:
Equations of motion with mass attached to end effector and external force
I have to find the equations of motion for the given manipulator:

The links of the manipulator are considered weightless and the equations of motion have to be derived by using the Lagrange equations. The coordinated of the point mass are:

$$ x_m = q_1+l_1+l_2c_2 $$
$$ y_m = l_2s_2 $$
And the Lagrangian of the system is:

$$ L = T - V = \frac{1}{2}\cdot \Big[\dot{q_1}^2-2l_2s_2\dot{q_1}\dot{q_2}+l_2^2\dot{q_2}^2\Big]-mgl_2s_2 $$

Now, in order to find the equations of motion, I applied the Lagrange's Method:

$$ \frac{d}{dt}\Big(\frac{\partial L}{\partial \dot{q_i}}\Big) - \frac{\partial L}{\partial q_i} = \tau_i $$

And the final equations of motion written in matrix form are:

$$ \begin{bmatrix} m & -ml_2s_2 \\
-ml_2s_2 & ml_2^2 \end{bmatrix}\cdot \begin{bmatrix} \ddot{q_1} \\ \ddot{q_2} \end{bmatrix} + \begin{bmatrix} 0 & -ml_2c_2\dot{q_2} \\ 0 & 0 \end{bmatrix}\cdot \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \end{bmatrix} + \begin{bmatrix} 0 \\ mgl_2c_2 \end{bmatrix}=\begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix} $$

The above equations of motion have been derived by ignoring the force applied $F_x$. Now, my issue is how to include this force in terms of the Lagrange Method. Should I modify the method like the one below:

$$ \frac{d}{dt}\Big(\frac{\partial L}{\partial \dot{q_i}}\Big) - \frac{\partial L}{\partial q_i} = \tau_i + F_x $$

But in which of the two equations of motion should the force be included ? The force $F_x$ is an external constant force applied in the direction of $x_0$.

答:
In Lagrangian mechanics, you're required to deal with the generalized forces and the concept of virtual work $\delta W$.

In our case we have:
$$
\delta W_{F_x} = F_x \cdot \delta q_1 + F_x \left( -l_2s_2\right) \cdot \delta q_2,
$$

where the coefficients of the terms $\delta q_i$ are given by $F_x \frac{\partial x_m}{\partial q_i}$. By contrast, $y_m$ doesn't provide any contribution to the virtual work as $F_y = 0$.

This sums up in the final equation as:
$$
...=\begin{bmatrix} \tau_1 + F_x \\ \tau_2 -F_xl_2s_2 \end{bmatrix}.
$$

In particular, $\tau_1$ is a force actuating the prismatic joint $q_1$, whereas $\tau_2$ is a torque actuating the revolute joint $q_2$. Therefore, units are consistent.


问:
How to make a synchronized LiDAR/Camera rig for Autonomous Driving research?
I am just starting a research project at my university involving sensor fusion between a Camera and a LiDAR device to detect and localize cars around our vehicle (bounded boxes around cars in 3D point-cloud). We need a rig having one Camera and one LiDAR to capture data, which we will label manually, and later we will train a deep neural model that takes the advantage of color intensity and depth information for increased accuracy. However, upon basic online search, I found that we'll need a frame synchronized rig for such purpose. I also found that usually LiDAR devices have a sync port for such purpose, but I don't know how I can connect it with a camera so that both devices generate frames capturing the exact same instant in time? Do usual cameras have such a port or do I need to buy a special camera?

Thanks.

答:
However, upon basic online search, I found that we'll need a frame synchronized rig for such purpose

This is not a simple problem and there exist several solutions, based on project needs, to achieve what you are trying to do. A basic search isn't enough. This forum is for specific questions / errors you want resolved.

As a starting point, you can take a look at Autoware's object detection. They have a pre-trained network you can use as a starting point. If you do not use a triggered camera, you may co-calibrate your lidar and camera (again autoware has one) to get transforms between them and use that as an input to your network.

You could also take a look at lidar-bonnetal who use range images projected from a lidar. Maybe you can incorporate color information in them?

You will also find several newer works on ArXiV like RoIFusion, end-to-end approach, etc.


问:
Examples of Zeno Behaviour in the Real World
Zeno Behaviour or Zeno Phenomenon can be informally stated as the behavior of a system making an infinite number of jumps in a finite amount of time.

While this is an important Control system problem in ideal systems, can Zeno behavior exist in real systems? Any examples?

If so, why don't noise or external factors deviate a system from achieving Zeno?

答:
To the principal question, "can Zeno behavior exist in real systems?" the answer is no.

Real systems don't do any infinite anything. The Quantum Zeno Effect, a.k.a. Turing paradox, is a purely quantum phenomenon. In this robotics forum, real systems are macroscopic, and macroscopic systems don't do any quantum anything. Hence, there are no examples and no need to explain about noise.

If you want to get all reductionist and want to explain how macroscopic phenomena arise from quantum behavior, or how the infinities and/or infinitesimals of differential equations are used to describe real behavior, I'll have to send you to some other StackExchange site.


问:
What robotic simulators simulate the reflectivity of objects?
I want to simulate the reflectivity of objects in a simulation because I want to use a 2D laser scanner to detect certain objects that have higher reflectivity than other objects in a particular scene. I was wondering which robotics simulators have built-in support in object reflectivity. I know that Gazebo default behavior doesn't simulate reflectivity in objects. There is a property called <laser_retro> that you can add to the collision property. The problem with that is that the reflection is not realistic and it always returns the intensity value that you set in that property. That is, the intensity value is constant. Sure, I can create a gazebo plugin to simulate that with a particular object using real data but I'm looking for a built-in solution in the simulator.

Is there any with that feature? maybe Unity Robotics, CoppeliaSim or Nvidia Isaac Sim? or any other?

答:
In professional robotics development, a company will build an experiment with their robots and lidar sensors to test reflectivity. No simulation is used for reflectivity.

Assuming you are not in a professional work environment, if you are going to use a simulator that incorporates realistic visuals, your best bet would be either Nvidia's Omniverse or Unreal Engine. Gazebo is EOL now and should not be used for brand new projects (unless you are shackled to ROS 1). Gazebo has been replaced with Ignition, but Ignition has only been released within the last 18 months. It shows promise with lighting configuration parameters. Unity is great for basic simulations but is less feature complete compared to Unreal Engine, which is (arguably) the best game engine/simulation engine on the market. I know you can tune the physics of object and lighting in Unreal, so that would be the other major software recommendation. The issue with this question is that it is more opinionated than a "how do you do this" question, which is more what this stack exchange website is designed to answer. You will have to do more research on this subject - there is a stack exchange cite for software recommendations. Does this help you?

Edit:

Gazebo Ignition is now called Gazebo again.
There is also Bullet/PyBullet and Nvidia Omniverse if you want to try other alternatives.


问:
Quadcopter instability with simple takeoff in autonomous mode
I'm trying to get a quad rotor to fly. The on board controller is an Ardupilot Mega 2.6, being programmed by Arduino 1.0.5.

I'm trying to fly it in simple autonomous mode, no Radio controller involved. I've done a thorough static weight balancing of the assembly (somewhat like this: http://www.youtube.com/watch?v=3nEvTeB2nX4) and the propellers are balanced correctly.

I'm trying to get the quadcopter to lift using this code:

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

int maxspeed = 155;
int minspeed = 0;

Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

int val = 0;
int throttleCurveInitialGradient = 1;

void setup()
{

val = minspeed;

motor1.attach(7);
motor2.attach(8);
motor3.attach(11);
motor4.attach(12);

}

void loop()
{
setAllMotors(val);
delay(200);
val>maxspeed?true:val+=throttleCurveInitialGradient;
}

void setAllMotors(int val)
  {
    motor1.write(val);
    motor2.write(val);
    motor3.write(val);
    motor4.write(val);
  }
</code>```

But the issue is, as soon as the quadcopter takes off, it tilts heavily in one direction and topples over.

It looks like one of the motor/propeller is not generating enough thrust for that arm to take-off. I've even tried offsetting the weight balance against the direction that fails to lift, but it doesn't work (and I snapped a few propellers in the process);

Is there something wrong with the way the ESCs are being fired using
the Servo library?

If everything else fails, am I to assume there is something wrong
with the motors?

Do I need to implement a PID controller for self-balancing the roll
and pitch just to get this quadrotor to take off?

Edit 1:    Thanks for all the replies.

I got the PID in place. Actually, it is still a PD controller with the integral gain set to zero.

Here's how I'm writing the angles to the servo:

```<code>motor1.write((int)(val + (kP * pError1) +(kI * iError1) +(kD * dError1)));  //front left
motor2.write((int)(val + (kP * pError2) +(kI * iError2) +(kD * dError2)));  //rear right
motor3.write((int)(val + (kP * pError3) +(kI * iError3) +(kD * dError3)));  //front right
motor4.write((int)(val + (kP * pError4) +(kI * iError4) +(kD * dError4)));  //rear left
</code>```

kI is zero, so I'll ignore that.

With the value of kP set somewhere between 0.00051 to 0.00070, I'm getting an oscillation of steady amplitude around a supposed mean value. But the problem is, the amplitude of oscillation is way too high. It is somewhere around +/- 160 degrees, which looks crazy even on a tightly constrained test rig.

[  Edit 2: How I calculate the term 'pError' - Simple linear thresholding.

I've a precomputed data of the average readings (mean and SD) coming out of the gyro when the IMU is steady. Based on the gyro reading, I classify any motion of the setup as left, right, forward or backward.

For each of these motion, I increase the pError term for two of the motors, i.e, for right tilt, I add pError terms to motors 2 & 3, for left tilt, I add pError term to motors 1 & 4 etc. (check the comment lines in the code snippet given above).

The magnitude of error I assign to the pError term is = abs(current gyro reading) - abs(mean steady-state gyro reading). This value is always positive, therefore the side that is dipping downwards will always have a positive increment in RPM.  ]

As I crank up the derivative gain to around 0.0010 to 0.0015, the oscillation dampens rapidly and the drone comes to a relatively stable attitude hold, but not on the horizontal plane. The oscillation dies down (considerably, but not completely) only to give me a stable quadrotor tilted at 90 - 100 degrees with horizontal.

I'm using only the gyros for calculating the error. The gyros were self calibrated, hence I do expect a fair amount of noise and inaccuracy associated with the error values.

Do you think that is the primary reason for the high amplitude
oscillation?

One other probable reason might be the low update frequency of the errors. I'm updating the errors 6 times a second.

Could that be a probable reason it is taking longer to stabilise the
error?

And, for the steady state error after the wild oscillations dampen, is it necessary to fine tune the integral gain to get rid of that?

Please help.

Edit 3:  I cranked up the frequency of operation to 150+ Hz and what I get now is a very controlled oscillation (within +/- 10 degrees).

I'm yet to tune the derivative gain, following which I plan to recompute the errors for the integral gain using a combination of gyro and accelerometer data.

Edit 4:  I've tuned the P and D gain, resulting in +/- 5 degrees oscillation(approx). I can't get it to any lower than this, no matter how much I try.

There are two challenges about which I'm deeply concerned:

After 5 to 8 seconds of flight, the quadcopter is leaning into one side, albeit slowly.

A) Can this drift be controlled by tuning the integral gain?

B) Can the drift be controlled by using accelerometer + gyro fused data?

C) Given that my drone still shows +/- 5 degrees oscillation, can I consider this the optimal set point for the proportional and derivative gains? Or do I need to search more? (In which case, I'm really at my wits end here!)

答:
I read the code but it looks like it is for the first attempt where you tried to hover "open loop"  that can't work.   So now you tried a PID control based straight off the raw IMU data.  Tha't better but you are going to need one more step between the two.   Will I say this assuming you are using a low-cost IMU, the kind that just breaks out the chip.

First off it would be ideal if you could measure the speed of each motor, and not just assume it rotates at a speed proportionate to the commanded value.  Likely it does not.  The BEST way to to place an encoder on the motor shaft that sends a few pluses per revolution.  Then you replace "motor2.write(val)" with something like "motor.setspeed(rotationVelocity); and the set speed method runs a PID loop just ontheat one motor's speed, sending whatever PWM value is required.   OK, lacking an encoder, get a hand held propeller tachometer.  And plot RPM vs PWM for a tried vehicle fixed to a stand at some hight from the ground.

NEXT YOU NEED TO KNOW HOW TO RELATE RPM TO THRUST.  One problem with yourPID control is it assume PWM value (voltage) is LINEARLY related to force in the vertical direction is is NOT because
1) If the vehicle is tilted the foxed is no longer down and is smaller be the cosine of the tilt and..
2) force is NEVER linearly replaced to voltage.  It think(???) there is a cubed relationship the is power cubed = force.

You might just kip all the math and measure the force as a function of PWM and make a lookup table.  Better to use encoders and

You over all PID loop will work much better when have a linear relationship.

Next idea:  You CAN make a quad more stable.  What if you tilt the plane of each door inwards just a little.  Each is slightly non-horizonal, now if one side dips it will generate more lift.

Last idea:  The IMU is a noisy device.  The accelerometer has high frequency noise on it becise of random vibrations and the gyro has drift.  The "standard" why to address this is with a Kalman Filter.  The math is not easy but you can find MANY examples of Kalman filters on-line.

Summary:
1) Use good filter model to correct the IMU data.  The raw data is not good.
2) Create a method (function) that takes llefting force as input then sets the PWM slue as required.

3) Used a top layer PID to drive the error to zero

What this does is address the basic flaw of all PID controller, they don't "see" inside the black box.  They don't know the gyro drift and accelerometer is noisy and that PWM has a non-linear relationship to lift.

Next you get into moving flight and you need to make your aerodynamic model better to account for the fixed pitch props


问:
Discretization of a control module
The desired control is $u_f=\frac{1+\eta s}{1+hs}a$, where $u_f$ is the feedforward control, $a$ is the feedforward observation and $\eta$ and $h$ are two constant parameters. However, I can only observe $a$ but not $\dot{a}$. How to realize this in discrete time?

The method I use now is the zero-order hold method, which yields the discrete-time transfer function as $H(z)=\frac{\eta z/h-\exp(-T/h)+1-\eta/h}{z-\exp(-T/h)}$. This result is the same as that of Matlab funtion c2d. With $H(z)$, I can calculate $u$ without knowing $\dot{a}$. Is it the right way to do it?

Moreover, the control system contains other modules such as the plant dynamics $x = Gu$ and feedback control $u_b = Ky$, where $u=u_f+u_b$. Can I discrete these modules one by one and then synthesize them?

答:
I can calculate u without knowing a˙. Is it the right way to do it?

Sure!

Moreover, the control system contains other modules such as the plant dynamics x=Gu and feedback control ub=Ky, where u=uf+ub. Can I discrete these modules one by one and then synthesize them?

Usually, we tend to first synthesize the controller in the s-domain and then discretize the continuous blocks with a given sample time $T$.

This pipeline is reported in https://robotics.stackexchange.com/a/4434/6941.

As a side note, consider using the Tustin formula for the discretization instead of the ZOH as the former method generally provides better results.

Title: Discretization of a control module


问:
Visual servoing: simple control law
The problem

I have a control scheme like the one in the image attached below (Source: Robotics, Vision and Control Fundamental Algorithms In MATLAB®, Peter Corke). But instead of a robotic arm, I have a pan/tilt head.

I am not sure what control law of the outer loop (IBVS controller) should I use. The input of this controller is the visual error in pixel units (in my case, the error is the difference between the image center and the centroid of the object of interest), and the output is the commanded position set-point of the inner loop. Notice that the output (set-point of the inner controller) is an absolute angular position, it is not a velocity or an angular variation / increment.

What I have tried

Like in this thread: Determining transfer function of a PTU for visual tracking I chose a simple proportional controller. However, I noticed that the output of this controller is an angular variation instead of an absolute angular position.

To solve this problem, I added the previous angular variation to the current angular variation. This way, the resulting position is an absolute angular position and can be properly interpreted by the inner loop.

Is this approach the same as using a PI? I mean, the strategy of adding angular increments is equivalent of using the integral term of a PI controller? Am I using an incremental PID or a positional PID, or a mixture of both? What am I exactly doing? I cannot find any papers that use this approach, most of them use a velocity control.

Results

The following results, for a proportional gain of 0.0004, where obtained. As it can be seen, there is no steady state error (even though I used only a P controller).

Another thing that bothers me is that the commanded tilt angle has a harder time reaching the steady state than the pan angle. Same behaviour is observed for smaller gains. Any idea why does this happen only in one angle?

答:
What you have implemented is a sort of proportional velocity controller. In turn, this controller can be seen as a pure integral position unit. The presence of the integral explains why you're able to reach for the setpoint with no steady-state error.

I can argue that it is somehow expected that the tilt joint responds slightly worse than the pan joint with the same controller's gain. In fact, the motor actuating the tilt needs to counteract the gravity, whereas the pan actuation is usually transparent to gravity. However, to confirm this hypothesis, one should have a look at the physical assembly of the camera.

No worries though, you ought only to tune up the tilt gain to achieve better performance. Also, inserting a derivative term can help you dampen the initial snap overshoot.

Title: Visual servoing: simple control law


问:
Reload Gazebo without quitting app
I'm using Gazebo with ROS 1. Every time I re-run my ROS code, I seem to have to quit the Gazebo client and start it up again. Starting up the Gazebo client application takes a long time and I am wondering if there is a way to keep the client GUI application alive and simply reload the simulation or connection to the server.

I'm running the Gazebo client gzclient on macOS while gzserver is on a Ubuntu VM (along with ROS). The connection between the two is fine.

Please let me know how this can be done!

Thank you!

答:
Gazebo had the ability to reset the world. You can do it from the GUI, or or programmatically.

It sounds like you are starting your ROS code at the same time as the simulation. There's nothing tying those lifetimes together except how you are choosing to launch them. You'll want to refactor your launch process to separately bring up the simulation then bring up your code under test. Then you can reset the world and relaunch you code without restarting the Gazebo instance.

Note that for manual testing unless you need a very specific starting condition most application can be setup to support being brought down and back up. In this case you may want to move more of your system into the persistent launch so that you minimize the amount of the system that you are iterating on and relaunching each time.

Title: Reload Gazebo without quitting app


问:
Why Does an Exponential Make ANYTHING a Probability Distribution
I am posting this here because my background in estimation theory and optimization has been developed entirely through my experience in robotics.

TLDR: What makes it so that any time you put something in an exponential you can suddenly call it a probability distribution?

The Deets: I thought of this question while reading A General and Adaptive Robust Loss Function by Jonathan Barron. The paper describes a single parameter loss function that can subsume the most commonly used individual loss functions:

$$f(x, \alpha, c)$$

Where $x$ is parameter, $\alpha$ is the "single parameter", and $c$ is a distance scale that describes the "quadratic bowl" around the origin which all the loss functions share in common. When $\alpha = 2$ it is squared error loss, $\alpha =1$ it is essentially absolute error loss, and when $\alpha = 0$ it is cauchy loss.

My confusion starts when the paper goes on to say "With our loss function we construct a general probability distribution".

$$ p(x|\mu,\alpha, c) = \frac{1}{cZ(\alpha)}\exp(-f(x-\mu, \alpha, c)) $$

My question is, why does dividing by a normalization constant, and taking the exponential of the negative loss function suddenly make it a pdf? I understand the results, I have used these same ideas in my estimation theory courses, but I just don't get how we can make the jump from loss to pdf OR why it is so intuitive to do so.

Cheers, and happy holidays :)

答:
Just thought I'd point out that this doesn't always work. Suppose your loss function is $f(x)=\ln(|x|+1)$. Then $e^{-f(x)}=(|x|+1)^{-1}$ and the area under this curve is $\infty$ since

$$
\int_{\infty}^\infty (|x|+1)^{-1} \,\mathrm{d}x
=2\int_0^\infty (x+1)^{-1} \,\mathrm{d}x
=2\ln(x+1)|_0^\infty
=\infty
$$

Thus there is no way to normalize and get a pdf even though $e^{-f(x)}>0$ everwhere.

With respect to the $f(x,\alpha,c)$ described in your linked paper, the paper itself tells us that for $\alpha<0$ that the integral of $e^{-f(x)}$ does not converge (stated below equation 17) i.e. normalization is impossible.

On the other hand, for some intuition, the construction described in the paper is ubiquitous in statistical physics and is called the Gibbs measure/distribution associated to an energy function (loss function). This provides us some expectation that this sort of thing should frequently work and is worth a try.


问:
What is the best SE3 library for python?
I am currently googling on this topic but I would like to hear your opinion.

What is the best self-contained SE3 library that offers interconversion between quaternion, rotation vector, transformation matrix?

Those are the functions that I often use for my projects and all implemented in Matlab. And I am looking for an alternative to avoid re-implementing them all in python.

Quaternion2RotVector and its inverse

Quaternion2RotMatrix and its inverse

RotMatrix2RotVector and its inverse

SE3 inverse, the inverse function of each rotation representation

答:
You should decide whether you would like built-in visualization tools or a coordinate frame management system, or something a little more bare-bones.

As stated, transformations.py is bundled with ROS tf and therefore is commonly used around the ROS community. It is primarily useful for constructing raw rotation matrices and homogeneous transformation matrices, from various rotation representations and translation vectors. It's only external dependency is numpy, and it can also be installed through PyPI if you are not using ROS.

When you introduce the concept of coordinate systems / frames, I recommend you study active vs. passive transformations (if you are not already familiar) and seek out a package which makes this distinction in the API. Anytime you interact with raw transformation matrices (create, multiply with a vector, etc.), you will need to know which paradigm you are dealing with.

For the above distinction and for visualization options, I would recommend looking at pytransform3d, which also uses numpy, but provides some ROS-like transform tree management, visualization with matplotlib and open3d, as well as excellent documentation (IMO).


问:
I want my stepper motor to switch speed while traveling (not acceleration wise)
I have this project I'm working on where I'll need the speed of the stepper motor to change set speed at a certain distance, I just can't figure out a way to do it. I'm using arduino and a stepper motor, this is the current code.

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

AccelStepper stepper1(AccelStepper::FULL4WIRE, 0, 1, 2, 3);

void setup()
{
    stepper1.setMaxSpeed(200.0);
    stepper1.setAcceleration(400.0);
    stepper1.moveTo(5000);

}

void loop()
{
    // Change direction at the limits
    if (stepper1.distanceToGo() == 0)
    stepper1.moveTo(-stepper1.currentPosition());
    stepper1.run();
</code>```

What I want it to do basically is to first moveTo(2500)  at the current speed 200 then after 2500 I want it to increase speed to 400. After it has moved 5000 it turns and moves back to position but that's implemented already.

答:
You can do what yu want with the standard arduino "Stepper" library.  You are using something called "accelsteper.h".

Using the standard one....

This c-like psudocode will run a motor at some speed for 2000 steps then run and some other speed for 4000 steps and then go back an do it again.

loop{
setspeed(speed1)
step(2000)
setspeed(speed2)
step(4000)
}

Look here for details on the standard stepper library
http://arduino.cc/en/Reference/Stepper

Get this to work FIRST.  then make it nicer if you need to.  for example place a half way between speed in there like this "set speed((speed1+speed2)/2). and run a few steps.


问:
From a linear force actuator that spans multiple links and joints to the equivalent motor torque
I am programming/using a rigid body joint-space dynamics solver for robots in which I want to implement linear force actuators between links with their equivalent torque sources that are 'spanned' by this linear actuator.

Consider the example figure sketch below:

It shows a three bar linkage floating in 3D space. We have a coordinate system $\Psi_0$ in which we can express all vectors. The left and right figure are the same, except for the fact that the linear force actuator in the left figure has been replaced by rotation actuator torques (see following text). Because they are supposed to be equivalent I have added that large 'equals' sign between the two figures.

There is a linear force actuator between points $\mathbf{p}_1$ and $\mathbf{p}_2$ that effectively pulls these points towards each other. This actuator delivers a force with size $F$, which is drawn along the dashed red line as $\mathbf{F}_1$ in point $\mathbf{p}_1$ and as $\mathbf{F}_2$ in point $\mathbf{p}_2$, in opposite direction, due to Newton's third law.

The three bar linkage has two passive single-DOF joints whose locations in 3D-space are given by $\mathbf{j}_1$ and $\mathbf{j}_2$. These joints rotate around their axis of rotation $\hat{\mathbf{u}}_1$ and $\hat{\mathbf{u}}_2$, given with unit length. The joint angles $q_1$ and $q_2$ show the angle in the respective joint.

Problem: I want to know the equivalent torque vectors $\mathbf{T}_1$ and $\mathbf{T}_2$ that would result from the linear force actuator $\mathbf{F}$. These torque vectors would give me the exact same dynamical behavior (I am not interested in joint constraint forces).

I know that the locations of $\mathbf{p}_1$, $\mathbf{p}_2$, $\mathbf{j}_1$, $\mathbf{j}_2$ are known, that vectors $\hat{\mathbf{u}}_1$ and $\hat{\mathbf{u}}_2$ are known, $\mathbf{F}_1=-\mathbf{F}_2$ is known, and that $q_1$ and $q_2$ are known. All vectors are numerically expressed in frame $\Psi_0$.

Attempt: Simpler case: If the middle link between $\mathbf{j}_1$ and $\mathbf{j}_2$ would be missing, then we have only a single revolute joint $\mathbf{j}_1$. Then I would calculate the equivalent torque vector to be:
\begin{align}
\mathbf{T}_1 &= \left(\left((\mathbf{p}_1-\mathbf{j}_1)\times\mathbf{F}_1\right)\cdot\hat{\mathbf{u}}_1\right)\hat{\mathbf{u}}_1 \\
&= \left(\left((\mathbf{p}_2-\mathbf{j}_2)\times-\mathbf{F}_2\right)\cdot\hat{\mathbf{u}}_1\right)\hat{\mathbf{u}}_1
\end{align}
Where $a\times b$ is the 3D cross-product, and $a\cdot b$ is the dot-product. The torque applied by the actuator is simply the length of the torque vector.

General case: I now get pretty confused when the linear actuator spans one or more extra links and joints between the attachment points (imagine $n$ joints with arbitrary rotation direction, and $n-1$ links between the two final links that have $\mathbf{p}_1$ and $\mathbf{p}_2$ attached).
I looked into what's explained here, but I need a method that works directly with numerical values for the vector elements for all vectors that I know.

Edit: am I just confusing myself and is it nothing more than Jacobian transpose (the Jacobian from joint-space to e.g. $\mathbf{p}_2$)? I'd rather not construct Jacobians for all attachment points.

答:
Yes, it is the Jacobian transpose.

The Shadmehr site you cited has been reorganized. The "Forces and torques" chapter is linked to by the lab's publication page.

The F&T chapter has the usual misleadingly concise expression for the Jacobian (unnumbered, following Eq. 9). It uses derivatives of Eq. 9, itself a nasty expression to work with by hand. Then, as they say, use $J$ in their Eq. 6. $\tau=-J^T f$ which gives exactly what you wanted, an expression for the torques in terms of the force.

Unfortunately, your system is more complicated than their planar system. Yours has an additional angle (not identified in your sketch), which I'll call $\phi$. It is the dihedral angle between two planes, one defined by the two links at your j1, the other by the links at j2. I think the analysis leading to their Eq. 9 could be extended to include that additional angle. The resulting expression will be even nastier, with sin's and cos's of $\phi$ multiplying those of the $\theta$s already there. Fortunately, $\phi$ is a parameter of your system, like the lengths of the links, so you don't need to find $\phi$ derivatives too.

When it comes to the numerics, there are three approaches.

Do the $\theta$ derivatives by hand. Ha ha.

Use a symbolic algebra program. Good luck getting the result coded correctly.

Use automatic differentiation methods developed recently by our machine learning colleagues.

It has been four years since your question was posted. I hope you graduated successfully!


答:
As your application does not need on-the-fly sync between the devices, running an SW-based time lag estimation is also one way to do it.

The following materials might help to start with.

https://github.com/ethz-asl/kalibr

Spatiotemporal camera-LiDAR calibration: A targetless and structureless approach, C Park, P Moghadam, S Kim, S Sridharan, C Fookes

But please be aware that this is one of the most difficult problems when you use a multi-modal sensor.

Title: How to make a synchronized LiDAR/Camera rig for Autonomous Driving research?

Body:
I am just starting a research project at my university involving sensor fusion between a Camera and a LiDAR device to detect and localize cars around our vehicle (bounded boxes around cars in 3D point-cloud). We need a rig having one Camera and one LiDAR to capture data, which we will label manually, and later we will train a deep neural model that takes the advantage of color intensity and depth information for increased accuracy. However, upon basic online search, I found that we'll need a frame synchronized rig for such purpose. I also found that usually LiDAR devices have a sync port for such purpose, but I don't know how I can connect it with a camera so that both devices generate frames capturing the exact same instant in time? Do usual cameras have such a port or do I need to buy a special camera?

Thanks.


问:
Is VIO a good solution for autonomous drone navigation at higher altiutues (200+ms)?
When it comes in indoor scenarios, VIOs may provide good performance, but when it comes to outdoor environments and high altitudes without proper GPS coverage/stability, are current VIOs capable of providing robust localization/navigation? are they used commercially at such height/environments?

I want to know if there are better solutions other than VIOs for autonomous navigation that are robust. I was looking at other approaches for autonomous navigation and found about depth-camera, laser range finders and optitrack? solutions as well, but it seems these solution only work at low altitudes. So VIO seemed like the only option as you install a cheap monocular/stereo camera and you go on about your business.

However, it seems VIO algorithms are not robust enough and also there is dust/fog and even night that normal RGB cameras cant handle.

So if I want to build a robust navigation system what should I turn to? depth cameras? or RGB cameras and estimate the depth myself? whats the current solution thats being used today?

答:
are current VIOs capable of providing robust localization/navigation?-> It might work but remember that the camera always looks down to find a feature to track. Old generation of vacuum robots is looking up and tracking the features on the ceiling. Yours is the opposite case. Also, you should expect the translation estimation from VIO might not be as accurate as the ground one. This is because at that high altitude a small movement does not make much movement on the pixel side. This will affect the IMU bias observability too. This might be overcome if you combine a zoom lens(for translation) with wide lens(for orientation).

So if I want to build a robust navigation system what should I turn to? -> I think making a multi modal sensor set(GPS+IMU+LiDAR+Camera) is a solution for you.


问:
Vex Robotics- Falcon 500
I need two motors powerful enough to accelerate a 200 lbs robot at 10 ft/s^2. From doing the calculations, I know I am looking for a two 430 watt motors. I found the Falcon 500 motor on vex robotics website. But those motors are only 3 inches in length and weigh 1 pound. They are advertised to be able to provide the necessary 430 watts, but it is extremely hard to believe this due to its small frame and weight.

If anyone has had first hand experience with the Falcon 500 and used them to power and maneuver heavy robots, please tell me if two falcon 500 motors would be sufficient for this project.

答:
It depends on how many seconds you want to run the motors, and how many minutes you want to wait between runs.

Those motors are certainly not going to operate at 430 watts continuously. At 40 amps, the "brochure" suggests a 32C rise at 150 seconds, and the motor data shows an overtemp shutdown at 260 seconds.

They are targeted at the FIRST community, so if you don't abuse them I'd expect them to work really well, for a few months at least.

Title: Vex Robotics- Falcon 500


问:
Humanoid balancing
I'm currently working on Humanoid robot. I've solved the Forward & Inverse Kinematic relations of the robot, and they turn out to be fine. Now I want to move onto Walking. I've seen tons of algorithms & research papers but none of them make the idea clear. I understand the concept of ZMP & what the method tries to do, but I simply can't get my head around all the details that are required to implement it in a real robot. Do I have to plan my gait & generate the trajectories beforehand, solve the joint angles, store them somewhere & feed it to the motors in real-time? or Do I generate everything at run-time(a bad Idea IMO)? Is there a step-by-step procedure that I can follow to get the job done? or Do I have to crawl all my way through those Research papers, which never make sense(at least for me).</p>

答:
They are two main approachs for humanoid robot walking. Practically it depends on the physical capabilities of your robot and what you want to achieve.

If your robot is able to sustain strong impact and you aim at cyclic walking on uneven terrain  I would recommend to try the Hybrid Zero Dynamics approach. The current best example is the Cassie or the Digit robot from Agility Robotics see [1] for a recent example.

For a more fragile robot, the usual step to make a humanoid walking on flat ground without obstacle is to:

Plan foot steps.

Generate a ZMP trajectory from the foot steps, and the foot trajectories (usually polynomials with zero speed, zero acceleration at the beginning/end and an intermediate vertical point).

Generate the CoM trajectory from the ZMP trajectory.

Use your IK solver to follow the CoM trajectory and the foot trajectories. An inverse dynamic solver would be better, but it is more difficult to achieve.

Once you have a first version of this working in simulation you can try to develop a stabilizer. It assumes usually an estimator of your robot's root attitude (often the waist), and a feedback on the Divergent Component of Motion of your robot. For a good overview on this topic I would recommend the following paper [2].

From this, they are then various approachs depending on what you want to do: multiple contacts, uneven terrain, manipulation, co-working with a human.

[1] Inverse Dynamics Control of Compliant Hybrid Zero Dynamic Walking, Jenna Reher and Aaron D. Ames, ICRA 2021, https://arxiv.org/pdf/2010.09047.pdf

[2] Stair Climbing Stabilization of the HRP-4 Humanoid Robot using Whole-body Admittance Control, Caron et al, ICRA 2018, https://arxiv.org/abs/1809.07073

Title: Humanoid balancing


问:
DC motor direct loading
I found many tutorials and online calculators for the selection of dc motor to drive wheel, I understood how the torque affect the driving of wheel.

But what happen when I change the orientation of motor and load? What is the main criteria for a dc motor to work when I want to rotate a plate which is vertically mounted on motor's shaft, when the motor is placed vertically also (as shown in the picture)?

I am not an engineering student so please provide me an answer as simple as possible.

答:
Are you asking about the gyroscopic effect?  What they call "gyroscopic precession".  Then Google that term "gyroscopic precession" and you can find some ways to calculate it.

But in practical terms your 0.5Kg wheel moving at 1 rpm will show very lift of this effect unless the diameter is huge.  For normal size wheels at 1 rpm you can ignore this.

Title: DC motor direct loading


问:
URDF for a real robot
I have been making robots in ROS/Gazebo simulation before, which makes use of the URDF to understand the robot model, different frames and transformations. For a real robot, do I need a URDF as well? I have a robotic platform with a lidar and an IMU. Do I need to create links in the URDF file in order to define respective frames?
The confusing thing is that how do I associate a frame within ROS with a real sensor mounted on the system and then perform static and dynamic transforms?</p>

答:
For a real robot, do I need a URDF as well?

It is not necessary to create a URDF for a real robot, but it's highly recommended. Many of the tools are designed to leverage it. For example the robot_state_publisher will take in joint states and generate the tf transform tree using the mapping provided by the URDF. All the motion planning and other libraries are designed to query the URDF to get many parameters of the robot.

Do I need to create links in the URDF file in order to define respective frames

That's the recommended way. You can do it manually too, but that's going to create more work in the long run.

The confusing thing is that how do I associate a frame within ROS with a real sensor mounted on the system and then perform static and dynamic transforms?

The sensor driver should take an argument of it's frame_id which you should match to the frames defined in the URDF. And with the robot_state_publisher active you'll have all the necessary transforms available.

Title: URDF for a real robot


问:
Alternative for a 18650 2200mAh 3.7V Li-on battery
I'm building my first robotic car (Elegoo Smartbot Car Kit) but I'm struggling with which batteries to buy.
The kit itself doesn't seem to say too much other than they're 18650 LiPo batteries which I've now learned 18650 refers to the dimensions of the battery and LiPo is Lithium Polymer.

I watched an assembly video and managed to pause right on the spot where the guy shows the batteries: ICR  18650 10/17 2200mAh 3.7V and the last part is difficult to read but it looks like 8.14Wh.

I've looked online in South Africa and can't find an exact match for these batteries.

I don't know much about batteries/electrics so I'm not sure what alternatives I can use and when I'll be looking at damaging the electronics.

I seem to be able to get anywhere from 2000mAh to 4200mAh all 18650 and 3.7V. Some of these are Li-Po and some are Li-On. Some research lead me to believe that Li-Po and Li-On are compatible, the difference being that Li-On is perhaps better?

My understanding is that mAh (milliamp hours) refers to the ability of the battery to hold a charge... Not exactly sure what that means though.

Is it safe to buy an 18650 battery with a higher mAh rating? I.e. it won't damage the car?

Is my understanding about Li-On and Li-Po correct?

答:
Welcome to Robotics, Jacques. Robot cars are a fun way to start down the <s>slippery slope</s> attractive activity of robotics. You won't run out of things to aspire to.

Is higher capacity safe? Yes.

The term "mAh" (milliAmp-hours) measures how much charge the battery can hold. (Literally charge -- mAh could be converted to Coulombs.) The general idea is that a 2200 mAh battery could provide 2200 mA for 1 hour, or 220 mA for 10 hours, etc. Whether you could actually get that much current for that long depends on charging profile, discharge profile, "memory" effects, temperature, lifetime, even (unfortunately) how comfortable the manufacturer's marketing people are with lying to us.

At the same voltage rating (3.7 V in your case) a larger capacity is not a problem. It'll just let your device run longer between charges.

The phrase "hold a charge" is somewhat ambiguous. There's capacity, how much charge you could put in and take out, as above. Then there's the ability to retain a charge. Batteries discharge themselves over time, different types at different rates.

Are Li-Po and Li-Ion compatible? Yes.

They are largely equivalent for your purposes. Each has its pluses and minuses. You could do a web search for "difference between lithium ion and polymer" and weigh which is best for your use. Cost and availability can be important considerations. Only you can judge those tradeoffs, and this Q&A site doesn't do "shopping" questions anyway.


问:
How is Screw Theory used in Robotics when you can do everything with regular kinematics?
I was seeing some Robotics lectures and a word which keeps coming up is "Screw Theory", on looking it up, I find that everything you can do with it practically, you can do with vector algebra itself.

So, in what exact way does the usage of screw theory simplfy the study of robotics?

答: Screw theory is another way to describe the motion of rigid bodies. The difference between this theory and the homogenous transformation matrix (i.e. standard approach) is the fact that with the latter, the translation and rotation are carried out separately whereas with the former the translational and rotational movement is carried out simultaneously. Both will provide same final equations.</p>


答:
Why use polar or spherical coordinates when you can use Cartesian coordinates for everything? Why use Laplace transforms to solve differential equations?

I think most of the challenge in higher math is choosing the representation that makes the problem easy (or easier) to solve.

Title: How is Screw Theory used in Robotics when you can do everything with regular kinematics?

Body:
I was seeing some Robotics lectures and a word which keeps coming up is "Screw Theory", on looking it up, I find that everything you can do with it practically, you can do with vector algebra itself.

So, in what exact way does the usage of screw theory simplfy the study of robotics?


问:
Is this motion possible for this manipulator?
I'm currently solving the exercises of Modern Robotics book. I came across this problem but it is not clear to me how to approach it. I computed the analytic Jacobian which is

$$
\begin{bmatrix} \dot{x}\\\dot{y} \\ \dot{z}\end{bmatrix} =
\left(\begin{array}{ccc} L\,s_{\theta_1}\,s_{\theta_2}\,s_{\theta_3}-L\,c_{\theta_2}\,s_{\theta_1}-L\,c_{\theta_2}\,c_{\theta_3}\,s_{\theta_1}-L\,c_{\theta_1} & -L\,c_{\theta_1}\,\left(s_{\theta_2}+\sin\left(\theta_2+\theta_3\right)\right) & -L\,c_{\theta_1}\,\sin\left(\theta_2+\theta_3\right)\\ L\,c_{\theta_1}\,c_{\theta_2}-L\,s_{\theta_1}+L\,c_{\theta_1}\,c_{\theta_2}\,c_{\theta_3}-L\,c_{\theta_1}\,s_{\theta_2}\,s_{\theta_3} & -L\,s_{\theta_1}\,\left(s_{\theta_2}+\sin\left(\theta_2+\theta_3\right)\right) & -L\,s_{\theta_1}\,\sin\left(\theta_2+\theta_3\right)\\ 0 & L\,\left(c_{\theta_2}+\cos\left(\theta_2+\theta_3\right)\right) & L\,\cos\left(\theta_2+\theta_3\right) \end{array}\right)
\begin{bmatrix} \dot{\theta}_1\\\dot{\theta}_2 \\ \dot{\theta}_3\end{bmatrix}
$$
At zero configuration $\theta_1=\theta_2=\theta_3=0.$, we get
$$
\begin{bmatrix} \dot{x}\\\dot{y} \\ \dot{z}\end{bmatrix} =
\begin{bmatrix} -L&0&0\\2L&0&0\\0&2L&L\end{bmatrix}
\begin{bmatrix} \dot{\theta}_1\\\dot{\theta}_2 \\ \dot{\theta}_3\end{bmatrix}
$$
The analytic Jacobian is not invertible at this configuration. My conclusion is the motion is not possible but it seems to me according to the question, there is a possibility. Any suggestions?

答: Here is a top view of the robot, and the end effector velocity if $\dot{\theta}_1 > 0$:


问:
In which frame this wrench is expressed?
Take a look at this question below. The relationship between torques and wrenches are

$$
\tau = J_b^T(\theta) \mathcal{F}_b = J_s^T(\theta) \mathcal{F}_s
$$
where the subscripts $\{b,s\}$ indicate in which frame the Jacobian matrix and the wrench are expressed. I can compute $J_b^T(\theta),J_s^T(\theta)$ but I'm not able to understand from the question in which frame the wrench expressed but I suspect it is in the frame $\{s\}$ since it says in the $\hat{x}_s$-direction.

答:
Edited Answer

From the question, it seems the loading is applied at the end effector but its line of action along the $x_s$ direction.

I do prefer to resolve everything on the world inertial frame which makes all the coordinate transformations unnecessary.

The force

$$ \boldsymbol{F} = \pmatrix{5 \\ 0\\ 0} \text{N}$$

going through the first joint has a loading wrench is

$$ \mathcal{F}_s = \begin{bmatrix} \boldsymbol{F} \\  \boldsymbol{M} + \boldsymbol{r} \times \boldsymbol{F} \end{bmatrix} = \begin{bmatrix} \boldsymbol{F} \\  \hat{0} \end{bmatrix} $$

Similarly for the kinematics

The unit twists of the joints (the columns of the jacobian) are defined based on their locations

$$ \begin{aligned}
 \boldsymbol{r}_1 & = \pmatrix{ 0 \\0 \\ 0 }\text{m} & \mathcal{s}_1 & = \begin{bmatrix} \boldsymbol{r}_1 \times \boldsymbol{\hat{z}} \\ \boldsymbol{\hat{z}} \end{bmatrix} \\
 \boldsymbol{r}_2 & = \pmatrix{ 1 \\ 0 \\ 0 }\text{m} & \mathcal{s}_2 & = \begin{bmatrix} \boldsymbol{r}_2 \times \boldsymbol{\hat{z}} \\ \boldsymbol{\hat{z}} \end{bmatrix} \\
 \boldsymbol{r}_3 & = \pmatrix{ 1 + \tfrac{1}{\sqrt{2}} \\ \tfrac{1}{\sqrt{2}} \\ 0 }\text{m} & \mathcal{s}_3 & = \begin{bmatrix} \boldsymbol{r}_3 \times \boldsymbol{\hat{z}} \\ \boldsymbol{\hat{z}} \end{bmatrix} \\
\end{aligned}$$

and finally the Jacobian

$$ \mathcal{J}_s = \begin{bmatrix} \mathcal{s}_1 & \mathcal{s}_2 & \mathcal{s}_3 \end{bmatrix}$$

This makes the generalized forces

$$ \boldsymbol{\tau} = \mathcal{J}_s^\top \mathcal{F}_s =  \pmatrix{ 0 \\ 0 \\ \tfrac{5}{\sqrt{2}} } $$

Let's see, if we can explain the results. From the wording of the question, the line of action of the force goes through the first joint, along the x-axis and thus it seems perfectly reasonable that no force component will cause any equipollent torque on the first two joints for them to counteract.

Thus it seems reasonable that only the 3rd joint is "energized" and the direction of the torque is positive. This is consistent with the required wrench acting counter-clockwise with respect to this joint.

This torque would try to unwind the mechanism which means since it is fixed on one end, the end effector will push to the right.


答:
Screw theory greatly simplifies the notation and expressions used. This allows for complex systems to be considered. As an added bonus it completely eliminates the need to specify which coordinate frame, and on what position things are summed up.

Example

Consider the use case of two connected rigid bodies floating in space

Definitions

At some instant, in time the locations of each body's center of mass are described by the vectors $\vec{r}_1$ and $\vec{r}_2$. The velocities and accelerations are respectively $\vec{v}_1$ and $\vec{v}_2$ and $\vec{a}_1$ and $\vec{a}_2$ also at the center of mass. Rotationally they move with $\vec{\omega}_1$ and $\vec{\omega}_2$ and their accelerations are $\vec{\alpha}_1$ and $\vec{\alpha}_2$.

The mass properties of the two bodies are described by the scalar masses $m_1$ and $m_2$ and the mass moment of inertia tensors $\mathrm{I}_1$ and $\mathrm{I}_2$ summed at each center of mass but using the inertial frame basis vectors.

The revolute joint between the two bodies is located at $\vec{r}_A$ at this instant, and the relative rotation axis is $\hat{z}_A$ with joint speed $\dot{q}_A$. The joint motor has specified torque $\tau_A$.

The equations of motion of this system need to consider not only the force and moment sums but also the kinematics between the bodies, and the joint condition.

Let us look at the kinematics first

Vector Notation

Here are all the equations needed to describe the relative motion between the bodies

$$\begin{aligned}\vec{\omega}_{2} & =\vec{\omega}_{1}+\hat{z}_{A}\dot{q}_{A}\\
\vec{v}_{2} & =\vec{v}_{1}+\vec{\omega}_{1}\times\left(\vec{r}_{A}-\vec{r}_{1}\right)+\vec{\omega}_{2}\times\left(\vec{r}_{2}-\vec{r}_{A}\right)\\
\vec{\alpha}_{2} & =\vec{\alpha}_{1}+\hat{z}_{A}\ddot{q}_{A}+\vec{\omega}_{1}\times\hat{z}_{A}\dot{q}_{A}\\
\vec{a}_{2} & =\vec{a}_{1}+\vec{\alpha}_{1}\times\left(\vec{r}_{A}-\vec{r}_{1}\right)+\vec{\omega}_{1}\times\left(\vec{\omega}_{1}\times\left(\vec{r}_{A}-\vec{r}_{1}\right)\right)+\vec{\alpha}_{2}\times\left(\vec{r}_{2}-\vec{r}_{A}\right)+\vec{\omega}_{2}\times\left(\vec{\omega}_{2}\times\left(\vec{r}_{2}-\vec{r}_{A}\right)\right)
\end{aligned}
$$

I count 4 vector equations (12 component equations). Ouch, my eyes hurt looking at these.

Screw Notation

Here are the exact same equations but with screw notation

$$\begin{aligned}\boldsymbol{v}_{2} & =\boldsymbol{v}_{1}+\boldsymbol{s}_{A}\dot{q}_{A}\\
\boldsymbol{a}_{2} & =\boldsymbol{a}_{1}+\boldsymbol{s}_{A}\ddot{q}_{A}+\boldsymbol{v}_{1}\times\boldsymbol{s}_{A}\dot{q}_{A}
\end{aligned}$$

I count 2 screw equations (12 component equations). What is neat is that the expressiveness offers an explanation of what is going on visually. For example,

$$ \underbrace{\boldsymbol{v}_{2}}_{\text{motion of 2}}  = \underbrace{\boldsymbol{v}_{1}}_{\text{motion of 1}}+\underbrace{\boldsymbol{s}_{A}\dot{q}_{A}}_{\text{joint motion}} $$

This is not the place to derive all of the above, but suffice to say that the entire motion of body 1 is described by one quantity defined as

$$ \boldsymbol{v}_1 = \begin{bmatrix} \vec{v}_1 + \vec{r}_1 \times \vec{\omega}_1 \\ \vec{\omega}_1 \end{bmatrix} $$

The above encompasses the translational and rotational motion as well as the location of the instantaneous axis of rotation (and thus the geometry of the kinematics).

Now let us look at the equations of motion

Vector Notation

Consider there is a force moment pair $(\vec{F}_A,\vec{M}_A)$ applied in equal and opposite terms between the bodies the equations of motion are

$$\begin{aligned}-\vec{F}_A & =m_{1}\vec{a}_{1}\\
-\left(\vec{r}_{A}-\vec{r}_{1}\right)\times\vec{F}_A-\vec{M}_A & =\mathrm{I}_{1}\vec{\alpha}_{1}+\vec{\omega}_{1}\times\mathrm{I}_{1}\vec{\omega}_{1}\\
\vec{F}_A & =m_{2}\vec{a}_{2}\\
\left(\vec{r}_{A}-\vec{r}_{2}\right)\times\vec{F}_A+\vec{M}_A & =\mathrm{I}_{2}\vec{\alpha}_{2}+\vec{\omega}_{2}\times\mathrm{I}_{2}\vec{\omega}_{2}
\end{aligned}
$$

Again, 4 vector equations (12 component equations). The sum of moments is always going to be a complex affair in dynamics as everything acting contributes an active moment and an equipollent moment at the same time.

Screw Notion

Forces and moments are combined in wrenches making the equations of motion satisfyingly simpler

$$\begin{aligned}-\boldsymbol{f}_A & ={\bf I}_{1}\boldsymbol{a}_{1}+\boldsymbol{v}_{1}\times{\bf I}_{1}\boldsymbol{v}_{1}\\
\boldsymbol{f}_A & ={\bf I}_{2}\boldsymbol{a}_{2}+\boldsymbol{v}_{2}\times{\bf I}_{2}\boldsymbol{v}_{2}
\end{aligned}
$$

I count 2 screw equations (12 component equations). Again there is an expressiveness here you see exactly what goes into each equation like a free body diagram. For example, the force pair from the joint acts in equal and opposite measures on the two bodies. This wrench is defined as

$$ \boldsymbol{f}_A = \begin{bmatrix} \vec{F}_A \\ \vec{M}_A + \vec{r}_A \times \vec{F}_A \end{bmatrix} $$

Joint Condition

Finally, the joint torque $\tau_A$ must be considered.

Vector notation

$$ \tau_A  =  \hat{z}_A ^\top \vec{M}_A $$

This is a scalar equation.

Screw notation

$$ \tau_A = \boldsymbol{s}_A ^\top \boldsymbol{f}_A $$

This is also a scalar equation.

Try to solve for the joint acceleration

With vector notation

You have 12+12+1 = 25 equations and 19 unknowns (6 DOF of motion for each body, 3 constraint forces, 3 constraint moments, 1 joint motion). So there the problem is overconstrained and some vector equations need to be disregarded to be solved.

With screw notation

You can reduce the system to 7 equations to be solved with 7 unknowns (6 motion DOF of first body plus joint motion).

$$ \begin{gathered}\tau_{A}=\boldsymbol{s}_{A}^{\top}\boldsymbol{f}_{A}\\
\tau_{A}=\boldsymbol{s}_{A}^{\top}\left({\bf I}_{2}\boldsymbol{a}_{2}+\boldsymbol{v}_{2}\times{\bf I}_{2}\boldsymbol{v}_{2}\right)\\
\underbrace{\tau_{A}=\left(\boldsymbol{s}_{A}^{\top}{\bf I}_{2}\boldsymbol{s}_{A}\right)\ddot{q}_{A}+\boldsymbol{s}_{A}^{\top}\left({\bf I}_{2}\left(\boldsymbol{a}_{1}+\boldsymbol{v}_{1}\times\boldsymbol{s}_{A}\dot{q}_{A}\right)+\boldsymbol{v}_{2}\times{\bf I}_{2}\boldsymbol{v}_{2}\right)}_{\text{1 equation, unknowns }\boldsymbol{a}_1\,\ddot{q}_A}\\
\\
0={\bf I}_{1}\boldsymbol{a}_{1}+\boldsymbol{v}_{1}\times{\bf I}_{1}\boldsymbol{v}_{1}+{\bf I}_{2}\boldsymbol{a}_{2}+\boldsymbol{v}_{2}\times{\bf I}_{2}\boldsymbol{v}_{2}\\
\underbrace{0=\left({\bf I}_{1}+{\bf I}_{2}\right)\boldsymbol{a}_{1}+{\bf I}_{2}\boldsymbol{s}_{A}\ddot{q}_{A}+\boldsymbol{v}_{1}\times{\bf I}_{1}\boldsymbol{v}_{1}+\boldsymbol{v}_{2}\times{\bf I}_{2}\boldsymbol{v}_{2}+{\bf I}_{2}\left(\boldsymbol{v}_{1}\times\boldsymbol{s}_{A}\dot{q}_{A}\right)}_{\text{6 equations, unknowns }\boldsymbol{a}_1\,\ddot{q}_A}\\
\\
\end{gathered}
$$

Summary

So screw notation is more expressive, more compact, leads easier to solver algorithms, and can combine complex connected structures with at least an order of magnitude less difficulty.

Addendum

Recently I look at a rigid body collision algorithm and in vector notation, you calculate the reduced mass of the system along the contact normal as

$$ m_{\rm reduced} = \left[\tfrac{1}{m_{1}}+\tfrac{1}{m_{2}}+\left(\hat{n}\times\vec{c}_{1}\right)\cdot {\rm I}_{1}^{-1}\left(\hat{n}\times\vec{c}_{1}\right)+\left(\hat{n}\times\vec{c}_{2}\right)\cdot {\rm I}_{2}^{-1}\left(\hat{n}\times\vec{c}_{2}\right)\right]^{-1} $$

reference https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf

link to [Physics.SE] answer https://physics.stackexchange.com/a/686727/392

But in screw notation, the above becomes much simpler (to remember and to write)

$$ m_{\rm reduced} = \frac{1}{ \boldsymbol{n}^\top (\mathbf{I}_1^{-1} + \mathbf{I}_2^{-1})  \, \boldsymbol{n} } $$

And again, you can see that the two masses are added like resistors in parallel (addition of inverse inertia) and then projected through the contact normal. This kind of insight cannot be achieved with vector notation.


问:
Peter Corke Robotics Matlab Toolbox the problem of inverse dynamics in the Franka Emika Panda model
I have the following question about the Franka Emika Panda dynamic model. In case I perform the solution of the inverse dynamics problem using Robotics Toolbox (Peter Corke) I get different results compared to the solution using the dynamic model published in the following article (for the simplicity I have neglected the friction and gravitational component in both cases):
C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano, A. De Luca, 'Dynamic Identification of the Franka Emika Panda Robot With Retrieval of Feasible Parameters Using Penalty-Based Optimization'. IEEE RA-L, 2019. The dynamic model is available here: https://github.com/marcocognetti/FrankaEmikaPandaDynModel/tree/master/matlab/dyn_model_panda

According to the available information, the dynamic model of Panda in Robotics Toolbox is created based on the kinematic and dynamic parameters available in the mentioned paper, so why does the use of Robotics Toolbox show different results, see attached figure?
I am attaching a modified Matlab script demonstrating the different behavior.
Thank you in advance for the answer.
enter link description here

enter link description here

答: @Daniel kajzr! Have you solved the problem? I think you may have forgotten to add tau_f to the inverse dynamic equation of the robot. Because the last three joints are mainly used to overcome the friction torque, you have not calculated the friction torque, so the error of the last three joints is relatively large.</p>


问:
Stabilizing a quadcopter with optical flow
My quad copter can balance itself in the air using data collected from mpu6050. With the sonar sensor, it can hover at a specific height, but it  moves on the horizontal plane in a random direction.  If i put an object below it, it will ascend to keep the distance between the sonar senor and the object.

Now i want to make it have the ability to hover stably. Is it possible to add a downward-facing camera to calculate the speed of optical flow in order to keep it hovering on the same point in the horizontal plane?  Could I use a forward-facing camera to stabilize its vertical speed?

答:
Using a so-called optical flow sensor is the best way to help with holding the horizontal (i.e. in X-Y plane) position. I don't see any reason why you couldn't do the same for vertical control, although a sonar is probably easier and cheaper to use for this (likewise, if you are indoors, you could use 2 sonars for the horizontal position as well)

People used to hack the sensors of optical mice to achieve this:

http://makezine.com/2007/12/15/using-an-optical-mouse-for-rob/
http://areciv.com/index.php?aid=18
http://home.roadrunner.com/~maccody/robotics/croms-1/croms-1.html
http://home.roadrunner.com/~maccody/robotics/mouse_hack/mouse_hack.html

Some ready-made sensors are available: http://www.buildyourowndrone.co.uk/Optical-Flow-Sensor-p/op-fs.htm

Some explanation on how to use such device with an Arduino can be found here:https://code.google.com/p/arducopter/wiki/AC2_OptFlow

Nowadays, better sensors are out there (but much more expensive) that works in low-light conditions (i.e. indoors) and are even combined with a sonar (as it makes senses to hold altitude in order to get a better/stable optical reading for XY positioning), such as this one:

http://store.3drobotics.com/products/px4flow


问:
robot_localization EKF node does not publish on /odometry/filtered (ROS2 Foxy)
I am using ROS2 Foxy and Gazebo 11 in Ubuntu 20.04. I have a URDF description of a mobile robot that uses 4 wheels for mecanum drive. Using the robot_localization package, I am creating an EKF node that subscribes to the /wheel/odometry topic, to which the mecanum drive node publishes the odometry data. Also, the EKF node is subscribed to data published by IMU. However, the EKF node is not publishing any data on /odometry/filtered topic. The node does publish some data on /diagnostics topic, but that does not seem to help me.
Here's my EKF config file-

```<code>ekf_filter_node:
    ros__parameters:

        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: false
        publish_acceleration: false
        publish_tf: true
        reset_on_time_jump: true
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

        odom0: wheel/odometry
        odom0_config: [true,  true,  false,
                       false, false, true,
                       true, true, false,
                       false, false, true,
                       true, true, false]
        odom0_queue_size: 2
        odom0_nodelay: false
        odom0_differential: false
        odom0_relative: false

        imu0: imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      true,  true,  true,
                      true, true, true]
        imu0_queue_size: 5
        imu0_nodelay: false
        imu0_differential: false
        imu0_relative: true
        imu0_remove_gravitational_acceleration: true

</code>```

Here are the sample messages from the sensors:-

----IMU----

```<code>header:
  stamp:
    sec: 99
    nanosec: 1000000
  frame_id: base_link
orientation:
  x: -3.2484036403009136e-07
  y: -1.233682290372918e-07
  z: -5.640383185432385e-06
  w: 0.9999999999840328
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: -0.00017136447475541198
  y: 0.00013966822040950878
  z: 4.4918826200876715e-06
angular_velocity_covariance:
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
linear_acceleration:
  x: -0.020305982380950664
  y: -0.005017642374176924
  z: 9.765243743038877
linear_acceleration_covariance:
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003
</code>```

----Gazebo Plugin that provides 4 wheel mecanum drive----

```<code>header:
  stamp:
    sec: 263
    nanosec: 153000000
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.0005238049487633187
      y: -0.00013799493152156435
      z: 0.17620003865237005
    orientation:
      x: -1.5027791139302502e-07
      y: 8.421334553204312e-09
      z: -4.986068853738537e-05
      w: 0.9999999987569447
  covariance:
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
twist:
  twist:
    linear:
      x: 9.15223527049435e-06
      y: 0.00028986655671066065
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 3.0622019018612144e-05
  covariance:
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
</code>```

Here's what the /diagnostics topic gets populated with:-

```<code>header:
  stamp:
    sec: 1642601976
    nanosec: 319066940
  frame_id: ''
status:
- level: "\x02"
  name: 'ekf_filter_node: odometry/filtered topic status'
  message: No events recorded.
  hardware_id: none
  values:
  - key: Events in window
    value: '0'
  - key: Events since startup
    value: '0'
  - key: Duration of window (s)
    value: '9.999977'
  - key: Actual frequency (Hz)
    value: '0.000000'
  - key: Target frequency (Hz)
    value: '0.000000'
  - key: Maximum acceptable frequency (Hz)
    value: '0.000000'
</code>```

Can you guys suggest what I might be missing here?

答:
You need to include sample messages from your sensor inputs, but this sometimes indicates that the topic name, time stamp, or frame_id of the input message is incorrect.

EDIT: removing question about use_sim_time in ROS 2 in response to @Tully's point in the comments.

I was able to run your config from the command line in foxy:

```<code>ros2 run robot_localization ekf_node --ros-args --params-file ekf_params.yaml
</code>```

I manually published messages to imu using the sample you provided, and the EKF was outputting messages on /odometry/filtered.

If I enable use_sim_time, it doesn't publish (it hangs here), presumably because I don't have a time server running. If I manually publish to /clock, I get output again. Can you confirm that your Gazebo instance is publishing to /clock? If it is, then can you possibly provide a bag so I can see if anything else is wrong?

Body:
You need to include sample messages from your sensor inputs, but this sometimes indicates that the topic name, time stamp, or frame_id of the input message is incorrect.

EDIT: removing question about use_sim_time in ROS 2 in response to @Tully's point in the comments.

I was able to run your config from the command line in foxy:

ros2 run robot_localization ekf_node --ros-args --params-file ekf_params.yaml

I manually published messages to imu using the sample you provided, and the EKF was outputting messages on /odometry/filtered.

If I enable use_sim_time, it doesn't publish (it hangs here), presumably because I don't have a time server running. If I manually publish to /clock, I get output again. Can you confirm that your Gazebo instance is publishing to /clock? If it is, then can you possibly provide a bag so I can see if anything else is wrong?