问:
Triangulation: Ceres solver minimizes only the last residual block
I have been attempting to estimate a bunch of 3d coordinates viewed via a multi-view setup with known poses, I have the code running, but the output of ceres seems baffling.

There are 7 image frames/poses, and only the re-projection error with respect to the last pose(7th frame) is close to 0. 2d Reprojections of the ceres-estimated 3dpoint onto rest of the frames(1-6) do not match with the actual pixel measurements.
If I reduce the number of frames, it gives out a different 3d point whose 2d projections again match only with the last frame.

I'm adding code snippets here, can someone just look at the code and give any insights as to why this could be happening?

```<code>
class ReprojectionError {
 public:
  ReprojectionError(
        const Eigen::Matrix<double, 3, 4>& projection_matrix,
        const Eigen::Vector2d& feature)
        : projection_matrix_(projection_matrix), feature_(feature) {}

  template <typename T>
  bool operator()(const T* input_point, T* reprojection_error) const {
        Eigen::Map<const Eigen::Matrix<T, 4, 1> > point(input_point);

        // Multiply the point with the projection matrix, then perform homogeneous
        // normalization to obtain the 2D pixel location of the reprojection.

        const Eigen::Matrix<T, 2, 1> reprojected_pixel =  (projection_matrix_ * point).hnormalized();
        // Reprojection error is the distance from the reprojection to the observed
        // feature location.
        cout << point << endl;
        reprojection_error[0] = feature_[0] - reprojected_pixel[0];
        reprojection_error[1] = feature_[1] - reprojected_pixel[1];

        return true;
  }
  static ceres::CostFunction * Create(const Eigen::Matrix<double, 3, 4>& projection_matrix_,
        const Eigen::Vector2d& feature_) {
            return (new ceres::AutoDiffCostFunction<ReprojectionError, 2,4>
                (new ReprojectionError(projection_matrix_,feature_)));
    }

 private:
    const Eigen::Matrix<double, 3, 4>& projection_matrix_;
    const Eigen::Vector2d& feature_;
};

Eigen::Vector3d Triangulate(std::vector<std::pair
    <Eigen::Matrix<double, 3, 4>,Eigen::Vector2d>> datas) {
        Eigen::Vector4d x;
        x << 0,0,0,1;
        ceres::Problem problem;
        ceres::Solver::Options options;
        ceres::LossFunction* loss= nullptr;
        ceres::Solver::Summary summary;

        for(auto data:datas) {
            problem.AddResidualBlock(ReprojectionError::Create(data.first,data.second),loss,&x[0]);
        }
        ceres::Solve(options,&problem, &summary);
        std::cout << summary.FullReport() << "\n";
        return x.hnormalized();
}

</code>```

Here's the summary:

```<code>Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  4                        4
Residual blocks                             7                        7
Residuals                                  14                       14

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          4.220501e+04
Final                            2.554256e-13
Change                           4.220501e+04

Minimizer iterations                        5
Successful steps                            5
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000060

  Residual only evaluation           0.000774 (5)
  Jacobian & residual evaluation     0.002446 (5)
  Linear solver                      0.000053 (5)
Minimizer                            0.003332

Postprocessor                        0.000002
Total                                0.003394

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 1.091788e-09 <= 1.000000e-08.)
</code>```

答:
Tricky to look at for me, but I'm not a C++ person in general. In looking at it, though, is your ReprojectionError::Create defined correctly?

You have:

```<code>static ceres::CostFunction * Create(const Eigen::Matrix<double, 3, 4>& projection_matrix_,
    const Eigen::Vector2d& feature_) {
        return (new ceres::AutoDiffCostFunction<ReprojectionError, 2,4>
            (new ReprojectionError(projection_matrix_,feature_)));
}
</code>```

But your call to ceres::AutoDiffCostFunction uses <ReprojectionError, 2,4>. In the documentation I found online, I see the following:

Then given this class definition, the auto differentiated cost function for it can be constructed as follows.

```<code>CostFunction* cost_function
= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
    new MyScalarCostFunctor(1.0));              ^  ^  ^
                                                |  |  |
                    Dimension of residual ------+  |  |
                    Dimension of x ----------------+  |
                    Dimension of y -------------------+
</code>```

You have <ReprojectionError, 2, 4>, which would make it look like the dimension of residual is 2, which may explain your problem. Not positive this is the answer, because I don't normally do anything with C++ and have never seen this library before, but I would think that, even if it were a 2x1 vector, you'd still need <ReprojectionError, 2, 2, 1>, right?

There's a note at the bottom of that section that states the following:

WARNING 1 A common beginner’s error when first using AutoDiffCostFunction is to get the sizing wrong. In particular, there is a tendency to set the template parameters to (dimension of residual, number of parameters) instead of passing a dimension parameter for every parameter block. In the example above, that would be <MyScalarCostFunction, 1, 2>, which is missing the 2 as the last template argument.

This seems to echo my gut reaction that the parameters aren't set correctly. I would have expected some sort of error or something, but C++ is a bunch of black magic that gives you enough rope to hang yourself and then shoots you in the face.

Body:
Tricky to look at for me, but I'm not a C++ person in general. In looking at it, though, is your ReprojectionError::Create defined correctly?

You have:

static ceres::CostFunction * Create(const Eigen::Matrix<double, 3, 4>& projection_matrix_,
    const Eigen::Vector2d& feature_) {
        return (new ceres::AutoDiffCostFunction<ReprojectionError, 2,4>
            (new ReprojectionError(projection_matrix_,feature_)));
}

But your call to ceres::AutoDiffCostFunction uses <ReprojectionError, 2,4>. In the documentation I found online, I see the following:

Then given this class definition, the auto differentiated cost function for it can be constructed as follows.

CostFunction* cost_function
= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
    new MyScalarCostFunctor(1.0));              ^  ^  ^
                                                |  |  |
                    Dimension of residual ------+  |  |
                    Dimension of x ----------------+  |
                    Dimension of y -------------------+

You have <ReprojectionError, 2, 4>, which would make it look like the dimension of residual is 2, which may explain your problem. Not positive this is the answer, because I don't normally do anything with C++ and have never seen this library before, but I would think that, even if it were a 2x1 vector, you'd still need <ReprojectionError, 2, 2, 1>, right?

There's a note at the bottom of that section that states the following:

WARNING 1 A common beginner’s error when first using AutoDiffCostFunction is to get the sizing wrong. In particular, there is a tendency to set the template parameters to (dimension of residual, number of parameters) instead of passing a dimension parameter for every parameter block. In the example above, that would be <MyScalarCostFunction, 1, 2>, which is missing the 2 as the last template argument.

This seems to echo my gut reaction that the parameters aren't set correctly. I would have expected some sort of error or something, but C++ is a bunch of black magic that gives you enough rope to hang yourself and then shoots you in the face.


答:
Suggestion by Markus was right :

I believe that your problem lies here:

```<code>for(auto data:datas) {
     problem.AddResidualBlock(ReprojectionError::Create(data.first,data.second),loss,&x[0]);
}
</code>```

data will be a copy of your matrix/point tuple. You're then storing references
to data's members in your newly created residuals. This is not what you want.

Try for(auto& data : datas) instead.

https://groups.google.com/forum/#!msg/ceres-solver/2IRuiVuRE54/wJzSC3X4BQAJ

ContentLicense: CC BY-SA 4.0

Title: Triangulation: Ceres solver minimizes only the last residual block

Body:
I have been attempting to estimate a bunch of 3d coordinates viewed via a multi-view setup with known poses, I have the code running, but the output of ceres seems baffling.

There are 7 image frames/poses, and only the re-projection error with respect to the last pose(7th frame) is close to 0. 2d Reprojections of the ceres-estimated 3dpoint onto rest of the frames(1-6) do not match with the actual pixel measurements.
If I reduce the number of frames, it gives out a different 3d point whose 2d projections again match only with the last frame.

I'm adding code snippets here, can someone just look at the code and give any insights as to why this could be happening?

```<code>
class ReprojectionError {
 public:
  ReprojectionError(
        const Eigen::Matrix<double, 3, 4>& projection_matrix,
        const Eigen::Vector2d& feature)
        : projection_matrix_(projection_matrix), feature_(feature) {}

  template <typename T>
  bool operator()(const T* input_point, T* reprojection_error) const {
        Eigen::Map<const Eigen::Matrix<T, 4, 1> > point(input_point);

        // Multiply the point with the projection matrix, then perform homogeneous
        // normalization to obtain the 2D pixel location of the reprojection.

        const Eigen::Matrix<T, 2, 1> reprojected_pixel =  (projection_matrix_ * point).hnormalized();
        // Reprojection error is the distance from the reprojection to the observed
        // feature location.
        cout << point << endl;
        reprojection_error[0] = feature_[0] - reprojected_pixel[0];
        reprojection_error[1] = feature_[1] - reprojected_pixel[1];

        return true;
  }
  static ceres::CostFunction * Create(const Eigen::Matrix<double, 3, 4>& projection_matrix_,
        const Eigen::Vector2d& feature_) {
            return (new ceres::AutoDiffCostFunction<ReprojectionError, 2,4>
                (new ReprojectionError(projection_matrix_,feature_)));
    }

 private:
    const Eigen::Matrix<double, 3, 4>& projection_matrix_;
    const Eigen::Vector2d& feature_;
};

Eigen::Vector3d Triangulate(std::vector<std::pair
    <Eigen::Matrix<double, 3, 4>,Eigen::Vector2d>> datas) {
        Eigen::Vector4d x;
        x << 0,0,0,1;
        ceres::Problem problem;
        ceres::Solver::Options options;
        ceres::LossFunction* loss= nullptr;
        ceres::Solver::Summary summary;

        for(auto data:datas) {
            problem.AddResidualBlock(ReprojectionError::Create(data.first,data.second),loss,&x[0]);
        }
        ceres::Solve(options,&problem, &summary);
        std::cout << summary.FullReport() << "\n";
        return x.hnormalized();
}

</code>```

Here's the summary:

```<code>Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  4                        4
Residual blocks                             7                        7
Residuals                                  14                       14

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          4.220501e+04
Final                            2.554256e-13
Change                           4.220501e+04

Minimizer iterations                        5
Successful steps                            5
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000060

  Residual only evaluation           0.000774 (5)
  Jacobian & residual evaluation     0.002446 (5)
  Linear solver                      0.000053 (5)
Minimizer                            0.003332

Postprocessor                        0.000002
Total                                0.003394

Termination:                      CONVERGENCE (Parameter tolerance reached. Relative step_norm: 1.091788e-09 <= 1.000000e-08.)
</code>```

Body:
Suggestion by Markus was right :

I believe that your problem lies here:

for(auto data:datas) {
     problem.AddResidualBlock(ReprojectionError::Create(data.first,data.second),loss,&x[0]);
}

data will be a copy of your matrix/point tuple. You're then storing references
to data's members in your newly created residuals. This is not what you want.

Try for(auto& data : datas) instead.

https://groups.google.com/forum/#!msg/ceres-solver/2IRuiVuRE54/wJzSC3X4BQAJ


问:
3D Inverse Kinematics Jacobian Construction
I am following this article: Overview of Jacobian IK

From the link, an example says:

O is a pose vector which represents the initial orientation of every joint

And following this, it gives an example of O:

For example, O would be (45°, 15°, -60°)

Later on it says for the 2D system, the Jacobian can then be constructed as follows:

I now have 2 questions if this was transferred to a 3D system:

How would O be represented?

How would the Jacobian be constructed?

答:
You can represent the O vector as a vector with 3 parameters

$$O = [q_1, q_2, q_3, q_4, q_5, q_6]$$

There is no conceptual difference in how to write the Jacobian matrix. It is the same idea written for both positions ad orientations. For 6 degrees of freedom (assuming your 3D robot has 6 degreees of freedom) the Jacobian will be a $6\times6$ matrix.

We can think of the columns which create the Jacobi Matrix seperatly

$$ J = [ J_1,  J_2,  \cdots J_n ] $$

where

$$J_i = \begin{cases}
\begin{bmatrix}z_{i - 1}\\0_{3\times1}\end{bmatrix} & \text{the $i^\text{th}$ joint is revolute}\\
\begin{bmatrix}z_{i - 1} \times (P - p_{i - 1})\\z_{i - 1}\end{bmatrix} & \text{the $i^\text{th}$ joint is prismatic (linear)}
\end{cases}$$

Where
$z_i$ is the axis of the $i^\text{th}$ joint expressed as a 3D vector, P is the position of the final frame,  in the and $o_i$ is the position of the origin of the $i^\text{th}$ frame. It is applied to a robot structure here.


问:
Help needed sourcing gear/track material for stepper motor
I am completely new to robotics and engineering and it looks as though I have stumbled at the first hurdle when building a prototype.

I am trying to build a curved track upon which a motorised carriage can run. I know that I need a stepper motor and a cog, however for the tracks I can't find anything similar to the picture below:

The youtube video is as follows: Lumia 640XL mach3 step motor curve motion

There is a slight difference in that I need straight sections and 90-degree bends for the tracks, rather than a circle.

Does anyone know what these sections are called, as "tracks" and "runners" doesn't yield anything.

答:
This particular part is called a ring gear, but you're probably not going to find one because the item in your picture is almost certainly custom-made.

The particular article I linked is for engine starter motors, so it has external teeth, but there are internally-toothed ring gears, too.

What you're describing, beyond the picture, does sound like a kind of modular track system, but I'm unaware of any off-the-shelf solutions. You might have luck with timing belts (or toothed belts), but it would probably be easier to get parts custom-fabricated to your exact specification than to try to cobble something together from miscellaneous parts.

There seems to be a good number of off-the-shelf small-scale timing belts like this intended for 3D printers, though. Not affiliated with or recommending the Amazon link, just wanted to give an example.


问:
Detection of linear acceleration vs rotational velocity/acceleration
I am wondering, how do you differentiate between linear acceleration and rotational velocity and acceleration for IMU? A rotational velocity or acceleration will still appear to the accelerometer as a time varying acceleration.</p>

答:
The IMU itself cannot distinguish between "true" linear acceleration and "fictitious" (Coriolis) linear acceleration induced by rotation of the IMU coordinate frame with respect to an inertial frame. You must make that distinction in your choice of models. Your estimator will represent your robot's acceleration in some way in its state, and the model that predicts an IMU reading from your state must take into account this Coriolis effect. As long as you are correctly modeling the IMU given your choice of state, your estimator should figure things out "for you".

Example

Consider for instance an estimator (e.g. a Kalman filter) estimating the IMU's 6DoF pose, along with first and second derivatives, expressed as follows (in the language of Lie groups):
\begin{align}
x(t) &\in \mathrm{SE}(3) \\
\dot x(t)
    &:= \left. \frac{d}{dh} \right|_{h=0} \log\left( x(t + h) x(t)^{-1} \right) \in \mathfrak{se}(3) \\
\ddot x(t) &:= \frac{d}{dt} \dot x(t) \in \mathfrak{se}(3).
\end{align}
where $x(t)$ is the transformation from world-frame coordinates to IMU-frame coordinates. The first derivative takes the form
$$
\dot x(t) = \begin{bmatrix} \omega(t) \\ v(t) \end{bmatrix},
$$
where $\omega(t)$ is the IMU-frame angular velocity and $v(t)$ is the (negative) IMU-frame linear velocity, each expressed in the IMU frame. The second derivative has the form
$$
\ddot x(t) = \begin{bmatrix} q(t) \\ a(t) \end{bmatrix} =
    \begin{bmatrix} \dot \omega(t) \\ \dot v(t) \end{bmatrix}.
$$
In this setup, $a(t)$ is the "true" linear acceleration (not taking into account the Coriolis acceleration); it is not the quantity that the accelerometer measures, which is the second derivative of the IMU frame origin in the world frame, rotated into the IMU frame. Note that if the IMU is revolving around the world frame at a constant angular and linear velocity, then the IMU will read positive acceleration orthogonal to the direction of motion, while $a(t)$ will be identically zero.

In order to perform estimation using the IMU, you will need a model for predicting what value the gyro and accelerometer will take given an arbitrary state. The gyro model will be simple:
$$
\mathrm{gyro}_{\mathrm{pred}}(t) = \omega(t).
$$
But the accelerometer model must take into account the IMU frame's angular velocity:
$$
\mathrm{accel}_{\mathrm{pred}}(t) = -a(t) + \omega(t) \times v(t) + g_{\mathrm{IMU}}
$$
(where $g_{\mathrm{IMU}}$ is the gravity vector rotated into the IMU frame).

Your filter will then distinguish the true from Coriolis acceleration "for you" by producing an estimated state maximizing the likelihood (or something similar) of your observed IMU readings.


问:
In HRI, how is the "uncanny valley" experienced by people on the autism spectrum?
I'm familiar with the idea of the uncanny valley theory in human-robot interaction, where robots with almost human appearance are perceived as creepy. I also know that there have been research studies done to support this theory using MRI scans.

The effect is an important consideration when designing robotic systems that can successfully interact with people. In order to avoid the uncanny valley, designers often create robots that are very far from humanlike. For example, many therapeutic robots (Paro, Keepon) are designed to look like animals or be "cute" and non-threatening.

Other therapeutic robots, like Kaspar, look very humanlike. Kaspar is an excellent example of the uncanny valley, since when I look at Kaspar it creeps me out. However, people on the autism spectrum may not experience Kaspar the same way that I do. And according to Shahbaz's comment, children with autism have responded well to Kaspar.

In the application of therapeutic robots for people on the autism spectrum, some of the basic principles of human-robot interaction (like the uncanny valley) may not be valid. I can find some anecdotal evidence (with Google) that people on the autism spectrum don't experience the uncanny valley, but so far I haven't seen any real studies in that area.

Does anyone know of active research in human-robot interaction for people on the autism spectrum?  In particular, how does the uncanny valley apply (or doesn't it apply) when people on the autism spectrum interact with a humanlike robot?

答:
I test positive for aspergers syndrome on highly validated tests and I can read facial expressions sufficiently well to notice certain patterns in my daily interactions and one of the most noticeable "common" patterns is the disgust/revulsion pattern. The sequencing is pretty simple, I start talking, within 500ms of hearing my talking, the face of the respondent contorts in a most hideous way, and then they immediately start wanting to leave/get-away by whatever means possible.

Researching the uncanny valley, as part of my studies into all things related to aspergers syndrome, I couldn't help but notice the similarity of the reactions I get and the reactions described on wikipedia.

Knowing of the evolutionary significance of autism spectrum disorders (Mainly, less of them work, they earn less money on average and, anecdotally, they tend to have fewer friends/girlfriends than average... and money has its evolutionary underpinnings...), it would seem to me there's probably an evolutionary connection between autism and the uncanny valley concept.

Now, according to the tests, I'm a "light sufferer"(I feel like I suffer plenty, though, BELIEVE me.), so I can read facial expressions in a way that more severely affected apparently can't(?). So, I appear to be right between the extremes of blends-in-and-reads-people-well/doesn't-blend-in-and-can't-read-people-well, getting the worst of both worlds.

ContentLicense: CC BY-SA 3.0


问:
Is it OK to apply too many volts to Actobotics HD motors?
Is it ok to apply too many volts to Actobotics motors?
Too many as in 14.8V. Each motor requires 12V.

Would it damage the motors in any way? and if so, in what way?

答:
Like jsotola said, the datasheet will tell you how much voltage is too much. Regarding damage, the higher voltage can cause too much current in the motor windings and potentially melt the insulation off, shorting the motor internally, or it can overspeed the motor and cause bearing damage.

I know it doesn't sound like a big difference, but 14.8 V is almost 25% more voltage than 12 V.


问:
What is the function of inductors in series between the terminals and the brushes of a DC motor?
I'm repairing (hopefully) a 12 V DC motor (Johnson Electric HC971(2)LG-101).

The motor has a coil in the motor cap between each terminal and the corresponding brush - two coils (inductors) one on each side of the armature electrically speaking. It also has what I think is a resistor on one side and what I think is a (broken) capacitor between the two terminals.

What is the function of the two inductors in series between the terminals and brushes?

I'm sure this info it's out there somewhere but I haven't found it online after several days of searching.

答: those inductors is to block back ac voltage from going back to the DC power source</p>


问:
I'd like to use gesture based input for my robot. What are the pros and cons between the Xtion Live and the Kinect?
As in the title, I'd like to implement gesture recognition on my robot and I'm looking for the pros and cons between the Kinect and the Xtion - and also if there are any other sensible options available.

I'm thinking of the following, but open to other suggestions:

Accuracy

Price

Driver quality

Power draw

答:
The robotics team I lead at NCSU has implemented the ability to drive our holonomic robot using a Leap motion controller. Here's a demo video.

Our codebase is in Python, so we used the Python Leap library, which was quite simple and friendly. Our Leap-related code is only about 150 lines. Our client basically takes pitch, yaw and roll data from the Leap controller and converts it into movements, which it uses to make calls to our bot's API using ZMQ. I can share the code with you if you're interested in additional details (currently in a private GitHub repo pending our contest, but it's open sourced under the BSD two-clause license).


问:
Software Architectures for Robots
When designing a robot (manipulator, house robot, drone, autonomous driving system), what options does one have regarding the general software architecture? Is there even such a thing, as a very high level view on different software architectures?

I have very little clue about software architectures, so hopefully the answers can give an entry point into the topic.

答:
First, start with the systems architecture. You need to design the system considering cost, safety, ease of development.

After that, for each item, main features are defined. This defines the high-level architecture of software elements (platform-side, maintenance computer or controller softwares).

The question is most probably about the main (vehicle) software, which conducts the most important calculations.

The main software architecture depends a lot on what one wants to accomplish. Is it a remotely-operated-vehicle? is it an autonomous system? how autonomous?

After that, software architecture is defined, taking into consideration any dictated or selected data interfaces.

The software architecture functionally resembles the systems architecture, because the main computer interacts with almost all of the elements of the robot.

Sample architecture for a robot racer

There are several studies regarding autonomous vehicle system and software solutions. You can dive deeper into the topic by visiting Google.

Title: Software Architectures for Robots


问:
Inverse Kinematics for 4 legged robots and body translation
The prompt here is to implement the IK on a real robot with constraints. I started off with writing the DH-Table.

$\begin{bmatrix}n & a_{i-1} & \alpha_{i-1} & d_i & \theta_i
\\1 & 0 & 0 & 0 & \theta_1 \\ 2 & a_1 & \frac{pi}{2} & 0 & \theta_2 \\ 3 & a_3 & 0 & 0 & \theta_3 \\ 4 & a_3 & 0 & 0 & 0 \end{bmatrix}$

I've deduced this DH table into its corresponding matrices and solved them for $\theta_1$, $\theta_2$ and  $\theta_3$. ( The math isn't shown here. )

I arrive at these 3 equations.

$$  \theta_1 = tan^{-1}\frac{P_y}{P_x}$$
$$  \theta_2 = ....$$
$$ \theta_3  = ...$$

Now the question is to place the legs on the edge of the robot's body.

1.) Is there material out there that explains how the frame can be translated from the center of the body to the legs?

2.) Is there any material out there that explains Body IK and how it can be implemented for Roll Pitch and Yaw? and how to develop gaits for a robot like this in terms of code?

any information, tips, and suggestions are much welcome. I've also placed a python script where the user can control the position of the leg using IK.

```<code>import math
import numpy as np
from mpl_toolkits import mplot3d
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider, RadioButtons

LINK_1 = 60 + 60
LINK_2 = 110
LINK_3 = 120

"""
Place simulation on leg and check. Makes sure the image is inverted on the real leg.
"""

def constrain(val, min_val, max_val):
    return min( max_val, max(min_val, val) )

class Joint:
    def __init__(self, link_1, link_2, link_3):
        self.link_1 = link_1
        self.link_2 = link_2
        self.link_3 = link_3

        self.theta_1 = -1
        self.theta_2 = -1
        self.theta_3 = -1

def FK( joint ):
    T0_1 = np.array([
        [math.cos(joint.theta_1), -math.sin(joint.theta_1), 0, 0],
        [math.sin(joint.theta_1),  math.cos(joint.theta_1), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1 ]
        ])
    T1_2 = np.array([
        [math.cos(joint.theta_2), -math.sin(joint.theta_2), 0, joint.link_1],
        [0, 0, -1, 0],
        [math.sin(joint.theta_2),  math.cos(joint.theta_2), 0, 0],
        [0, 0, 0, 1]
        ])
    T2_3 = np.array([
        [math.cos(joint.theta_3), -math.sin(joint.theta_3), 0, joint.link_2],
        [math.sin(joint.theta_3),  math.cos(joint.theta_3), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
        ])
    T3_4 = np.array([
        [1, 0, 0, joint.link_3],
        [0, 1, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
        ])

    POINT_SET_1 = np.dot( T0_1, T1_2 )
    POINT_SET_2 = np.dot( POINT_SET_1, T2_3 )
    POINT_SET_3 = np.dot( POINT_SET_2, T3_4 )
    return [POINT_SET_1, POINT_SET_2, POINT_SET_3]

def leg2( x, y, z ):
    #Final calculation resulting in smooth motion.

    theta_1 = math.atan2(y, x)
    A = z
    B = math.cos(theta_1) * x + y * math.sin(theta_1) - LINK_1
    C = ((math.pow(A, 2) + math.pow(B,2) - math.pow(LINK_3, 2) - math.pow(LINK_2,2)) / (2 * LINK_3 * LINK_2))
    theta_3 = math.atan2( math.sqrt( 1 - math.pow( C,2 )), C)
    D = math.cos(theta_3) * LINK_3 + LINK_2
    E = math.sin(theta_3) * LINK_3

    numerator = (A * D - B * E) / (math.pow(E,2) + math.pow(D,2) )
    denominator = 1 - math.pow(numerator,2)

    theta_2 = math.atan2(numerator, math.sqrt(denominator))
    return [ math.degrees(theta_1), math.degrees(theta_2), math.degrees(theta_3)]

def filterEndPoints( point_set ):
    return (
        [
          [point_set[0][0][3], point_set[0][1][3], point_set[0][2][3]],
          [point_set[1][0][3], point_set[1][1][3], point_set[1][2][3]],
          [point_set[2][0][3], point_set[2][1][3], point_set[2][2][3]]
        ]
    )

fig  = plt.figure()
ax = plt.axes(projection='3d')
ax.set_xlim3d(-300, 300)
ax.set_ylim3d(-300, 300)
ax.set_zlim3d(-300, 300)
ax.set_xlabel("X-axis")
ax.set_ylabel("Y-axis")
ax.set_zlabel("Z-axis")
axe = plt.axes([0.25, 0.85, 0.001, 0.001])

axxval = plt.axes([0.35, 0.9, 0.45, 0.03])
axyval = plt.axes([0.35, 0.93, 0.45, 0.03])
axzval = plt.axes([0.35, 0.96, 0.45, 0.03])
a0_val = Slider(axxval, "X", -300, 300, valinit=180)
a1_val = Slider(axyval, "Y", -300, 300, valinit=180)
a2_val = Slider(axzval, "Z", -300, 300, valinit=180)

x_value = 0
y_value = 0
z_value = 0

joint =  Joint(60+60, 110, 120)
joint_values = leg2( 100, 100, 100 )
joint.theta_1 = joint_values[0]
joint.theta_2 = joint_values[1]
joint.theta_3 = joint_values[2]
link_1 = filterEndPoints(FK( joint ))
originPoints = [0, 0, 0]

def returnConstrainedAngles( joint_values ):
    theta_1 = -constrain(joint_values[0], 0.0, 180.0)
    theta_2 = -constrain(joint_values[1], 0.0, 180.0)
    theta_3 = -constrain(joint_values[2], 0.0, 180.0)
    # theta_1 = -joint_values[0]
    # theta_2 = -joint_values[1]
    # theta_3 = -joint_values[2]
    return [math.radians(theta_1), math.radians(theta_2), math.radians(theta_3)]

def update_ao_val(val):
    global link_1
    global x_value, y_value, z_value
    x_value = int(val)

    try:
        #joint_values = leg(x_value, y_value, z_value)
        joint_values = leg2(x_value, y_value, z_value)
    except Exception as e:
        return

    joint.theta_1 = returnConstrainedAngles( joint_values )[0]
    joint.theta_2 = returnConstrainedAngles( joint_values )[1]
    joint.theta_3 = returnConstrainedAngles( joint_values )[2]
    serial_print = " T1 : "+ str( abs( round( math.degrees( joint.theta_1 ) ) ) )  + " T2: " + str( abs( round( math.degrees( joint.theta_2 ) ) )) + " T3 : "+ str( abs( round( math.degrees( joint.theta_3 ) ) ))
    print( serial_print )

    link_1 = filterEndPoints(FK( joint ))
    print( str(link_1[0][2]) + " : " + str(link_1[1][2]) + " : " + str(link_1[2][2]) )
    fig.canvas.draw_idle()
    ax.clear()
    ax.set_xlabel("X-axis")
    ax.set_ylabel("Y-axis")
    ax.set_zlabel("Z-axis")
    ax.set_xlim3d(-300, 300)
    ax.set_ylim3d(-300, 300)
    ax.set_zlim3d(-300, 300)
    ax.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [ 0, 0, 0, 0, 0 ], "-r*" )
    ax.plot3D( [ originPoints[0], link_1[0][0], link_1[1][0], link_1[2][0] ],
               [ originPoints[1], link_1[0][1], link_1[1][1], link_1[2][1] ],
               [ originPoints[2], link_1[0][2], link_1[1][2], link_1[2][2] ], "-go" )
    fig.canvas.draw()

def update_a1_val(val):
    global link_1
    global x_value, y_value, z_value
    y_value = int(val)
    try:
        #joint_values = leg(x_value, y_value, z_value)
        joint_values = leg2(x_value, y_value, z_value)
    except NoneType as e:
        return
    joint.theta_1 = returnConstrainedAngles( joint_values )[0]
    joint.theta_2 = returnConstrainedAngles( joint_values )[1]
    joint.theta_3 = returnConstrainedAngles( joint_values )[2]
    serial_print = " T1 : "+ str( abs( round( math.degrees( joint.theta_1 ) ) ) )  + " T2: " + str( abs( round( math.degrees( joint.theta_2 ) ) )) + " T3 : "+ str( abs( round( math.degrees( joint.theta_3 ) ) ))
    print( serial_print )
    link_1 = filterEndPoints(FK( joint ))
    print( str(link_1[0][2]) + " : " + str(link_1[1][2]) + " : " + str(link_1[2][2]) )
    ax.clear()
    ax.set_xlabel("X-axis")
    ax.set_ylabel("Y-axis")
    ax.set_zlabel("Z-axis")
    ax.set_xlim3d(-300, 300)
    ax.set_ylim3d(-300, 300)
    ax.set_zlim3d(-300, 300)
    ax.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [0, 0, 0, 0, 0], "-r*" )
    ax.plot3D( [ originPoints[0], link_1[0][0], link_1[1][0], link_1[2][0] ],
               [ originPoints[1], link_1[0][1], link_1[1][1], link_1[2][1] ],
               [ originPoints[2], link_1[0][2], link_1[1][2], link_1[2][2] ], "-go" )
    fig.canvas.draw()

# 228 81
# 116 169
def update_a2_val(val):
    global link_1
    global x_value, y_value, z_value, radius_value
    z_value = int(val)
    try:

        joint_values = leg2(x_value, y_value, z_value)
    except NoneType as e:
        return
    joint.theta_1 = returnConstrainedAngles( joint_values )[0]
    joint.theta_2 = returnConstrainedAngles( joint_values )[1]
    joint.theta_3 = returnConstrainedAngles( joint_values )[2]
    serial_print = "T1: "+ str( abs( round( math.degrees( joint.theta_1 ) ) ) )  + " T2: " + str( abs( round( math.degrees( joint.theta_2 ) ) )) + " T3 : "+ str( abs( round( math.degrees( joint.theta_3 ) ) ))
    print( serial_print )
    link_1 = filterEndPoints(FK( joint ))
    print( str(link_1[0][2]) + " : " + str(link_1[1][2]) + " : " + str(link_1[2][2]) )
    ax.clear()
    ax.set_xlabel("X-axis")
    ax.set_ylabel("Y-axis")
    ax.set_zlabel("Z-axis")
    ax.set_xlim3d(-300, 300)
    ax.set_ylim3d(-300, 300)
    ax.set_zlim3d(-300, 300)
    ax.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [0, 0, 0, 0, 0], "-r*" )
    ax.plot3D( [ originPoints[0], link_1[0][0], link_1[1][0], link_1[2][0] ],
               [ originPoints[1], link_1[0][1], link_1[1][1], link_1[2][1] ],
               [ originPoints[2], link_1[0][2], link_1[1][2], link_1[2][2] ], "-go" )
    fig.canvas.draw()

def generateTrajectorySemicircle( radius ):
    theta = np.linspace(0, -np.pi, 100)
    r     = np.sqrt( radius )
    x1    = r   * np.cos( theta )
    x2    = 75  + r * np.sin( theta )
    x3    = 170 - np.linspace(0, 0, 100)
    return [ x1, x2, x3 ]

def plotGraph( graphInstance, currentPoint, trajectoryPoints ):
  graphInstance.clear()
  graphInstance.plot3D( trajectoryPoints[2], trajectoryPoints[0], trajectoryPoints[1], "*", markerSize="4")

  #Setting limits
  graphInstance.set_xlim3d(-200, 200)
  graphInstance.set_ylim3d(-200, 200)
  graphInstance.set_zlim3d(-200, 200)

  graphInstance.set_xlabel("X-axis")
  graphInstance.set_ylabel("Y-axis")
  graphInstance.set_zlabel("Z-axis")
  graphInstance.plot3D( [ -50, 50, 50, -50, -50 ], [ -50, -50, 50, 50, -50 ], [0, 0, 0, 0, 0], "-r*" )
  graphInstance.plot3D(
      [ 100, currentPoint[0][0], currentPoint[1][0], currentPoint[2][0] ],
      [ 0, currentPoint[0][1], currentPoint[1][1], currentPoint[2][1] ],
      [ 0, currentPoint[0][2], currentPoint[1][2], currentPoint[2][2] ], "-go")

  plt.pause(0.00001)

points = generateTrajectorySemicircle( 1000 )

a0_val.on_changed(update_ao_val)
a1_val.on_changed(update_a1_val)
a2_val.on_changed(update_a2_val)
plt.show()
</code>```

答:
1.) Is there material out there that explains how the frame can be translated from the center of the body to the legs?

When you say I've deduced this DH table into its corresponding matrices - presumably these matrices you mention are homogenous transformation matrices that capture the four movements implied by the DH parameters ($a, \alpha, d, \theta$). You can also use these to describe the (fixed) relationship between the centre of the body and the base frame of the leg. For example, if your body frame has the x axis pointing forwards, and the front left leg is located 2 units forward and 1.5 units left, with the base frame of that leg rotated (around z axis) so that the leg is pointed 'outwards' by 90 degrees, then you'll have
$$ {}^{body}T_{frontleftleg} = \begin{bmatrix}
\mathbf{R} & t \\
\mathbf{0} & 1\\
\end{bmatrix}
=
\begin{bmatrix}
0 & -1 & 0 & 2 \\
1 & 0 & 0 & 1.5\\
0 & 0 & 1 & 0\\
0 & 0 & 0 & 1\\
\end{bmatrix}
$$

2.) Is there any material out there that explains Body IK and how it can be implemented for Roll Pitch and Yaw?

Once you've got the leg IK worked out, there isn't much more to do. Consider the robot being stationary with all feet on the ground and the body in some known position. You can work out where a foot is relative to the body by combining the fixed transformation between body and frame at the base of the leg with the transformation from the base of the leg to the foot which is a function of the joint angles (which you control and so should be known). $${}^{body}T_{foot} = {}^{body}T_{leg} * {}^{leg}T_{foot}(\theta_1,\theta_2,\theta_3)$$
Now if you want to move the body, create a transformation matrix representing that movement ${}^{body}T_{bodynew}$ and use it to find out the required position (relative to the current body) that the leg base frame would need to have $${}^{body}T_{legnew} = {}^{body}T_{bodynew} * {}^{body}T_{leg}$$ in order to produce this body position.

Then find out where the foot is relative to this desired leg base position $${}^{legnew}T_{foot} = ({}^{body}T_{legnew})^{-1} * {}^{body}T_{foot}$$

And you can just use your leg IK equations to find the joint angles that give this transformation. Do that for all legs and the body will move as desired.


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

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

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

which also ended without showing issues.

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

For example, typing ROS-commands, such as

roscore or rqt

results in:

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

What has happened? How to fix this?

Additional Info:

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

Where is all the rest gone?

答:
Install ROS properly again using (Just execute this command, don't worry nothing will be installed twice)
sudo apt-get update

sudo apt-get install ros-melodic-desktop-full

also source your ROS packages using (there seems to be a typo in what you've typed above)

source /opt/ros/melodic/setup.bash

You can add this line to your .bashrc file so your ROS automatically get  sourced everytime you open any terminal.


问:
How to deal with Gaussian Errors, which don't have a mean value of 0?
I'm writing a SLAM algorithm. One of the main problems is, that the landmark measurement error is not really Gaussian distributed, which means, their error doesn't have a mean value of 0.

I already tried other M-Estimators, but they all make the assumptions, that the error has a mean of 0. How do you cope with these problems? I think it is safe to say, that most measurements in real life are not gaussian distributed with a mean error of 0.

答:
I don't know if this is mathematically sound or not, but it's given me good results in practice. What I've done in this situation personally, when I expect/know that there is a bias, is to include the bias as a state in my state space representation of the system.

For example, if I have a system to track position, I might write the state space models like:

$$
\left[\begin{matrix}
\dot{x} \\
\ddot{x} \end{matrix}\right] =
\left[\begin{matrix}
0 & 1 \\
0 & 0 \end{matrix}\right]\left[\begin{matrix}
x \\
\dot{x} \end{matrix}\right] + \left[\begin{matrix}
0 \\
K \end{matrix}\right] u
$$

So in this model, hypothetically, the input is maybe some motor torque through a gearbox to a wheel, so the input is a torque, some constant K converts it to an acceleration. Straightforward enough, but what if I know that I'm operating on an incline, but can't measure it? Or, if I know I'm testing on flat ground, but the event track will have an incline?

The trick here requires a bit of fundamental understanding of the differential equation - a differential equation tells you how the variable changes. So, if I expect the slope to be constant in the long-term ("long" relative to the other dynamics of the system), then I can get away with describing the change in slope as being zero! So I modify my state space representation to be:

$$
\left[\begin{matrix}
\dot{x} \\
\ddot{x} \\
\frac{d}{dt}\theta \end{matrix}\right] =
\left[\begin{matrix}
0 & 1 & 0\\
0 & 0 & -mg\\
0 & 0 & 0 \end{matrix}\right]\left[\begin{matrix}
x \\
\dot{x} \\
\theta \end{matrix}\right] + \left[\begin{matrix}
0 \\
K \\
0 \end{matrix}\right] u
$$

So where previously I was assuming the acceleration is some linear function of my input (torque):

$$
\ddot{x} = Ku \\
$$

Now I'm saying that the acceleration is the torque times a constant minus the effect of the slope:

$$
\ddot{x} = Ku - mg\theta \\
$$

This is a use of the small angle approximation $\sin\left(\theta\right) \approx \theta$.

Assuming I can measure position and speed, but not slope, this leaves me with the typical state-space matrices:

$$
A = \left[\begin{matrix}
0 & 1 & 0\\
0 & 0 & -mg\\
0 & 0 & 0 \end{matrix}\right]
$$

and

$$
C = \left[\begin{matrix}
1 \\
1 \\
0 \end{matrix}\right]
$$

and then I can check for observability in Matlab:

```<code>>> syms m g
>> A = [0, 1, 0; 0, 0, -m*g; 0, 0, 0];
>> C = [1, 0, 0; 0, 1, 0];
>> O = [C; C*A; C*A*A];
>> rank(O)

ans =

     3
</code>```

And so you can see that the bias-estimating state-space form is observable, meaning that it is possible to estimate the bias term (slope, in this example) just by measuring the position and speed of the vehicle.

Body:
I don't know if this is mathematically sound or not, but it's given me good results in practice. What I've done in this situation personally, when I expect/know that there is a bias, is to include the bias as a state in my state space representation of the system.

For example, if I have a system to track position, I might write the state space models like:

$$
\left[\begin{matrix}
\dot{x} \\
\ddot{x} \end{matrix}\right] =
\left[\begin{matrix}
0 & 1 \\
0 & 0 \end{matrix}\right]\left[\begin{matrix}
x \\
\dot{x} \end{matrix}\right] + \left[\begin{matrix}
0 \\
K \end{matrix}\right] u
$$

So in this model, hypothetically, the input is maybe some motor torque through a gearbox to a wheel, so the input is a torque, some constant K converts it to an acceleration. Straightforward enough, but what if I know that I'm operating on an incline, but can't measure it? Or, if I know I'm testing on flat ground, but the event track will have an incline?

The trick here requires a bit of fundamental understanding of the differential equation - a differential equation tells you how the variable changes. So, if I expect the slope to be constant in the long-term ("long" relative to the other dynamics of the system), then I can get away with describing the change in slope as being zero! So I modify my state space representation to be:

$$
\left[\begin{matrix}
\dot{x} \\
\ddot{x} \\
\frac{d}{dt}\theta \end{matrix}\right] =
\left[\begin{matrix}
0 & 1 & 0\\
0 & 0 & -mg\\
0 & 0 & 0 \end{matrix}\right]\left[\begin{matrix}
x \\
\dot{x} \\
\theta \end{matrix}\right] + \left[\begin{matrix}
0 \\
K \\
0 \end{matrix}\right] u
$$

So where previously I was assuming the acceleration is some linear function of my input (torque):

$$
\ddot{x} = Ku \\
$$

Now I'm saying that the acceleration is the torque times a constant minus the effect of the slope:

$$
\ddot{x} = Ku - mg\theta \\
$$

This is a use of the small angle approximation $\sin\left(\theta\right) \approx \theta$.

Assuming I can measure position and speed, but not slope, this leaves me with the typical state-space matrices:

$$
A = \left[\begin{matrix}
0 & 1 & 0\\
0 & 0 & -mg\\
0 & 0 & 0 \end{matrix}\right]
$$

and

$$
C = \left[\begin{matrix}
1 \\
1 \\
0 \end{matrix}\right]
$$

and then I can check for observability in Matlab:

>> syms m g
>> A = [0, 1, 0; 0, 0, -m*g; 0, 0, 0];
>> C = [1, 0, 0; 0, 1, 0];
>> O = [C; C*A; C*A*A];
>> rank(O)

ans =

     3

And so you can see that the bias-estimating state-space form is observable, meaning that it is possible to estimate the bias term (slope, in this example) just by measuring the position and speed of the vehicle.


问:
How can I calibrate 3 cameras without knowing global pose of the object & camera locations? How can I find the pose of each camera wrt the first one?
I have camera feed (in the form of RGB images) from 3 cameras with partially overlapping Field-of-view i.e. for the time stamp 0 to 100, I have total 300 frames or say synchronized 100 RGB frames for each camera.

The object (Robot) is moving from one place to another place. I don't know about the camera locations but for each frame & for each camera, I have 8-corner points of 3D bounding-box which are just 2D projections of corresponding 3D camera points on an image. For example, in frames, depending upon the time stamp, if the object appears then I have 2D coordinates of the 8 corners of the blue (3D) bounding box. Note that, in the below image, I have shown only an example object not the entire frame!

image source

Apart from this, I have an Intrinsic Matrix which is same for all the cameras. I also know the object (Robot) dimensions (length, breadth, & height).

So, using these RGB frames from all the cameras, 2D coordinates of 8 corner-points and object (Robot) dimensions, how can I calibrate 3 cameras as well as how can I find the poses of each camera w.r.t. the first camera in the form of a 4x4 transformation matrix [SE(3)]?

Edit 1:

After the calibration, how can I get the global 3-DOF pose of the robot (center of 3D bounding box) for all the 100 frames? For this purpose, I'm assuming the pose of the first camera as a global coordinate frame.

答:
The process you need to go through is actually similar to the camera calibration procedure in OpenCV or other software. The chessboard is replaced by your robot, and you can skip the intrinsic estimation step. I would actually recommend you take a look https://github.com/hengli/camodocal a multi rig camera calibrator.

Anyways a high level overview.

The two steps you have to take are:

Initial Pose Estimation

Refinement via Bundle Adjustment.

Step 1:

You actually only need 1 frame for this. Probably take the synced images that has the most the most projected points in common. (Minimum is 3 points, but you really need each camera to see the 4 same points).

Define your reference frame/origin in your robot/object. You will be estimating the camera positions relative to this. You now also have the 3D bounding box corner positions relative to this frame. If you define it to be the center of the object then a point might look like $[\frac{width}{2},\frac{length}{2},\frac{height}{2}]$

Take your pick of PnP algorithm, and estimate the camera poses(Se3) individually. The 3D points are your bounding box corners relative to your origin. The projections are the 2D coordinates in your image. If you pick the origin to be the center of your robot then you now have calculated the pose of your camera with respect to the center of the robot.

Do some matrix multiplication to convert the camera poses in the object frame, to be relative to the first camera pose.

$$ T_{1,2}=T_{o,1}^{-1}*T_{o,2} $$

Should look like that. Here $o$ is your object coordinate frame, and $1,2$ refer to camera 1 and camera 2.

If your cameras only partially overlap(e.g. only cameras 1,2 and 2,3 have overlaps) then do the same steps for each pair, and then just chain the transforms.

$$T_{13} = T_{1,2}*T_{2,3} $$

Step 2:

I will say this step might be optional for you. You already have the camera positions from step 1, so this just helps refine the results.

Essentially you just need to set up a large Bundle Adjustment problem and solve it using something like Ceres.

Build your 3D Pointcloud. This pointcloud is composed of your bounding box corners at every timestamp. So in total you should have maximum $8*100=800$ points(probably less because sometimes a point isn't visible).

How to exactly do this is tricky. If your robot has perfect odometry then you can just multiply your points by the odometry transform. You can run an object pose estimator algorithm for all timestamps in camera 1. You can use the PnP algorithm again. You just need all 800 3D corner positions in a common reference frame, and there are different ways of doing that.

Build your optimization problem in something like Ceres. Your cost function terms should link the 3D points and the cameras that observe it. See the camodocal code for examples of this.

Solve the bundle adjustment problem.


问:
DIY Smart Devices
I am a software developer. I make apps.

I have several ideas for Bluetooth-enabled smart devices using basic accessories and peripherals like cameras, speakers/headphones, lights and some external buttons/controls - Like a helmet with front and rear camera, headset/speakers and physical volume/call controls or buttons.

I can code the apps but where do I start with the electronics?

Is it best to just buy a Raspberry Pi kit with all the USB accessories, install android or Linux and code directly against that Pi unit - or is there a simpler way?

Is there anywhere I can find learning or study material to help me?

答:
where do I start with the electronics?

and

basic accessories and peripherals like cameras, speakers/headphones, lights and some external buttons/controls - Like a helmet with front and rear camera, headset/speakers and physical volume/call controls or buttons

While it would be easy for anyone to connect some button to an electronic device successfully, peripherals like video cameras, network communication, etc., use very high frequencies, which pose very tricky challenges.

Possible solutions:

(long learning curve) Actually learn electronics, from the ground up. You will be able to design your device by yourself. Learn the basics (you might already know them), and then concentrate on the details that you actually need. Ideally, you learn the theory too, but it should be enough (for the beginning) to concentrate on the practical stuff.

(almost no learning curve) Team-up with a friend/colleague, who already knows how to design electronic circuits.

Title: DIY Smart Devices


答:
To learn electronics (which is pretty much essential), then a copy of The Art of Electronics, by Horowitz and Hill (ISBN: 978-0-521-37095-0), is indispensable.

Nevertheless, on its own, it might not get you up and running as quickly as you like. Also, as you want to build a smart device, you will probably also need a controller of some sort, and you have mentioned the Pi (or Pi kit).

However, as an alternative to the Pi you might want to consider Arduino (or Arduino beginners kit).

Arduino is a lot simpler than Pi, with a lot less over head, code footprint wise. Yes, you can get a Python script running quickly on a Pi, that is true, so the Pi is simpler in that respect.

However, with Arduino, you are coding onto bare-metal, so to speak. You write and compile a "few" lines of code, and upload that to the Arduino, and that is all of the code that you need. Whereas, with Pi, you (usually) end up writing a Linux based application, to run on an OS. So, in that respect, Arduino code can run faster (generally - although some more argue otherwise), as there is no code running for screen updates or keyboard input, and all of the other baggage that comes with a full-blown PC (which is what a Pi is, albeit a rather small one).

Arduino comes with countless libraries so, when it comes to interfacing the Arduino with a particular device, most to the work will be done for you. There's also a lot of ready-to-go examples and projects out there for you to copy or adapt for your own purposes. So, while you will need to, or should, learn electronics (in the end), for a while you may be able to get away without learning/knowing the theory.

That said, I have a few friends (with no electronics training/background), who have tried Arduino, and after trying to run before learning to walk, they have sadly given up. This is because they refused to start off slowly with the excellent basic examples provided and tried to do big projects without any understanding of either C/C++/OOP and/or electronic theory.

You could counter all of the above, with equal arguments for the Pi... It really depends on what you want to do. For simple devices, like wearables, which you seem to refer to, then Arduino would seem to be more sensible, due to the possibility size reduction. For large complex projects, with a lot of data processing, a Pi would be more useful (as the memory (and processing power) on an Arduino is rather limited). Of course, you could use both, with an Arduino monitoring sensors and feeding that data back to the Pi for processing and display on a large monitor.

Depending upon your requirements for quantity of IO ports, you can make large or tiny devices (using large (Arduino Mega 2560) or tiny (Arduino Micro) Arduino based boards respectively), or just the µController IC itself with a few extra support components (two capacitors and a crystal), and the the components for your project (i.e. LEDs, speakers, sensors, or what-have-you).

Finally, Arduinos can be a lot cheaper than Pi - if you go for the Chinese clones, they can be found for a few dollars, and work just as well, in my experience.

Either way, in addition to the very good Arduino and Pi support sites, both Pi-SE and Arduino-SE are excellent Stack Exchange sites, right here, as well.

If you really need faster devices then look into FPGA, but that would seem to not be necessary for you yet, nor is it really for beginners.

Title: DIY Smart Devices

Body:
I am a software developer. I make apps.

I have several ideas for Bluetooth-enabled smart devices using basic accessories and peripherals like cameras, speakers/headphones, lights and some external buttons/controls - Like a helmet with front and rear camera, headset/speakers and physical volume/call controls or buttons.

I can code the apps but where do I start with the electronics?

Is it best to just buy a Raspberry Pi kit with all the USB accessories, install android or Linux and code directly against that Pi unit - or is there a simpler way?

Is there anywhere I can find learning or study material to help me?


问:
How to pick the right motor driver?
I have this brushed DC motor (Vex 775pro: https://motors.vex.com/vexpro-motors/775pro; motor curve data here I assume is useful but I do not exactly know how to make sense of it) My question is, how do I select a motor driver that can handle the current that my motor is drawing and how do I know how much current my motor is drawing?

I would like to purchase a standard H-bridge motor driver so I can control the speed and make the motor go clockwise or counterclockwise. Looking up some things online, I see that I should pick a motor driver that can provide current equal to or above the motor's continuous current consumption under load, however I am not sure what the "continuous current consumption under load" is for this motor. Also in case it matters for anything, I do plan on using this with an Arduino. Any help would be greatly appreciated.

答: For every component you can search for data sheets over the respective manufacturer site. You can know the rated current of motor otherwise on the shopping site. Then find a motor driver for that rating. Or else you can design it on your own.</p>

Title: How to pick the right motor driver?


问:
Adding external heat sinking to a Dynamixel RX-24F servo?
Hobby servos are generally not sufficient for real robotics for a number of reasons: Quality, precision, range of motion, torque, etc.

Meanwhile, industrial servos, such as ABB, Emerson, GE, etc, are generally both heavy and expensive, and not suitable for small-humanoid scale actuation. Similarly, building your own servo from motors, gearboxes, and encoders, is akin to trying to design your own CPU just to control a motor -- too much detail getting in the way of real work.

There exists an in-between level of servos -- reasonably priced, reasonable performance, and reasonably controllable -- in the form of the competing brands of Dynamixel and HerculeX servos.

The smallest offerings in those lines generally are not strong enough for real-world interaction, but the next step up hold a lot of promise. For the Robotis Dynamixel line, this is the RX-24F servo (priced between the cheap AX-12F and the next step up the MX-28R.) Asking around, it seems that the specs and interface on that servo is great, but that it shuts down from thermal overload if you actually try to run it at rated load -- something that I'd expect from a hobby servo, but not a robotics servo.

Now, stepping up to the MX-28R doubles the price. Thus, if the RX-24F heat flaw could be fixed, it would be positioned at a nice price/performance point.

Does anyone have experience in providing additional cooling for this servo? Anything from thermal-gluing heat sinks to the case, to drilling holes and running cooling fluid tubing to any hot parts on the interior would be reasonable approaches. However, before I spend significant time and effort investigating this, I'd like a second opinion -- is it possible, does anyone have experience doing this, is it worth it?

答:
The problem with the Dynamixel servos is that they're trapped in an insulating plastic jacket. There is no convection, and no good thermally conductive material to let heat out efficiently.

The first thing I'd suggest is to get some air flow. Moving air has a surprising ability to cool something hot. You hardly need any movement to get good results. If you're willing to hack the servo, then hack away at the plastic casing. First dismantle it, removing the PCB and motor too if you can get the screws undone. Place the gears somewhere clean and safe in such a way you can remember which order they go back in.

Now get your Dremmel out and carefully remove some of the casing around the motor (green areas). You're aiming to remove enough so that air can flow through the casing. In one side, out the other. Note the plastic surface under the gears, there's no point cutting through to the gear cavity. In fact doing this will allow debris to enter the gear chain and stall it, so keep your cutting behind this plastic surface.

Also be careful not to remove any of the material around the screws (red areas). You'll want that when you re-assemble the motor.

Just doing this should give you some extra power margin before it overheats, especially if you can mount the motor so that the cool air enters at the bottom and warm air leaves at the top. It will be even better if you can blow air through the casing using a small fan. If you have several motors close together, you might be able to blow air through all of them in series with a single fan.


问:
List of tuples as argument in ROS2 launch
I'd like to define an argument in ROS2 launch file which is a list of tuples. How should the list in default_value be formatted?

```<code>def generate_launch_description():
   argument = LaunchConfiguration('argument', default_value=[(1, 0, 0), (0, 1, 0), (0, 0, 1)])
   node = Node(package='...', executable='...', parameters=[{'argument': argument}])
   return node, argument
</code>```

Currently, I'm getting the following errors based on different formatting:

```<code>argument.default_value = [(1, 0, 0), (0, 1, 0), (0, 0, 1)]
>>> Failed to normalize given item of type '<class 'tuple'>', when only 'str' or 'launch.Substitution' were expected.

argument.default_value = ['(1, 0, 0)', '(0, 1, 0)', '(0, 0, 1)']
>>> In the node argument.value[0] is '(' whereas it should be (1, 0, 0).
</code>```

答:
I found a solution which works yet is not perfect. When the argument value in the launch file is formatted as:

```<code>argument.default_value = "['(1, 0, 0)', '(0, 1, 0)', '(0, 0, 1)']"
</code>```

the argument is passed to the node as:

```<code>argument.value[0]
>>> '(1, 0, 0)'

eval(argument.value[0])
>>> (1, 0, 0)
</code>```

If anyone knows a way to skip eval(..) by passing Tuple directly, that'd be better.

Body:
I found a solution which works yet is not perfect. When the argument value in the launch file is formatted as:

argument.default_value = "['(1, 0, 0)', '(0, 1, 0)', '(0, 0, 1)']"

the argument is passed to the node as:

argument.value[0]
>>> '(1, 0, 0)'

eval(argument.value[0])
>>> (1, 0, 0)

If anyone knows a way to skip eval(..) by passing Tuple directly, that'd be better.


问:
"catkin_make" prompts "+make" and "recipe for target ... failed" errors
I have been working on a ground vehicle project for some time that has bunch of sensors, until now I have managed to overcome the problems I have encountered. I have used catkin_make many times and had no problem until I try to include another sensor (an imu by xsens). After including the package to the workspace I work on, when I ran the command "catkin_make" I got these errors in the error image below.

I have done some research, and tried "sudo apt-get install build-essential" getting the response that states everything is up-to-date, nothing to be installed etc.

Then I tried checking the system dependencies using the command "rosdep check --from-paths /home/bfd/catkin_opt/src --ignore-src" and saw that there was only 1 that has not been satisfied, so I used "rosdep install--from-paths /home/bfd/catkin_opt/src --ignore-src", and I got the response "All dependencies have been satisfied". Thinking that the problem is solved, I tried running catkin_make command on my workspace again, yet I got the same errors once again.

At this point, I do not know what to do or try next. Can you help me fixing this problem or lead me to where the solution is? Thanks.

答:
Could these set of drivers work for you? :
https://github.com/ethz-asl/ethzasl_xsens_driver

If yes clone this package inside src folder  your workspace and then try to build your workspace.


答:
I have figured out the solution for the problem. I was using the driver folder directly to run the package. Instead if you are going to use the driver on this link http://wiki.ros.org/xsens_mti_driver , follow exactly every step through.

Especially, execute this mtsdk_linux-x##_####.#.sh file as stated, and then follow the 5th step. Otherwise, you may get the same error as I did.

Title: "catkin_make" prompts "+make" and "recipe for target ... failed" errors

Body:
I have been working on a ground vehicle project for some time that has bunch of sensors, until now I have managed to overcome the problems I have encountered. I have used catkin_make many times and had no problem until I try to include another sensor (an imu by xsens). After including the package to the workspace I work on, when I ran the command "catkin_make" I got these errors in the error image below.

I have done some research, and tried "sudo apt-get install build-essential" getting the response that states everything is up-to-date, nothing to be installed etc.

Then I tried checking the system dependencies using the command "rosdep check --from-paths /home/bfd/catkin_opt/src --ignore-src" and saw that there was only 1 that has not been satisfied, so I used "rosdep install--from-paths /home/bfd/catkin_opt/src --ignore-src", and I got the response "All dependencies have been satisfied". Thinking that the problem is solved, I tried running catkin_make command on my workspace again, yet I got the same errors once again.

At this point, I do not know what to do or try next. Can you help me fixing this problem or lead me to where the solution is? Thanks.


问:
Sam C21 Xplained Pro - Linux toolchain
I recently got a ATSAMC21N Xplained Pro dev board.

I was planning to use avr-g++ and avrdude to do development (I’m running Linux so Atmel Studio isn’t an option). I’ve used avr-gcc and avrdude in the past for Atmel development but the SAM C21 chip doesn’t seem to be on the supported target list.

Does anyone have an idea about what toolchain I could use to build and deploy the program to the board?

答:
The SAM chips have ARM cores, so you should look compiliing with arm-none-eabi-gcc and flashing/debugging with openocd instead.

OpenOCD is a program that communicates with debugging probes for a number of processor/microcontroller architectures, analogous to avrdude for AVR. You will have to figure out how to configure openocd to talk to your board. A quick google search brought me this guide, which might point you in the right direction, although I don't have a SAM breakout board to verify it with.

OpenOCD is definitely worth learning if you're going to be working with ARM chips, but you could probably also flash your code using Atmel's IDE, Atmel Studio. I shy away from using proprietary IDEs, but if you need to get up and running quickly, you could probably use it to both build and flash your project. Hope this helps.

edit: I glossed over the word "Linux" in the title of your question. I believe Atmel Studio is only available for Windows, so unfortunately you're stuck using the much more versatile open source tools which are available for Linux.

Title: Sam C21 Xplained Pro - Linux toolchain


问:
RC Transmitter Quadcopter with Arduino
I have a WL v262 quadcopter and I want to control it using Arduino instead of the joysticks on the transmitter. I opened up the transmitter and saw that each joystick has 2 potentiometers on the PCB and that the voltage for each pot goes from 0-3.3V. I used arduino's PWM and a low pass filter and connected the output of the filtered output to the potentiometer's analog pin which is connected to the PCB (I cannot desolder and take out the pots from the PCB) but even with this $V_{out}$ going onto the analog pin, my transmitter's display gave ????

Now I am really confused and frustrated because I don't know how else to control this transmitter other than attaching stepper motors to the joysticks and manually controlling the transmitter but this is really my last resort. Can someone help me with this? I have spent hours and hours trial and error but I am getting nowhere.

Here is the PCB of the transmitter:

答:
Did it actually output only "????"
Does removing the PWM restore normal operation?
 It is likely that the load from the PWM is not initially within correct voltage range at RC's bootup and it is detecting this and faulting.
With the Pot's still inplace the range will be limited, but you could try to match it. Initially (and even while ????'ing) try to set the PWM's output to be the same voltage as when it is not connected. This may solve your ???? problem.

I also note from your picture the Transmitter module at top between joy sticks. I would suspect that you can better decipher the stream of information sent into this module by the controller and send your own information. This would be similar to what I did for IR Helicopter controllers

Title: RC Transmitter Quadcopter with Arduino


问:
Sensor fusion of GNSS and IMU using UKF
I do have a land-based robot with an IMU and a GNSS receiver.

From the IMU, I get the velocity and acceleration in both $x$ and $y$ directions.

From the GNSS receiver, I get the latitude and longitude.

I want to fuse these data to get my exact position on the field. To get the most accurate position I want to use an Unscented Kalman Filter (UKF).

However, I don't really understand the concept of fusing these data. Most of the books I found just fused the IMU data and used it together with the GNSS data but by my understanding, I should get a more precise position when I fuse IMU and GNSS.

Can someone explain to me the concept of doing so or has a good source/tutorial?

This is for my bachelor thesis. Help would be much appreciated. Thanks!

答:
How did you come to the conclusion that you will need an UKF? Make sure you understand the math behind a Kalman Filter first and understand why you would need an EKF or UKF over a normal KF. This article describes the math behind a Kalman Filter using an IMU but you can add more sensors to this setup..
https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

Also, what platform (hardware/software) do you plan to implement your project in?

Title: Sensor fusion of GNSS and IMU using UKF


问:
360 degree ultrasonic beacon sensor
Basically, I want to detect an ultrasonic beacon in a radius around the robot.
The beacon would have a separate ultrasonic emitter while the robot would have the spinning receiver.

Are there any existing ultrasonic sensors that would meet this use case or am I stuck hacking one together myself?

Is ultrasonic even the best choice? I was hoping that the beacon would be kept in a pocket, so I figured optical sensors were out.

Edit: The beacon and robot will both be mobile so fixed base stations are not an option.

答: If you were to place 3 ultrasonic microphones/sensors in a triangle layout you should be able to determine the direction of the beacon based on the time difference the ping arrives at each sensor with some straightforward trigonometry. They'd all have to be facing in the same direction (probably upwards) and you may have to find some way to make them omnidirectional. The further apart you can space the sensors, the more accurate the calculation will be.</p>

Title: 360 degree ultrasonic beacon sensor


问:
difference between non-holonomic and linear control systems
I already asked this question in the mathoverflow forum, but no one could answer. Probably it is not a question of mathematics but robotics.

I'm struggling with the definitions and differences between a non-holonomic and a linear control systems.

I'm working with a phase space $P\subset R^{m}$ and a control space $U\subset R^{l}$ and my control system

$\dot{p}=f(p,u)$

is of the form

$\dot{p}=\sum^{l}_{i=1}u_{i}P_{i}(p)$

where $P_{1},\dots,P_{l}$ are smooth vector fields on $P$.

In Frederic Jeans book Definition 1.1 a control system of this form is called non-holonomic. I already read that there is the adjective "linear" which would fit better in my opinion since whenever I use the form of the control system I use the fact that it depends linearly on the control.

Can someone advice me literature where it is stated like that?

When I read more about non-holonomicity I found that it is a question whether the vectorfields $P_{i}$ span the whole $TP$.

Can someone explain how linearity and non-holonomicity are connected?

答:
I have to admit, I have no experience with non-holomonic CONTROL System. I have dealt with non-holomonic MECHANICAL systems.

As defined here, a non-holominc mechanical system annot move in arbitrary directions inits configuration space. A mundane example is a car. It can occupy and X, Y position and Z rotation on a flat surface, but it cannot move laterally in the Y direction. It has to manuver to get there.

In mobile robotics, simplisticly this problem is dealt with at a higher level then the closed loop control system. It is considered a constraint for the path/trajectory planner (open loop). After the path/trajectory has been planned, the setpoints are given to the closed loop control system (cyclically), which can be a linear control system.

I understand that there is research in Optimal Control Theory, which includes the non holomonic planning in the closed loop as an optimization problem, e.g. this paper and this paper, but i cannot offer any insight on the intricacies of these methods.


问:
How does a Tesla or any self-driving car self-drive to a remote place and align itself to an object?
I am trying to understand the technology/procedure related to one of the trivial functions of a modern self-driving cars.

Let us consider the example of Tesla's Model S (or any).

The car is parked at the garage and needs to get get its batteries charged at the local charging station/gas station. It needs to self-drive to this station (say Monty's) and then, on reaching there, find where the charging dock is, and finally, align itself such that the charging port on the car's body, is precisely in front of the charging hose (or cable).

For this, the Model-S needs to do the following:

Locate Monty's and find a way to self-drive there.

Stop at the station and align its charging port to the charging dock's hose.

What is the method(s) used by the robots/cars to achieve these two functions ?

答:
High level path planning is present in any navigation system. You can select a goal and a start position (or your current position) and a (drivable) path will be planned by any navigation system, incl. google maps.

Tesla cars use a neural network based control system. Video and radar feeds are combined and a semantic segmented map of the environment is available. This will identify a charger for example. Aligning to that charger, technology-wise could be done by a neural network (being trained by a vast amount of labeled charging videos, or by some classical trajectory planing application for non-holomonic systems (a car is a non-holomonic system). I do not know which option is used by Tesla.


问:
irobot create 2: random changes in driving speed
I am trying to make the Create2 dance a specific choreography. I´m not using the sensors, I just let it drive and turn for a specific amount of time. In general it works fine.

But after working with it for a while, it suddenly doesn't do a full turn any more. It seems like something changes and than driving in a specific angle and turning in place takes a bit longer or shorter. It also happens, that Roomba doesn't drive straight any more but slightly turns to the right. And than it changes again and it´s back to the first configuration.

I tried changing the baudrate, I did manual resets, I charged Roomba and tried in full battery power and low battery power but I can´t find an explanation. The bug occurs without changing code or anything.
Does anyone have an idea, what the problem might be?

Another problem that I ran into: I let Create2 play songs. Most of the time it works. But from time to time I get some weird issues. The tune doesn't play until the end but lingers on one note for a moment. Than the tune stops. Often the next song won´t play at all. Next time I run the code it works just fine.
Any ideas?

The whole thing makes me go crazy because it messes up the whole choreography and makes it totally unreliable...

My set up:
Arduino Nano connected to serial port of Create2.
Arduino-Create2 Libary https://github.com/brinnLabs/Create2

```<code>// Include Create2 libary
#include <iRobotCreate2.h>

// connect Roomba TX to pin 10 and Roomba RX to pin 11,
// connect Roomba BaudRateChange to pin 5
iRobotCreate2 roomba(true, 10,11,5);

// set pins for LED and Startbutton:
const int ledPin =  13;
const int buttonPin = 4;
int incomingByte = 0;

// variable for reading the Startbutton status
int buttonState = 0;

// full rotation of roomba with VEL 100
int fullspin100 = 7319;
int fullspin200 = 3659;
int fullspin300 = 2439;

void setup(){

  Serial.begin(115200);

  // set pinmode for LED and Startbutton
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);

  // reset BaudRate to 19200
  roomba.resetBaudRate();
  delay(2000);

  // start communication and safe mode
  roomba.start();
  roomba.safeMode();
  delay(1000);

  // song nr. 1
  Notes songNotes[16];
  songNotes[0].note = 31;
  songNotes[1].note = 43;
  songNotes[2].note = 31;
  songNotes[3].note = 43;
  songNotes[4].note = 55;
  songNotes[5].note = 67;
  songNotes[6].note = 79;
  songNotes[7].note = 91;
  songNotes[8].note = 103;
  songNotes[9].note = 91;
  songNotes[10].note = 79;
  songNotes[11].note = 67;
  songNotes[12].note = 55;
  songNotes[13].note = 43;
  songNotes[14].note = 31;
  songNotes[15].note = 103;

  songNotes[0].duration = 8;
  songNotes[1].duration = 8;
  songNotes[2].duration = 8;
  songNotes[3].duration = 8;
  songNotes[4].duration = 8;
  songNotes[5].duration = 8;
  songNotes[6].duration = 8;
  songNotes[7].duration = 8;
  songNotes[8].duration = 8;
  songNotes[9].duration = 8;
  songNotes[10].duration = 8;
  songNotes[11].duration = 8;
  songNotes[12].duration = 8;
  songNotes[13].duration = 8;
  songNotes[14].duration = 8;
  songNotes[15].duration = 8;

  roomba.createSong(1, 16, songNotes);

  // turn on PowerLed red to indicate that roomba is ready for dancing
  roomba.setPowerLEDs(255, 255);
  delay(10);
}

void loop(){
  // listen to input from the button
  buttonState = digitalRead(buttonPin);

  // if button is pressed, start dancing
  if (buttonState == LOW) {
    digitalWrite(ledPin, LOW);
    dance();

  }
  if (buttonState == HIGH) {
   digitalWrite(ledPin, HIGH);
  }
}

void dance(){

  roomba.turnCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.playSong(1);
  delay(1300);

}
</code>```

答:
Looking at your code quickly, it looks like you are sending velocity commands and then waiting for some amount of time to execute your motions, without using any encoder feedback to verify the robot is moving the amount you expect. I would suggest that you implement a control loop to make your movements more accurate. I suspect a simple PI controller would get you where you need to be, though then your timings might not work out. You'd need to figure out a way to synchronize with your external track.

Please note that I am an iRobot employee, but the postings on this site are my own and don't necessarily represent iRobot's positions, strategies, or opinions.


问:
Identify this robot?
Seen in this story, and in * many * others relating to AI and robots (determined via google image search...)

What build is this and it's origin?

答: The robot is called Alter. It is part of the robot exhibit at the Miraikan museum in Tokio. It seems from this website, that it has been created by Ikegami Takashi, specially for the exhibit, in order to explore human interaction by mimicking human poses.</p>

Title: Identify this robot?


问:
How to make a CNC machine from scratch?
I want to make my own CNC machine from scratch because it's cheaper to make than buy a premade one.  I noticed that some machines go for tens of thousands of dollars and I cannot understand how they can cost that much to build.  I am not afraid to learn what I need to learn to make one.

I can do basic Arduino programming and stuff.  I understand the concept of G-code, its not that hard.  The thing I cannot understand is how you go from G-code to moving the stepper motors.

How does a controller translate a move $x$ 20 units to moving the stepper $x$ amount of steps and keep dimensional accuracy?  If the CAD G-code says move 200 mm in the $x$ direction, how to you translate the G-code to tell the stepper motor that 200 mm?

Any help is greatly appreciated. If you need clarification, please ask in the comments.

答:
How does a controller translate a move x 20 units to moving the stepper x amount of steps and keep dimensional accuracy? If the CAD G-code says move 200 mm in the x direction, how to you translate the G-code to tell the stepper motor that 200 mm?

The G-Code interpreter interprets the motion command. It applies some basic transformations on it to make sure that all coordinates in the motion buffer are in the same coordinate system. Assuming it is G01 (linear interpolation) X 200, The interpreter will place the appropriate command to the motion buffer. Please note that there is a difference between currently executed command and interpreted command. The difference is a lookahead, or a motion command buffer with several hundreds of commands (in case of industrial high-end CNC machines).

The trajectories are planned by evaluating machine dynamics (the trajectory generator is aware of the maximum velocities and acceleration of each axis). So if the max velocity of your CNC is 5000 m/min and you have set the F paramter for your G01 to 8000 your machine will accelerate to 5000 and keep that velocity until possible or set otherwise.

There are some advanced transformation functions (optionally) applied, like tool radius compensations, etc.

At this point there is a trajectory in form of a spline curve with attached velocity/acceleration profiles planned for the entire lookahead buffer. This spline is then discretized to achieve the next reference point for the closed loop controls (position, velocity and torque control loops on an industrial CNC machine) or just a stepper motor on a hobby CNC.So if you have a linear motion on the x axis, the spline will be only a line segment from the previous value in the motion buffer to X200. If the motion buffer is empty, i.e. this is the first command the current position will be used to start the line.

There is another transformation here. The industrial system measure current position in encoder ticks, the hobby systems in stepper motor steps. There has to be a transformation function available to convert from ticks or steps to mm and vice-versa. This comes from the geometrical model of the machine and includes transmission ratios of gearboxes, ballscrew drives or belt drives. This is a design parameter of the machine and is included as parameter for the software. More advanced functions, like compensating for belt elasticity and/or backlash are also common. So 200 mm might translate to 2 000 000 encoder ticks or steps.

For a stepper motor quite simply the missing number of steps are calculated and executed. So if you are at x100 mm you will need to execute 1 000 000 steps. For a stepper motor without an encoder you have to count the steps travelled and use this count instead of an encoder position reading. If you loos count (e.g turn off machine) there is a reference position with a mechanical switch at one end of the axis. You drive to that end and reset count to 0 when you hit the switch.

For a closed loop system and error signal is created between current and target position for the first control loop, and cascaded error signals including feed forward reference signals for the velocity and torque control loops. Also here there might be a reference position, or you might have absolute encoders, which do not need a reference position.

Title: How to make a CNC machine from scratch?


问:
Why Euler Angle is set to be in ZYZ order?
We have 3 DOF for rotation in 3D space. So to describe an arbitrary rotation, we need to describe its 3 DOF. Euler angle does this by dividing a rotation in 3 steps, first rotate along the Z axis of the world frame ($z_0$), then rotate along Y axis of current frame ($y_1$), and finally rotate along the Z axis of the current frame($z_2$).

And of course we have other conventions like z-x-z, x-y-x, y-z-y, x-z-x, y-x-y.

And my question is, why the euler angle is define in this "ZYZ" manner? Why the first and third rotation are about the same axis (of course the first Z and the last Z are in different frames) ? Why not to use ZYX?

答:
According to what I was taught in physics class, the convention Leonhard Euler introduced in the 1700s was YZY Eulers -- but the phrase "Euler Angles" is commonly applied to any of the 12 rotation order conventions mentioned by Peter Corke.

At my company we used to always use XYZ or ZYX rotation order, but we have discovered that using something like YZY is more intuitive.  Suppose you have some type of member (say the forearm of an arm mechanism) that in its default configuration points in the Y-direction.  If you consider a north pole to lie on the +Y axis, YZY rotation corresponds to a roll, followed by a change in latitude, followed by a change in longitude.  If I can envision the orientation I want to achieve, I can guess its roll, latitude and longitude; or if the forearm is already at some orientation I can predict what will happen when I manipulate roll, latitude and longitude sliders.  On the other hand, if the forearm is far from the default orientation, I have no idea what its X,Y,Z Eulers will be or what changes in one of those angles will do to the orientation.


问:
Kalman-Filter: how to solve angles near +/-pi?
I'm trying to get into Kalman filters. I've noticed an issue with Euler angles near -180°/180° (or -pi/pi) and wonder how to correctly resolve this. Its often said you need to normalize the angles into this range. However, this isn't as easy as it seems at first sight. Especially, when using a Kalman filter class from a library (e.g. OpenCV).

Let's take a very simple examle. We have only a compass sensor, which will give us a heading in the XY-plane. And our state keeps track of the current heading and its first derivative (speed of change).

Matrix Algebra

First, I'll give all information in the typical matrix form to show how this actually relates to the Kalman filter. Later I'll reduce this to simple formulas for better reasoning.

We define our state space vector as:

$x_{k} = \begin{bmatrix} \alpha \\ \dot{\alpha} \end{bmatrix}$

Where $\alpha$ is the current heading in degrees (for simplicity) and $\dot{\alpha}$ is the current change of the heading in degrees per second.

The new a priori state estimate (prediction) is calculated by:

$\hat{x}_{k|k+1} = F_k x_k$
where $F_k = \begin{bmatrix} 1 & \Delta t \\ 0 & 1\end{bmatrix}$

Our observations look like this:

$z_k = \begin{bmatrix} \alpha_{new} \end{bmatrix}$

And the correction / update step looks like this:

$\tilde{y}_k = z_k - H_k \hat{x}_{k|k+1}$ where $H_k = \begin{bmatrix} 1 & 0\end{bmatrix}$

Later the Kalman gain $K_k$ is calculated and influences the state estimate like this:

$\hat{x}_{k+1} = \hat{x}_{k|k+1} + K_k \tilde{y}_k$

Simplifications

$\hat{x}_{k|k+1} = \begin{bmatrix} \alpha + \Delta t \dot{\alpha} \\ \dot{\alpha}\end{bmatrix}$

$\tilde{y}_k = \alpha_{new} - (\alpha + \Delta t \dot{\alpha})$

$\hat{x}_{k+1} = \hat{x}_{k|k+1} + K_k \tilde{y}_k$

Now lets only focus only on the estimation of $\alpha$:

$\alpha_{k|k+1} = \alpha_k + \Delta t \dot{\alpha}_k$

$\alpha_{k+1} = \alpha_{k|k+1} + K_k^1 (a_{new} - \alpha_{k|k+1})$

The Issue and Semi-Solution

For simplicity I'll use angles in degrees. Let's assume, we've got the following situation:

$\alpha_k = 160°$, $\dot{\alpha}_k = 100°/s$, $\Delta t = 0.1 s$, $a_{new} = -170°$ ($\equiv$ 190°) and $K_k^1 = 0.1$.

Hence, $\alpha_{k|k+1} = 160° + 0.1 * 100° = 170°$ and therefore $a_{new} - \alpha_{k|k+1} = -170° - 170° = -340° \equiv 20°$.

The updated estimate would therefore (incorrect estimate) be:

$\alpha_{k+1} = 170° + 0.1 * (-340°) = 170° - 34° = 136°$

If we normalize $\alpha_{k|k+1}$ to [-180°, 180°] we get the equivalent 20° and the result is (correctly estimated):

$\alpha_{k+1} = 170° + 0.1 * 20° = 170° + 2° = 172°$

The normalization would fix the issue. However, if I use a normal Kalman filter class, I usually cannot influence how the innovation $\tilde{z}_k$ is calculated and the filter class cannot know that this is an Euler angle that needs special handling. Thus, I would need to write it my own... Or is there any way around this?
A quick solution might be to use a unit vector $\begin{bmatrix} cos(\alpha) \\ sin(\alpha) \end{bmatrix}$ instead of the euler angle $\alpha$, which I would have to renormalize every now and then.
I know that one could maybe use quaternions for this, but I currently can't grasp the math behind it to use it. Also I saw someone argue that it would be a bad idea to use quaternions in the state space of a Kalman filter (e.g. see comments to this question). This could also apply to my unit vector approach. Hence, I would like you to discuss the issue and give me some clues on how this is usually resolved.

答:
Here is a paper on the subject.  By doing the proper maths with the SO(2) Lie group to represent the angle, they show that the very simple approach of wrapping the predicted state angle to $[-\pi, \pi)$ with
$$
\mod(\alpha + \pi, 2 \pi)-\pi
$$
is equivalent, so you don't have to worry about Lie algebras.


答:
I see that working with control loops is a more elegant way of controlling the robot. But I’m pretty sure that this isn’t the problem. I uploaded a video to demonstrate the strange behaviour:

https://www.youtube.com/watch?v=hms2z7WoUgQ&feature=youtu.be

One time it does exactly what it is supposed to do and the next day it drifts to the left, turns at different speed and ends up in the wrong place. But Running the program several times in a row it repeats the movement (doesn’t matter if it chose the right or the wrong) precisely. It’s doing exactly the same movement in every run (expect when it is changing all parameters mysteriously) So the "drive motor - wait - stop" method is actually accurate enough for me. And a control loop also wouldn't prevent the robot to drift to the left instead of driving straight.
That's why to me it seems to be a different problem. Could there be something wrong in the firmware?
This is my version: r3-robot/tags/release-stm32-3.7.5:6216 CLEAN

Any other ideas?

Thanks for helping!

Title: irobot create 2: random changes in driving speed

Body:
I am trying to make the Create2 dance a specific choreography. I´m not using the sensors, I just let it drive and turn for a specific amount of time. In general it works fine.

But after working with it for a while, it suddenly doesn't do a full turn any more. It seems like something changes and than driving in a specific angle and turning in place takes a bit longer or shorter. It also happens, that Roomba doesn't drive straight any more but slightly turns to the right. And than it changes again and it´s back to the first configuration.

I tried changing the baudrate, I did manual resets, I charged Roomba and tried in full battery power and low battery power but I can´t find an explanation. The bug occurs without changing code or anything.
Does anyone have an idea, what the problem might be?

Another problem that I ran into: I let Create2 play songs. Most of the time it works. But from time to time I get some weird issues. The tune doesn't play until the end but lingers on one note for a moment. Than the tune stops. Often the next song won´t play at all. Next time I run the code it works just fine.
Any ideas?

The whole thing makes me go crazy because it messes up the whole choreography and makes it totally unreliable...

My set up:
Arduino Nano connected to serial port of Create2.
Arduino-Create2 Libary https://github.com/brinnLabs/Create2

```<code>// Include Create2 libary
#include <iRobotCreate2.h>

// connect Roomba TX to pin 10 and Roomba RX to pin 11,
// connect Roomba BaudRateChange to pin 5
iRobotCreate2 roomba(true, 10,11,5);

// set pins for LED and Startbutton:
const int ledPin =  13;
const int buttonPin = 4;
int incomingByte = 0;

// variable for reading the Startbutton status
int buttonState = 0;

// full rotation of roomba with VEL 100
int fullspin100 = 7319;
int fullspin200 = 3659;
int fullspin300 = 2439;

void setup(){

  Serial.begin(115200);

  // set pinmode for LED and Startbutton
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);

  // reset BaudRate to 19200
  roomba.resetBaudRate();
  delay(2000);

  // start communication and safe mode
  roomba.start();
  roomba.safeMode();
  delay(1000);

  // song nr. 1
  Notes songNotes[16];
  songNotes[0].note = 31;
  songNotes[1].note = 43;
  songNotes[2].note = 31;
  songNotes[3].note = 43;
  songNotes[4].note = 55;
  songNotes[5].note = 67;
  songNotes[6].note = 79;
  songNotes[7].note = 91;
  songNotes[8].note = 103;
  songNotes[9].note = 91;
  songNotes[10].note = 79;
  songNotes[11].note = 67;
  songNotes[12].note = 55;
  songNotes[13].note = 43;
  songNotes[14].note = 31;
  songNotes[15].note = 103;

  songNotes[0].duration = 8;
  songNotes[1].duration = 8;
  songNotes[2].duration = 8;
  songNotes[3].duration = 8;
  songNotes[4].duration = 8;
  songNotes[5].duration = 8;
  songNotes[6].duration = 8;
  songNotes[7].duration = 8;
  songNotes[8].duration = 8;
  songNotes[9].duration = 8;
  songNotes[10].duration = 8;
  songNotes[11].duration = 8;
  songNotes[12].duration = 8;
  songNotes[13].duration = 8;
  songNotes[14].duration = 8;
  songNotes[15].duration = 8;

  roomba.createSong(1, 16, songNotes);

  // turn on PowerLed red to indicate that roomba is ready for dancing
  roomba.setPowerLEDs(255, 255);
  delay(10);
}

void loop(){
  // listen to input from the button
  buttonState = digitalRead(buttonPin);

  // if button is pressed, start dancing
  if (buttonState == LOW) {
    digitalWrite(ledPin, LOW);
    dance();

  }
  if (buttonState == HIGH) {
   digitalWrite(ledPin, HIGH);
  }
}

void dance(){

  roomba.turnCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(100, 0);
  delay(fullspin100);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(200, 0);
  delay(fullspin200);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.turnCCW(300, 0);
  delay(fullspin300);
  roomba.drive(0, 0);
  delay(1000);

  roomba.playSong(1);
  delay(1300);

}
</code>```


问:
What flight firmware stack is most common in aerial robotics research environments?
I'm considering Aero-Astro grad school and am looking into acquiring a drone hardware platform to implement GNC/ PNT/ autonomy papers like those I might see from, e.g., the Navigation and Autonomous Vehicles group at Stanford, the Robust Robotics group at MIT or the Model Based Robotics Systems group at MIT. My current plan is to take a PX4/Ardupilot based drone and modify the code as needed to implement the concepts of a given paper.

Which of these firmware stacks, if any, is more popular in aerial GNC/PNT/autonomy research environments, and beyond algorithms themselves, are any kinds of low-level/ infrastructural modifications made to the firmware stack? When implementing a new idea for a given paper, do researchers in this space generally start by modifying the public repo, or do they have their own forks with some levels of improved infrastructure, broadly defined, that they use as a starting point?

答:
It is usually a combination of 2 different pieces software. Generally a higher level software which implements most of your autonomy, advanced navigation algorithms, and a lower level software which deals with interfacing the motors, a simple state estimator, and accepts waypoint commands.

Most common is ROS for the higher level software, and a PX4 for lower level. They are also frequently run on separate computers. With ROS running on something beefier like a TX2 or Intel Nuc, and the px4 software running on a pixhawk.

Generally the way it is set up is that your pixhawk is connected to the motors, and connected to your bigger computer via a serial link. It receives waypoint commands from your more powerful computer. And does the necessary logic to convert them to motor RPMS. It is also running a basic state estimator which can fuse data from your more advanced algorithms running on the stronger computer.

Regarding research most of the work is done on the higher level/ROS side. It is rare that someone makes a modification to the PX4 firmware. Most labs have a set of frameworks in place to take care of motion planning, SLAM, decision making, and more. So when someone wants to implement a new idea they can modify existing code, or just re-implement 1 module. Rather than having to write the whole system from scratch. Since it is mostly ROS that is pretty easy to do.

Note that ROS + Px4 is probably the most common combination, but different labs may have different things. I know certain groups at MIT use ROS, and sometimes a custom middleware solution based off of LCM.


问:
POMDPs in robotics
POMDPs are used when we cannot observe all the states.
However, I cannot figure out when these POMDPs can be useful in robotics. What is a good example of the use of POMDPs? (I have read one paper where they used them, but I didn't find it obvious why pomdps should be used) What would be good projects ideas based on POMDPs?</p>

答:
A good rule of thumb is that where ever an MDP is useful in theory a POMDP would likely need to be used in reality.

To answer your question directly I would direct you to some of the latest work coming out of the Algorithmic Robotics Lab. My advisor and I recently developed a method wherein we use a POMDP at the core of a new grey-box system identification method. In this method we know an approximate kinematic or dynamic model of the robot in question. We do not however know some of the associated parameters (e.g. mass, center of mass). One way to learn the parameters is to use an extended Kalman filter (EKF) to monitor the system while the motor babbling. However more can be learned if the dynamics are excited correctly. To achieve this part we use a POMDP to plan controls. The details can be found in our paper Online Parameter Estimation via Real-time Replanning of Continuous Gaussian POMDPs.

Title: POMDPs in robotics