问:
How to properly initialize every new pose in a Visual SLAM algorithm (namely DSO)?
My question is a bit specific, because it is linked to a certain algorithm. Therefore I didn't find any other solutions on how to go about this problem. If you could refer me to research papers, instructions or anything similar which is already available in regard to this problem, I would really appreciate it. Thank you in advance for reading my post and taking your time.

I am currently trying to find out how someone would go about supporting the pose estimation in a Visual SLAM algorithm, since the optimization procedure would overwrite that initial guess anyway. But what if I want to have the algorithm use the real world scale of the reconstructed camera trajectory? Suppose, I know the prefectly accurate trajectory of how the camera moved (ground truth). In my view it should be as easy as setting the new poses for every frame. Unfortunately my try to replace the camera poses with this information actually "broke" the algorithm (that is, it didn't help with the pose estimation, but lead to it failing entirely). Now, I am a bit sceptical as to whether or not this is even possible.

Let's make my question more specific:

There is an algorithm called Direct Sparse Odometry (compare: https://vision.in.tum.de/research/vslam/dso). It is not based on detecting and matching features (so called "indirect" methods) but operates on a direct comparison of pixel intensities (thus called a "direct" method). At the time of this writing the current open sourced version is tailored for monocular input videos / image sequences. You can see that there are actually three parts that are central to initializing the new pose:

1.) CoarseInitializer::setFirst()

This is called once as tracking of the camera movement starts.

Source: https://github.com/JakobEngel/dso/blob/master/src/FullSystem/CoarseInitializer.cpp#L771

2.) CoarseInitializer::trackFrame()

This is called for the first few frames to initialize the scene based on the beginning of the recorded sequence. To me it seems like it is used to get the rotation and scaling of the cameras right at the beginning. This procedure uses a high amount of points / pixels to estimate the initial configuration.

Source: https://github.com/JakobEngel/dso/blob/master/src/FullSystem/CoarseInitializer.cpp#L114

3.) CoarseTracker::trackNewestCoarse()

After initialization is completed (around 10 frames), the camera pose is being tracked across the sequence. However, the number of points used to track the camera is reduced signifcantly to speed up the tracking. This method is executed in a loop until the end of the recording.

Source: https://github.com/JakobEngel/dso/blob/master/src/FullSystem/CoarseTracker.cpp#L556

Question:

How would one incoorporate the ground truth in this algorithm? Or in general: Is replacing initial poses (basically the lines in the source code where the default constructor is called to define a pose) a good idea or does it need to be more sophisticated than that?
Maybe it is even possible to get a measure of how well the estimation is (something like a hessian for the poses and not for the individual points) in order to compare, but let's clarify if the fundamental basics for such a comparison can be laid out first. ;)

Any input is highly appreciated. Thank you!

答:
DSO initializes the scene and camera poses with a specific scale factor such that the average inverse depth of the pointHessians is one. After the initialization the first two frameHessians are led into the backend to do a bundle adjustment like optimization in which, however, the previous determined scale can change (because the absolute scale is not observale in a monocular VO). After the initialization the scale should stay constant but still unknown. If you manually set new frame poses with the GT as predicted values for the trackNewCoarse function, the scales are not consistant. That's why your system gets lost right after the initialization. </p>

<p>If you want do the fusion in a tightly coupled way, you need do following things:
1. Determined the absolute scale after the initialization.
2. Incorperate ground truth poses into the backend optimization, that is, add residuals to the objective function which can be simply defined as the SE3 error between the estimated pose and ground truth pose. For the implementation, you need to add extra quantities to the HFinal_top and bFinal_top matrices.
3. Incorperate ground truth poses into the frontend coarse tracking like in the backend. </p>

Body:
DSO initializes the scene and camera poses with a specific scale factor such that the average inverse depth of the pointHessians is one. After the initialization the first two frameHessians are led into the backend to do a bundle adjustment like optimization in which, however, the previous determined scale can change (because the absolute scale is not observale in a monocular VO). After the initialization the scale should stay constant but still unknown. If you manually set new frame poses with the GT as predicted values for the trackNewCoarse function, the scales are not consistant. That's why your system gets lost right after the initialization.

If you want do the fusion in a tightly coupled way, you need do following things:
1. Determined the absolute scale after the initialization.
2. Incorperate ground truth poses into the backend optimization, that is, add residuals to the objective function which can be simply defined as the SE3 error between the estimated pose and ground truth pose. For the implementation, you need to add extra quantities to the HFinal_top and bFinal_top matrices.
3. Incorperate ground truth poses into the frontend coarse tracking like in the backend.


问:
Controlling bot using video and image processing
I am going to start a project on controlling robots using hand gestures.
Though I have used MATLAB for this purpose earlier, I realized it is extremely slow for any practical real-time system.
I am currently planning to use OpenCV for this purpose.
I want suggestions on, if OpenCV is the best alternative, are there any other alternatives and if I use OpenCV, which language should I go for, C, C++ or Python?</p>

答:
Though I did not use hand gestures, I used color based object tracking for my undergrad project. I did it through MATLAB as at that point of time I had no idea about Open-CV. I am guessing you will have a laptop for image processing and send commands run-time to your robot.
Ok first thing, sending serial commands though the serial port / USB port / wifi to your robot will be damn easy (at-least in the computer part - sending). You can do your gesture recognition using low resolution say 240*240. This will be fast considering a moderately new machine. MATLAB is itself not slow. It is your implementation and algorithms used which determines slow or fast.

Now if you want to do the entire thing in a mobile platform say Android, MATLAB is out of the question. Or if you are using any embedded platform then Open-CV is the way to go. You will have to code the communication part of things too as OpenCV does not have any serial communication or related toolbox. But even with Open-CV your implementation and algorithms will determine slow or fast.

In open-CV 2.0 the new C++ interface is much better and also faster than the old openCV 1.0 functions. You also have OO with OpenCV 2.0 and using CVMat is much more related and easy like MATLAB than the native IPLImage interface.

ContentLicense: CC BY-SA 3.0


问:
Converting from Modified to Standard Denavit Hartenberg
This is probably a noob question.

But as of right now I am having difficulty converting from the Modified DH to a Standard DH parameters for the Franka Emika Panda. I need standard because it allows me to avoid recoding a lot of my other functions.

The modified DH parameters are provided in the following link:
https://frankaemika.github.io/docs/control_parameters.html#constants

```<code>    Joint   a(m)     d(m)     α(rad)   θ(rad)
------------------------------------------
Joint 1     0        0.333    0         θ1*
Joint 2     0        0        −π/2      θ2*
Joint 3     0        0.316    π/2       θ3*
Joint 4     0.0825   0        π/2        θ4*
Joint 5     -0.0825  0.384    −π/2       θ5*
Joint 6     0        0        π/2        θ6*
Joint 7     0.088    0        π/2        θ7*
Flange      0        0.107    0         0
</code>```

I suspect my brain has frozen in favor of idiocy as I tried finding the parameters by doing it by hand, but right now I am consistently getting something that doesn't look right is there someone who can help me out?

And to the bigger question: is there like an efficient way of doing this "matlab, python, etc"? Like a nice conversion method/strategy/algorithm etc?

Any help would be appreciated. Thanks in advance.

Edit: Is it really just shifting the a(m) and alpha(m) parameters?

答:
No, unfortunately there is no automated way to convert between different DH parameter conventions.  See my similar question here: How to convert between classic and modified DH parameters?

Also, you might be interested in this paper: "Lipkin 2005: A Note on Denavit-Hartenberg Notation in Robotics".  It explains the 3 main DH parameter conventions and how they differ.  It is also good background reading on DH parameters.

ContentLicense: CC BY-SA 4.0


问:
The Jacobian resulted from Screw method is different from analytical one (Example Inside)
I am currently solving a kinematics example that asks for the Jacobian. However, when I solve it using the Screw method I get different results from the analytical method, The example is kinda hard to write here but I will demonstrate on a 2D RR robot which shows the same behavior.

Here is the simple problem:

Now lets find the Jacobian when $θ_1=pi/4 $ while  $θ_2=0$, $L1=L2=1$

Here is the Jacobian using Screw Method:

$J =  \begin{bmatrix}
0 & 0\\
0 & 0 \\
1&1 \\
0& 0.7\\
0& -0.7\\
0&0
\end{bmatrix}
$

Here is the Jacobian using Analytical Method:

$J =  \begin{bmatrix}
-L_1 sin(θ_1) - L_2 sin(θ_1+θ_2)  & -L_2 sin(θ_1+θ_2)\\
L_1 cos(θ_1) + L_2 cos(θ_1+θ_2) & L_2 cos(θ_1+θ_2) \\
\end{bmatrix},
$

$J =  \begin{bmatrix}
-\sqrt{{2}} & -\sqrt{{2}} / 2 \\
\sqrt{{2}} & \sqrt{{2}} / 2 \\
\end{bmatrix}
$

Notice that even singularity of both methods do not match. Where did I make the mistake?

Also, why the screw method does not take into account the length of last link? I mean if the length of last link is 0 or 1000, the linear velocity W x R should change.

Edit1: Here is the method I used to calculate the first jacobian.

答:
There are in fact two types of Jacobians, a geometric Jacobian and an analytical Jacobian.  The intro to chapter 3 in the book: Robotics: Modelling, Planning and Control by Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo says it well:

...differential kinematics is the relationship between the joint
  velocities and the corresponding end-effector linear and angular
  velocity.  This mapping is described by a matrix, termed geometric
  Jacobian, which depends on the manipulator configuration.
  Alternatively, if the end-effector pose is expressed with reference to
  a minimal representation in the operational space, it is possible to
  compute the Jacobian matrix via differentiation of the direct
  kinematics function with respect to the joint variables.  The
  resulting Jacobian, termed analytical Jacobian, in general differs
  from the geometric one.


问:
Jacobian of a planar manipulator with 4 links
I have a mobile manipulator that moves on the plane. The base moves in SE(2) and the manipulator is also planar with 4 links. The mobile base will be a differential drive robot and its motion is defined as:

The manipulator is attached to the center of our mobile base. How do I compute the Jacobian of the mobile manipulator?

I know that for the mobile manipulator I have something of the form:

The middle matrix is what I need to compute.

答:
This wikibook on robot kinematics does a pretty good job of explaining a 2 link arm example.  https://en.wikibooks.org/wiki/Robotics_Kinematics_and_Dynamics

You should be able to extend it to 4 links like this:

$$
J =
\begin{bmatrix}
-a_1 s_1 - a_2 s_{12} - a_3 s_{123} - a_4 s_{1234} & -a_2 s_{12} - a_3 s_{123} - a_4 s_{1234} & - a_3 s_{123} - a_4 s_{1234} & - a_4 s_{1234} \\
a_1 c_1 + a_2 c_{12} + a_3 c_{123} + a_4 c_{1234} & a_2 c_{12} + a_3 c_{123} + a_4 c_{1234} & a_3 c_{123} + a_4 c_{1234} & a_4 c_{1234}\\
0 & 0 & 0 & 0
\end{bmatrix}
$$

Where: $a_1$ is the link 1 length, $a_2$ is the link 2 length, etc.  $s_1$ is $\sin(\theta_1)$, $c_{12}$ is $\cos(\theta_1 + \theta_2)$ etc.


问:
Reference frame, vector
How do I specify the directions of a unit vector $X_n$ and $Y_n$ of a frame $\{n\}$ at an angle $θ$ relative to a frame $\{a\}$ in the form
$$
X_n=cosθX_a + sinθY_a
$$
$$
Y_n=-sinθX_a+cosθY_a
$$
? Can someone please help how the above equatione were derived?

答:
Here's a graphical answer. Given some vector $r$ in Frame 1:

You can rotate to a new frame, F2:

All you do is project to the new axes. Counting along the x-axis in $F_2$, you can see that you go right (positive) in an amount $x_a \cos{\theta}$, then you go right (positive) in the amount $y_a \sin{\theta}$, so the total x-axis location is

$$
x_{F_2} = +\left(x_a\cos{\theta}\right) + \left(y_a\sin{\theta}\right)
$$

Similarly, count "down" (negative) an amount $x_a \sin{\theta}$ and then count up (positive) an amount $y_a \cos{\theta}$ and get:

$$
y_{F_2} = -\left(x_a\sin{\theta}\right) + \left(y_a\cos{\theta}\right)
$$

Title: Reference frame, vector


问:
where can I find a good source to study robotics kinematics and trajectory planning relative to robotic arm manipulator?
I am a student, and I have to study for a Robotics exam which consistes mainly in kinematics for a robotic manipulator and trajectory planning also for a robotic manipulator.

In the part relative to kinematics, I have found many sources to study, but for the part relative to trajectory planning I cannot find a complete explanation that allow me to understand deeply the concept.

where can I find a good source to study robotics kinematics and most of all trajectory planning relative to robotics arm manipulator?

I don't know if this is the right place to ask for this, so if it is not in topic with the Robotics exchange policy please tell me and i will delete the question.

答:
Here is my main source, take a look at this book :

Craig, John. J. (2005). Introduction to Robotic : Mechanics and
Control. Pearson Education Inc.

Corke, Peter P. (2017). Robotics, Vision and Control. Springer.

Siciliano, B. (2009). Robotic Modelling, Planning and Contol.
Springer.

Paul, P.R. (1981). Robot Manipulator : Mathematics, Programming, and
Control. MIT.


问:
EKF sensor fusion
What is the standard way to fuse multiple sensor measurements in an EKF framework?

Say you have Odometry, IMU and some form of Lidar which can produce landmarks.

EKF is normally presented as a prediction/correction framework after each action but it's not clear how 3+ sensor sources can be integrated.

答: An EKF or any of the variants of the Kalman filter, as you said mainly works in two steps: prediction and correction. The prediction steps gives you a state estimate based on your process model and the correction step updates your state estimate based on the current measurement. If you have multiple measurements from more than one sensor, you would just update the state as you would in the correction step whenever a new measurement is available. The only difference is that your measurement model would change depending on which sensor is giving you the measurement. If your sensors give you data at different rates then you would do something like: prediction, correction1, prediction, correction2 ... and so on, where correction 1 and correction 2 are based on measurements from sensor 1 and sensor 2 respectively.</p>

Title: EKF sensor fusion


问:
How do I convert centre-returning joystick values to dual hobby-motor direction?
Currently my setup is:

2x hobby TT motors connected to a Pi Zero with a motor HAT

Python script running on the Pi which runs a Blynk python client and controls
motors via the gpiozero library

Blynk iOS app using the joystick widget

Motors are attached on either side of a 'homebrew' chassis at the rear, parallel to each other, with a free-rotating wheel at the front.

What I want to do is to smoothly control motor direction & speed using the joystick values.  I have smooth control over speed forward / backwards on the y axis, but want to somehow interpolate x & y for fine control of direction also.

Something like this math.stackexchange question is what I'm looking for, but I have no idea how to convert the accepted answer into code.

答:
Converting a joystick $(x,y)$ into differential-drive $(left, right)$ control is not difficult.  But there are multiple approaches because of different robot and joystick characteristics.  For example, compare the differences between a "playstation-style" controller joystick (which will usually be used in an all-or-nothing kind of way and might have a sloppy dead-band near the center) vs. a large PC gaming flight simulator joystick (which gives you much finer grained control).  Or a narrow 2-wheeled robot (which will rotate in place very fast) vs. a wide, tracked, skid-steer robot (which has lots of rotational friction).  So you can see how you will need to tweak the feel of the controller.

This simplest procedure falls out of the discovery that if you just rotated the joystick 45 degrees, then you can use the joystick's $x$ and $y$ channels directly as the $left$ and $right$ wheel controls.  For example, with the joystick all the way to the upper right corner $(1, 1)$, this is both wheels full speed ahead.  And with the joystick to the upper left corner $(-1, 1)$, this is a full-speed turn in place with left wheel going backwards and the right wheel going forwards.  In other words:

$$
\begin{bmatrix}
left \\
right \\
\end{bmatrix} = \begin{bmatrix}
cos(-45^o) && -sin(-45^o)\\
sin(-45^o) && cos(-45^o) \\
\end{bmatrix} \begin{bmatrix}
x_{joy} \\
y_{joy} \\
\end{bmatrix}
$$

Or in Python code:

```<code>def joy_to_diff_drive(joy_x, joy_y):
    left = joy_x * math.sqrt(2.0)/2.0 + joy_y * math.sqrt(2.0)/2.0
    right = -joy_x * math.sqrt(2.0)/2.0 + joy_y * math.sqrt(2.0)/2.0
return [left, right]
</code>```

But if you use this, you will notice something strange.  The robot will go faster when turning than when going straight.  For example if your joystick axes go between -127 and 127, when going straight they will only be commanded to be 90, and when you put the joystick in a corner to turn with only 1 wheel, you will get a motor command of 180!  Of course you could scale and cap the output to achieve the desired behavior, but the root of the problem is what is described in the post you mention here: https://math.stackexchange.com/questions/553478/how-covert-joysitck-x-y-coordinates-to-robot-motor-speed.  I also discuss it in this post: How to program a three wheel Omni?.  The problem is that the joystick range is square, but you really want it to be circular.

The easiest way to think about the problem from here on is in polar coordinates.  Some Python code again:

```<code># convert to polar coordinates
theta = math.atan2(joy_y, joy_x)
r = math.sqrt(joy_x * joy_x + joy_y * joy_y)
</code>```

Then, let's determine the maximum "r" for the given theta.  (Actually, the math for this is quite simple using "similar triangles", no trig required.)  This let's us determine the actual speed.

```<code># this is the maximum r for a given angle
if abs(joy_x) > abs(joy_y):
    max_r = abs(r / joy_x)
else:
    max_r = abs(r / joy_y)

# this is the actual throttle
magnitude = r / max_r
</code>```

Next we turn this into left and right wheel velocities.  Now we'll use theta instead of the raw joystick left and right.  But we do a similar rotation matrix thing.

```<code>turn_damping = 3.0 # for example
left = magnitude * (math.sin(theta) + math.cos(theta) / turn_damping)
right = magnitude * (math.sin(theta) - math.cos(theta) / turn_damping)
</code>```

As you can see, I threw in something I alluded to earlier.  For many robots, turning in place with full speed is way too fast, so you can turn it down with a damping factor.

Actually, the astute reader will find that for low values of "turn_damping", the output can go above the input.  So some capping may still be required.  I haven't figured out how to get rid of this yet.  I believe it is because we really have an oval space instead of a circle, and this must be taken into account when calculating the magnitude.

Body:
Converting a joystick $(x,y)$ into differential-drive $(left, right)$ control is not difficult.  But there are multiple approaches because of different robot and joystick characteristics.  For example, compare the differences between a "playstation-style" controller joystick (which will usually be used in an all-or-nothing kind of way and might have a sloppy dead-band near the center) vs. a large PC gaming flight simulator joystick (which gives you much finer grained control).  Or a narrow 2-wheeled robot (which will rotate in place very fast) vs. a wide, tracked, skid-steer robot (which has lots of rotational friction).  So you can see how you will need to tweak the feel of the controller.

This simplest procedure falls out of the discovery that if you just rotated the joystick 45 degrees, then you can use the joystick's $x$ and $y$ channels directly as the $left$ and $right$ wheel controls.  For example, with the joystick all the way to the upper right corner $(1, 1)$, this is both wheels full speed ahead.  And with the joystick to the upper left corner $(-1, 1)$, this is a full-speed turn in place with left wheel going backwards and the right wheel going forwards.  In other words:

$$
\begin{bmatrix}
left \\
right \\
\end{bmatrix} = \begin{bmatrix}
cos(-45^o) && -sin(-45^o)\\
sin(-45^o) && cos(-45^o) \\
\end{bmatrix} \begin{bmatrix}
x_{joy} \\
y_{joy} \\
\end{bmatrix}
$$

Or in Python code:

def joy_to_diff_drive(joy_x, joy_y):
    left = joy_x * math.sqrt(2.0)/2.0 + joy_y * math.sqrt(2.0)/2.0
    right = -joy_x * math.sqrt(2.0)/2.0 + joy_y * math.sqrt(2.0)/2.0
return [left, right]

But if you use this, you will notice something strange.  The robot will go faster when turning than when going straight.  For example if your joystick axes go between -127 and 127, when going straight they will only be commanded to be 90, and when you put the joystick in a corner to turn with only 1 wheel, you will get a motor command of 180!  Of course you could scale and cap the output to achieve the desired behavior, but the root of the problem is what is described in the post you mention here: https://math.stackexchange.com/questions/553478/how-covert-joysitck-x-y-coordinates-to-robot-motor-speed.  I also discuss it in this post: How to program a three wheel Omni?.  The problem is that the joystick range is square, but you really want it to be circular.

The easiest way to think about the problem from here on is in polar coordinates.  Some Python code again:

# convert to polar coordinates
theta = math.atan2(joy_y, joy_x)
r = math.sqrt(joy_x * joy_x + joy_y * joy_y)

Then, let's determine the maximum "r" for the given theta.  (Actually, the math for this is quite simple using "similar triangles", no trig required.)  This let's us determine the actual speed.

# this is the maximum r for a given angle
if abs(joy_x) > abs(joy_y):
    max_r = abs(r / joy_x)
else:
    max_r = abs(r / joy_y)

# this is the actual throttle
magnitude = r / max_r

Next we turn this into left and right wheel velocities.  Now we'll use theta instead of the raw joystick left and right.  But we do a similar rotation matrix thing.

turn_damping = 3.0 # for example
left = magnitude * (math.sin(theta) + math.cos(theta) / turn_damping)
right = magnitude * (math.sin(theta) - math.cos(theta) / turn_damping)

As you can see, I threw in something I alluded to earlier.  For many robots, turning in place with full speed is way too fast, so you can turn it down with a damping factor.

Actually, the astute reader will find that for low values of "turn_damping", the output can go above the input.  So some capping may still be required.  I haven't figured out how to get rid of this yet.  I believe it is because we really have an oval space instead of a circle, and this must be taken into account when calculating the magnitude.


问:
Problem in Jacobian matrix
I have a 5DOF robotic arm were 4 joints rotates around a Z axis and one around an x-axis. I am not that good with the Jacobian, but I think I have this.
$$
J[v,\Omega]=[Z_{i-1}×(O_n-O_{i-1}), Z_{i-1} Z_{i-1}×(O_n-O_{i-1}), z_{i-1} ... Z_{n-1}×(O_n-O_{n-1})].
$$

I am a bit confused about Jacobian matrix on the part  ($Z_{i-1}$), Eg say $Z_1=[0,0,1]$. whereby I know that "$Z$" is the axis of actuation. What about when one of the joint in my robotic arm rotates around an x-axis does it affect the matrix??

Is it correct to write, $X_{i-1}$, eg $X_1=[1,0,0]$?

答:
Usually the axes of robots are determined using the so called Denavit Hartenberg convention. Based on this convention all actuated axes are Z axes (and the transformation matrices are defined in a way which enables this). This is why everybody assumes that the actuation axis is the Z axis (like in the formula above).

If you do not follow the DH Convention, you can have an actuation axis, which is not Z. In this case it is correct to use $X_i$ instead of $Z_i$. Your assumption, that is equals $[1, 0, 0]$ might be correct. If the $O$ matrices are $4x4$ in size, then $X$ would equal $[1, 0, 0, 1]$ (as in this case $Z$ would also be $[0, 0, 1, 1]$). Also, please make sure that the positive movement (rotation or translation) direction of the motion axis matches the positive direction of the geometrical axis.

Title: Problem in Jacobian matrix


问:
What is my last rotation matrix for the last three angles when i have found the first three when doing inverse kinematics to a 6dof robot?
I am doing inverse kinematics for a 6dof robot.

I have used the geometrical method to describe the angles from one to three. I have trouble doing the next part. I know I that I have found the location by trigonometry* and now I need to find the rotation algebraically. Since it is the rotation I can use the rotation matrix that describes the rotation from 0 to 6. Since I know my angles from 0 to 3 I can do this:

$T_6^0=T_0^3\cdot T_3^6$

You can take the inverse of $T_0^3$ on both sides and you end up with

$(T_3^0)^{-1} \cdot T_6^0= T_3^6$

I don't know what to do now. I know I have to equate the different terms but what terms. How do I get the rotation matrices to solve the inverse kinematics? Essentially my question is how do I get the rotation matrices that will let me solve the problem. How do I insert angle $\theta_1$ to $\theta_3$?

*Just a side question: They say that in 6d0f freedom robot the first three give the location and the last three the rotation. However if I change $\theta_5$ on a 6dof the location changes too? How come? The only one who doesn't change the location is $\theta_6$.

Thank you for reading and hopefully answering,
Have a nice day.

Here is my matlab code. I have also added a picture of the lengths and the DH-table we made.
https://i.imgur.com/6QTCO2P.png
[![enter image description here][1]][1]

The desired angles are 10,20,30,40,50,60 for theta1 to theta 6.

    %% Definitions.
    clear all
    syms theta1 theta2 theta3 theta4 theta5 theta6

    %% MatriX
     T01=[ cos(theta1), -sin(theta1), 0, 0;
    sin(theta1),  cos(theta1), 0,   0;
                  0,                   0, 1,   0;
                  0,                   0, 0,   1];

    T12= [cos(theta2) -sin(theta2) 0 25;
          0                0     -1 0;
          sin(theta2) cos(theta2)  0 0;
          0              0        0 1];

    T23=[ cos(theta3 + pi/2), -sin(theta3 + pi/2), 0, 315;
    sin(theta3 + pi/2),  cos(theta3 + pi/2), 0,   0;
                  0,                   0, 1,   0;
                  0,                   0, 0,   1];

     T34= [cos(theta4) -sin(theta4) 0 -35;
          0                0     -1 -365;
          -sin(theta4) -cos(theta4)  0 0;
          0              0           0 1];

      T45= [cos(theta5) -sin(theta5) 0 0;
          0                0     -1 0;
          -sin(theta5) -cos(theta5)  0 0;
          0              0           0 1];

      T56= [cos(theta6) -sin(theta6) 0 0;
          0                0         1 0;
          -sin(theta6) -cos(theta6)  0 0;
          0              0           0 1];

    %% Wrist and tool
    T6W= [1  0  0  0;
          0  -1  0  0;
          0  0  -1 -80;
          0  0  0  1];

      TB0= [1 0  0  0;
          0 -1 0  0;
          0 0  -1  400;
          0 0  0  1];

    %% Placement of End Effector
    TBTtarget=eulerZYX2T(563.879,-139.427,-34.414,100.551*pi/180,29.536*pi/180,178.188*pi/180); %location of end effector. // Location from robotdk
    TWT=eulerZYX2T(556.600,-168.144,-86.590,100.551*pi/180,29.536*pi/180,178.188*pi/180);% the tool with respect the base  // FK works with Robodk
    %% Changing the target
    T06=inv(TB0)*TBTtarget;  %Remove the base so i have treansfomation 0->6

    t06=[T06(1,4);
        T06(2,4);
        T06(3,4)];

    %% step one
     r_x=TBTtarget(1,3)
     r_y=TBTtarget(2,3) %The rotation from T06. This is from the third collumn.
     r_z=TBTtarget(3,3)

    direction=[r_x,;
               r_y;     %made into a roation vector.   These are the values from the third collumn
               r_z];
    t04=t06-(direction*-80)% Here it is as you wanted it. My d is -80. D6 is the lenght from frame 6 to the wrist right?
    %% step Two
    T04=T01*T12*T23*T34;    % Here i do forward kinematics, I defined the matrixes above but this gives Transformation from 0 to 4.
    x=T04(1,4)               % Here i Equal x,y,z to the so I can  do this
    y=T04(2,4)                %x=f(J1, J2, J3), y=f(J1, J2, J3), z=f(J2, J3)
    z=T04(3,4)                  % in the next section.
    %% step Three solving equations
    eqn1 = x==t04(1,1);
    eqn2 = z==t04(2,1);      %% Here the equations should had been solved but there are unsolveable.
    eqn3 = y==t04(3,1);

```

答:
Whether you can use this technique depends on the actual arrangement of your actual robot. I will give an overview of the approach given some assumptions about your robot and then if you add details of your robot e.g. diagram of the joints I can provide more detail if required.

Assumptions:

Your robot arm has the same arrangement in the last 3 joints as the robot shown in slide 2 here: https://www.uio.no/studier/emner/matnat/ifi/INF3480/v14/undervisningsmatriale/lec08_inv_jaco.pdf (this also describes the same approach that I have written here)

Joints are labelled J1 to J6.

Frames are labelled F0 to F6.

${}^0T_1$ is a transformation from F0 to F1 and is a function of J1, ${}^1T_2$ is a function of J2 etc.

You are able calculate all transformations as a function of joint angles (forward kinematics).

Now look at your robot and consider the following:

The position of F4 e.g. the translation component xyz of ${}^0T_4$ is not affected by changes in J4, J5 or J6. I.e. It is a function only of J1, J2 J3.
(I think this is where you've misunderstood that it is the last frame that should be unaffected)

${}^3R_6$ is not affected by changes in J1, J2, J3 i.e. it is a function of only J4, J5, J6.

For a given ${}^0T_6$ i.e. end position (that you choose), the position of F4 is fixed i.e. not affected by any joints.

Do not continue until you're confident that the above statements are correct.

So to do the IK:

Given a desired pose ${}^0T_6$, you can calculate the actual numerical values for x,y,z of ${}^0T_4$ - it is the F4-F6 link lengths $d$ (which are known values) back down the z axis of F6 (and J5 which is the same). The z axis of F6 is the 3rd column of the rotation matrix ${}^0R_6$ (which is a known value, because it's whatever you set it to).
$$
\text{direction} = \text{3rd column of } {}^0R_6 \\
{}^0t_4 = {}^0t_6 - d * \text{direction} \\
$$

Where 't' is the translation part only from the full transformation matrix 'T'. You will get a result with known numerical values.

Calculate FK for ${}^0T_4$. The xyz components will only depend on J1, J2, J3.
You will have something like x=f(J1, J2, J3), y=f(J1, J2, J3), z=f(J2, J3)

Set the results of (1) and (2) equal to each other and you now you have 3 equations (one for x, one for y, one for z).

Solve for J1, J2, J3. If this is what you've already done so far then great. Now you know J1, J2, J3 for a given ${}^0T_6$.

Calculate ${}^0R_3$ using forward kinematics.
Since you know J1, J2, J3 you will get a matrix of numbers (no unknowns).

Calculate ${}^3R_6$ using forward kinematics.
Since you don't know J4, J5, J6 yet, the values in this matrix will be expressed as a function of those joints.

Now you have ${}^0R_3$ with known values, ${}^0R_6$ with known values, and ${}^3R_6$ as a function of J4, J5, J6.

$$
{}^0R_3 * {}^3R_6 = {}^0R_6 \\
({}^0R_3)^{-1} * {}^0R_3 * {}^3R_6 = ({}^0R_3)^{-1} * {}^0R_6 \\
{}^3R_6 = ({}^0R_3)^{-1} * {}^0R_6
$$

You have 1 matrix of unknowns on the left, and a matrix of known values on the right i.e. you have 9 equations.

Solve these equations to find J4, J5, J6.

And you're done!
Hopefully if you are still unsure you can narrow it down to one (or more) of the steps above.

Note on doing FK with unknown variables:
If you are doing this by hand it's quite easy to make mistakes because there are a lot of terms. It is much easier to use a symbolic algebra library, they are available for matlab and python and probably other languages. Wherever something e.g. a, d, alpha etc is zero, put it as zero, it will make the results much simpler.


问:
Robust Controls of Quadcopter
Can anybody please guide me about the robust modeling and control of a quadcopter. Where can i find the robust modeling that can be understandable for me as i am a undergraduate student. I am doing this in my FYP. Please suggest me some book that is written on robust modeling and control on quadcopter. Also, which uncertainties can we model? What are the best robust controllers. </p>

答:
Check this out

https://www.hindawi.com/journals/jr/2018/5607362/

Its not a book but its a very good paper.

I also recommend you to read some general robust Control and think about how would you do for a quadrotor

Title: Robust Controls of Quadcopter


问:
What is the reasoning for solving inverse kinematics problems?
I am a student, and I am studying robotic arm manipulators. I am focusing on the concept of inverse kinematics, I have understood is the process of finding, from the direct kinematics, the joint varibles.

For easy problems, such as a 2R manipulatoer, I am able to solve algebraically the inverse kinematics problem, but for more complex problems I really get in troubles, and almost every time I can't solve the preoblem.

For example, consider the following arm manipulator:

if I compute the direct (or forward) kinematics of it, I get:

I have tried computing analitically the inverse kinematics for it, but without success.

My problem is that I don't understand how to tackle the problem of the inverse kinematics and so I don't know how to move.

What is the reasoning for solving inverse kinematics problems (both easy and hard) ?

答:
Deriving analytic IK for higher DOFs is tricky. Numerical IK solvers are a good alternative usually, unless you absolutely must use analytic IK.

In various robotics textbooks, there are examples on how to derive analytical IK for 6R robots. The way it's done is by considering position IK and rotation IK independently (note that this is possible only if the wrist is spherical).


问:
How to handle landmarks in Bundle Adjustment?
At the moment I try to understand Bundle Adjustment on a level to be able to implement it on my own. The major point I'm still confused at is the management of the landmarks. So, let's think of a few frames, that all share  some landmarks with each other (like in the image below).

I can triangulate the same corresponding feature in image 1 to image 2, from image 2 to image 3 and so forth. However, I get (slightly) different results for the same point in the world. I first tried it with fusing all the landmark estimates from the different triangulations (see an older post). But, I'm not sure if that is really the way to go.

If my question is still a bit to unclear, please let me know and I will improve it. I'm looking forward to interesting answers.

答:
This question is already marked as answered, but I found an interesting paper (To Bundle Adjust or Not: A Comparison of Relative Geolocation
Correction Strategies for Satellite Multi-View Stereo, Marí et al.) that suggests an additional way to cope with the 3D points.</p>


答:
The reason for solving this problem becomes very obvious when you think of how to grab things. For example, consider a camera that is able to locate an object in the working range of the robot. Suppose you know the relation between camera and robot, but this is another problem...

The visual system tells you the coordinates where your target is located. Now the robot has to move its joints to reach the object. Arises the question, what are the correct joint parameters? You get the answer from inverse kinematics. And yes, this can be hard to answer and mostly (for real robots) you need to use iterative numerical approaches.

Title: What is the reasoning for solving inverse kinematics problems?

Body:
I am a student, and I am studying robotic arm manipulators. I am focusing on the concept of inverse kinematics, I have understood is the process of finding, from the direct kinematics, the joint varibles.

For easy problems, such as a 2R manipulatoer, I am able to solve algebraically the inverse kinematics problem, but for more complex problems I really get in troubles, and almost every time I can't solve the preoblem.

For example, consider the following arm manipulator:

if I compute the direct (or forward) kinematics of it, I get:

I have tried computing analitically the inverse kinematics for it, but without success.

My problem is that I don't understand how to tackle the problem of the inverse kinematics and so I don't know how to move.

What is the reasoning for solving inverse kinematics problems (both easy and hard) ?


问:
Help with code that is supposed to drive a servo controller
I posted a similar question before as I was just getting started with the project but I wasn't specific enough so got a weak response from the SE community. But now I am at a point where I have Python code which is supposed to rotate a servo through Pololu's Maestro Serial servo controller (https://www.pololu.com/product/207). Based on the "Serial Interface" section of the user's guide (https://www.pololu.com/product/207/resources), I sent a sequence of numbers starting with decimal 170 and 12, which are the "first command byte" and the "device number data byte", respectively. The user guide says that 12 is the default device number, so I tried changing it to 18 because that's how many servos my servo controller can drive. But that doesn't make much difference because the servo doesn't rotate at all. The numbers after that are the same as the example from the user's guide. I am not sure what the 4, 112 and 46 are doing, but the 0 targets the servo port "0" on the servo controller (the port to which my servo is connected). The servo doesn't move, regardless of what sequence of numbers I put in. I have very little experience, so if you guys could point me in the right direction or at least point to some useful resources on the web, I'd be very grateful.</p>

```<code>import serial
import struct
import time
import Adafruit_BBIO.UART as UART

drive_Motor_Port = serial.Serial(port = "/dev/ttyO1", baudrate=9600)
drive_Motor_Port.close()
drive_Motor_Port.open()

drive_Motor_Port.write(chr(170)); # User guide says we must start with OxAA = 170
drive_Motor_Port.write(chr(18)); # Device number
drive_Motor_Port.write(chr(04));
drive_Motor_Port.write(chr(00));
drive_Motor_Port.write(chr(112));
drive_Motor_Port.write(chr(46));

time.sleep(5);
</code>```

答:
Sorry that this response is very late. I had a little trouble when I first came across the Maestro board as well while working on a project.

Doing research on controlling the board, I came across this Github:

https://github.com/FRC4564/Maestro

I wanted more comprehensive instructions then the README provided and also wanted to refactor the code to match PEP8 standards for my modules. I have put up the code here in order to help anyone else who finds themselves working with the Maestro board. Hope it helps:

https://github.com/RedSunDave/maestro-python3

Important things to remember:

Make sure to run ./MaestroControlCenter go to the "Serial Settings" tab and change the "Serial Mode" to "USB DUAL PORT

Install the appropriate drivers and "pyserial" module

Look through the README, it provides step by step setup instructions.

Here is a quick sample code for using the maestro module:

```<code>import maestro
servo = maestro.Controller()
servo.set_acceleration(0,4)   #set servo 0 acceleration to 4
servo.set_target(0,6000)      #set servo to move to center position
servo.set_speed(1,10)         #set speed of servo 1
x = servo.get_position(1)     #get the current position of servo 1
servo.close()
</code>```

Good luck and hope this answer helps anyone who comes across.

Body:
Sorry that this response is very late. I had a little trouble when I first came across the Maestro board as well while working on a project.

Doing research on controlling the board, I came across this Github:

https://github.com/FRC4564/Maestro

I wanted more comprehensive instructions then the README provided and also wanted to refactor the code to match PEP8 standards for my modules. I have put up the code here in order to help anyone else who finds themselves working with the Maestro board. Hope it helps:

https://github.com/RedSunDave/maestro-python3

Important things to remember:

Make sure to run ./MaestroControlCenter go to the "Serial Settings" tab and change the "Serial Mode" to "USB DUAL PORT

Install the appropriate drivers and "pyserial" module

Look through the README, it provides step by step setup instructions.

Here is a quick sample code for using the maestro module:

import maestro
servo = maestro.Controller()
servo.set_acceleration(0,4)   #set servo 0 acceleration to 4
servo.set_target(0,6000)      #set servo to move to center position
servo.set_speed(1,10)         #set speed of servo 1
x = servo.get_position(1)     #get the current position of servo 1
servo.close()

Good luck and hope this answer helps anyone who comes across.


问:
Motion of a mobile platform inaccurate than industrial robot arm
It is known that with a 7DoF industrial you can achieve an accuracy of ~2mm.
However, the motion with a mobile platform is known to be more inaccurate and expensive. </p>

<p>I can follow why the motion is more expensive, probably because of the larger mass. But what is the reason for this inaccuracy? </p>

答:
Instead of motion accuracy, usually positioning accuracy is measured.

Positioning of a robot arm is relative to its fixed base. We can measure the relative angular positions of its joints very accurately using encoders and using a geometrical model of the robot, these measurements give an accurate estimate of the position of the end effector (or tool center point) of the robot. Encoders are relatively inexpensive and used in almost every industrial robot arm

In case of mobile robots there is no fixed base, there is no relatively inexpensive way of measuring its position relative to a fixed coordinate system in space. In some cases this fixed coordinate system is not even visible form the robot (e.g. in a cluttered room). There are very expensive systems which could measure the position if there is direct visibility (e.g. laser tracker systems) but this leads to other complications (line of sight visibility) and significant cost increase. If we could measure the position accurately, we could position the robots accurately. Instead robots use onboard laser scanner (inaccurate compared to laser tracker systems), cameras and-or other sensor systems and calculates its position relative to features in its environment, which are themselves sometimes inaccurately positioned on a map.

It all comes down to measurements. If we can measure accurately, we can usually also position accurately.


问:
Unable to read pushbutton press properly in Arduino
I am trying to use a push button in order to know and print number of time a push button is pressed using a counter.But everytime i press the button , counter get incremented to sometime 3 and sometime 5 and some time counter does start >100 and continue.

I had preferred the this link for wiring PUSH button with arduino.

and here is my code

```<code>const int buttonPin = 2;     // the number of the pushbutton pin
const int ledPin =  13;      // the number of the LED pin

// variables will change:
int buttonState = 0;         // variable for reading the pushbutton status
int count = 0;
void setup() {
  // initialize the LED pin as an output:
  pinMode(ledPin, OUTPUT);
  // initialize the pushbutton pin as an input:
  pinMode(buttonPin, INPUT);
  pinMode(buttonPin, INPUT);
  Serial.begin(9600);
    buttonState = digitalRead(buttonPin);
  digitalWrite(ledPin,LOW);
}

void loop(){
    // read the state of the pushbutton value:
  buttonState = digitalRead(buttonPin);

  // check if the pushbutton is pressed.
  // if it is, the buttonState is HIGH:
  if (buttonState == HIGH) {

    // turn LED on:
    digitalWrite(ledPin, HIGH);
    count = count + 1;
    Serial.print(count);
  }
  else {
    // turn LED off:
    digitalWrite(ledPin, LOW);
  }
}
</code>```

I dont know why count is coming absurdly and unevenly.

答:
What your experiencing is known as "button bounce". When you push a standard pushbutton, it actually makes and breaks contact many times very quickly for a few microseconds, usually enough to make the processor detect between 10 and 100 pushes, or more. There are many different methods of "debouncing" a pushbutton. The easiest approach is to add a small delay in your code right after you check if the pushbutton is pressed:

```<code>if(buttonState == HIGH){
    delay(10);
    //turn the light on, etc.
}
</code>```

There are other methods, both hardware and software. The most elegant method which will not interrupt the running of other code is to have a timer that scans the pushbutton periodically (i.e. every 5 milliseconds or so), and sets a boolean variable to TRUE when it detects a push. You could also rig up a small capacitor to the button; the schematic is available on the internet.

Body:
What your experiencing is known as "button bounce". When you push a standard pushbutton, it actually makes and breaks contact many times very quickly for a few microseconds, usually enough to make the processor detect between 10 and 100 pushes, or more. There are many different methods of "debouncing" a pushbutton. The easiest approach is to add a small delay in your code right after you check if the pushbutton is pressed:

if(buttonState == HIGH){
    delay(10);
    //turn the light on, etc.
}

There are other methods, both hardware and software. The most elegant method which will not interrupt the running of other code is to have a timer that scans the pushbutton periodically (i.e. every 5 milliseconds or so), and sets a boolean variable to TRUE when it detects a push. You could also rig up a small capacitor to the button; the schematic is available on the internet.


问:
Automatic License Plate Recognition System in Highway
We are working on a computer vision project where an automated robotic system will be deployed on a 3-lane highway.

We are using a bosch IP camera (I don't remember the model exactly, but it cost around $ 1k). The video resolution is 1920 by 1080 @ 60 FPS.

The problem is our LP (license plate) detector is really struggling to read the license plates. I have attached a sample frame from the video that we extracted from the IP camera.

 original pasteboard image link

If we cannot get this camera to provide the necessary resolution, how can we determine which cameras with a simple interfacing option (RTSP or streaming option) are optimized for text-based detection? I suspect due to compression the text quality is getting somewhat deteriorated. Our detector can run at 30 FPS max. so anything with >= 30 FPS and good resolution (especially text) would be an option. (I do bufferless processing, so yes my model only processes half the frames at a time. But, I can configure my detector to run at different FPS, the thing is I can't go lower than 30 FPS for a camera.)

Our budget is around (USD 1k to 2k) for the camera.

I'm open to any suggestion (maybe using a different setup, optimizing the current camera itself) from the experts in the field.

答:
I think your big problem is that in the worst case, the characters on your number plates are only a few pixels wide, and the script really doesn't look ideal for OCR anyway.

Camera resolution

By my calculations, at the top of each frame the resolution is around 6mm/pixel (assuming a 7m wide highway which is 1200 pixels wide at that point in the image).

A higher resolution camera sensor might help, but you may be limited more by the lens than by the sensor. A higher resolution camera might just give you more, blurrier pixels.

Before you go buying another camera, I would suggest that you need to determine what the limiting factor on image quality is.

Camera field of view

You may get significantly better results with three cameras, one per lane but zoomed in closer.

I would make sure that there is an overlap of half the width of a number plate between each cameras field of view, so a vehicle switching lanes would be captured on both cameras rather than a number plate being split between the two. You could stitch the images together after doing perspective correction, but that has other problems.

Assuming the same camera angle and resolution with a 2.3m lane and 0.3m overlap, that would increase your resolution to around 2.2mm/pixel (2.6m/1200) which is better than an expensive 4k camera trying to cover a 7m width (also, three HD cameras would be 3/4 the bandwidth of one 4k camera and 3/4 of the pixel processing).

Camera orientation

Whether you orient the cameras in landscape or portrait will depend on resolution and field of view requirements, but also on traffic speed. While landscape maximises your resolution, portrait maximises the time dimension.

If you orient the camera in landscape and a vehicle travelling at the maximum expected speed can pass the field of view of the camera without the number plate being captured, then orienting the camera in portrait would increase the speed it would have to be travelling at to have a chance of avoid detection.

For instance, if your field of view is 2.6m wide and 1.46m deep and a car speeds past at 100mph (161kph, 45m/s), then at 30 frames per second (1.5m/frame), there is a (small) chance that only the top half of the number plate will be visible on one frame, while only the bottom half of the number plate will be visible on the next. If a vehicle is going faster than that, there is an increasing chance that it may not be in frame at all.

Reorienting the camera to portrait would cut the top of frame resolution from 2.2mm/pixel to 3.9mm/pixel (2.62600m/(1200/16*9)), but increase time resolution significantly, potentially giving you several frames containing the numberplate of even the fastest moving vehicles (assuming the shutter speed isn't too high enough that the individual frames are blurry).

Summary

I think before you spend more money, you need to characterise what you can achieve with the hardware you already have. If you can get good results with your current \$1000 camera when looking at a single lane, then your \$2000 budget could buy you two more identical cameras to extend the system to all three lanes (you may even be able to get a discount on buying a pair).

If this is a prototype, and you will be doing other installations elsewhere later, then you can look into cost reduction mechanisms once the prototype is performing well and you you have a test bed for trying out cheaper components.


问:
Inverse Kinematics does not give the desired joint angles
I have a location of a tool with respect to the base. That location is described by an x,y,z coordinates and three rotations. I call that $Target$. I want to make a geometrical approach on the first three angles. I first have to remove some parts of my target to get it to the wrist and also remove the base. This. First I remove the base, then the wrist to tool(TWT) and lastly the frame 6 wrist(T6T). Then you end up with transformation from 0 to 6 (T60). There are 6 frames.

Essentially we did this

$T06=TB0^{-1}\cdot target \cdot TWT^{-1} \cdot T6W^{-1}$

When I as shown in the figure, when theta 1 is calculated for it makes athis equation.

$\theta_1=tan^{-1}(y/x)$

This gave the wrong answer. Since the trigonometry was straightforward, my hunch is that the above statement where we changed the transformation was at fault. Did we do it correctly is my question.

答:
I think your transforms are assembled in reverse order maybe? I didn't really follow your explanation of what the frames are (and they aren't labeled on your drawing), so I'll say that you can compose a transform matrix the following way:

$$
T_0^6 = T_0^1 T_1^2T_2^3 ... T_5^6 \\
$$

That is, to get from Frame 0 to Frame 6, you multiply together the transforms from 0 to 1, then 1 to 2, then 2 to 3, and so on.

This seems to be kind of similar to what you wrote:

$$
T06=TB0^{−1}⋅target⋅TWT^{−1}⋅T6W^{−1} \\
$$

But I'm not sure where your missing frames are (there are 6, right?) or what $target$ is (a point or a frame or a transform?)

But if you had some point in the end-effector frame (frame 6), then you would represent that via the base-to-frame-6 transform:

$$
T_B^6 = T_B^1T_1^2...T_5^6 \\
$$

And then

$$
\overrightarrow{x}_B = T_B^6 \overrightarrow{x}_6 \\
$$

If you wanted to express a point given in base coordinates in the end effector's frame, just invert the transform:

$$
\overrightarrow{x}_6 = (T_B^6)^{-1} \overrightarrow{x}_B \\
$$

because:
$$
\begin{array}{ccc}
\overrightarrow{x}_B &=& T_B^6 \overrightarrow{x}_6 \\
(T_B^6)^{-1}\overrightarrow{x}_B &=& (T_B^6)^{-1}T_B^6 \overrightarrow{x}_6 \\
(T_B^6)^{-1}\overrightarrow{x}_B &=& I \overrightarrow{x}_6 \\
(T_B^6)^{-1}\overrightarrow{x}_B &=& \overrightarrow{x}_6 \end{array}
$$


答:
The equation

$T06=TB0^{-1}\cdot target \cdot TWT^{-1} \cdot T6W^{-1}$

seems to be the probem.

The first equality which can be written is:

$$
T_{robot} = T_{TCP}
$$

Two transformation matrices which transform the same base frame to the same TCP Frame, but use different atomic transformations. $T_{robot}$ uses joint angles and the $T_{TCP}$ matrix uses the end effector coordinates as atomic transformations.

$$T_{robot} = T_{base} \cdot   T_{0,6} \cdot  T_{tool}$$
$$T_{TCP} = T_x \cdot T_y \cdot  T_z \cdot  R_{X} \cdot  R_{Y} \cdot  R_{Z}$$
$$
T_{0,6} = T_{0,1} \cdot  T_{1,2} \cdot ...\cdot  T_{5,6}\\
$$

If the inverse kinematics problem is addressed, only $T_{0,6}$ has unknowns, every other matrix is known and fully defined.

$$ T_{base} ^{-1} \cdot  T_{robot} \cdot  T_{tool}^{-1} = T_{0,6}$$

The first step for solving inverse kinematics for robots with a wrist is finding the angle of the first joint. This is done by expressing the wirst point coordinates on both sides of the equation

$$ T_{base} ^{-1} \cdot  T_{robot} \cdot  T_{tool}^{-1} = T_{0,1} \cdot  T_{1,2} \cdot ...\cdot  T_{5,6}$$

$$ T_{base} ^{-1} \cdot  T_{robot} \cdot  T_{tool}^{-1} \cdot T_{5,6}^{-1} \cdot \cdot T_{4,5}^{-1} = T_{0,1} \cdot  T_{1,2} \cdot ...\cdot  T_{3,4}$$

In the matrix on the left hand side the last columnt should only contain known paramters, this is the X, Y, Z coordinate of the wrist point. Using the $atan2()$ function, the $q_1$ joint angle of the first axis can be calculated.

Title: Inverse Kinematics does not give the desired joint angles

Body:
I have a location of a tool with respect to the base. That location is described by an x,y,z coordinates and three rotations. I call that $Target$. I want to make a geometrical approach on the first three angles. I first have to remove some parts of my target to get it to the wrist and also remove the base. This. First I remove the base, then the wrist to tool(TWT) and lastly the frame 6 wrist(T6T). Then you end up with transformation from 0 to 6 (T60). There are 6 frames.

Essentially we did this

$T06=TB0^{-1}\cdot target \cdot TWT^{-1} \cdot T6W^{-1}$

When I as shown in the figure, when theta 1 is calculated for it makes athis equation.

$\theta_1=tan^{-1}(y/x)$

This gave the wrong answer. Since the trigonometry was straightforward, my hunch is that the above statement where we changed the transformation was at fault. Did we do it correctly is my question.


问:
What are the numbers to the left when logging with ROS_INFO_STREAM?
Here's what I currently see in my terminal when logging:

```<code>[ INFO] [1589145832.980129261, 843.227000000]: DriveToTarget received:
linear_x=0.00
angular_z=0.00
[ INFO] [1589145832.980202376, 843.227000000]: Drive parameters sent:
linear z = 0.000000
angular z = 0.000000
[ INFO] [1589145833.363180798, 843.266000000]: Driving bot
</code>```

I have code to log parameters received, sent and indication that the bot is moving. One example is:

```<code>DriveToTarget received:
linear_x=XXXXXX
angular_z=YYYYY
</code>```

I can tell that the [INFO] is logged because it is ROS_INFO_STREAM but what are the 2 doubles that are logged before my message, i.e.,  [1589145832.980129261, 843.227000000]?

答:
The first number is the walltime (sec seconds and nsec nanoseconds since January 1, 1970 UTC).

The second number is the simulated time (also sec.nsec) since you started your simulation environment. (I assume you use some simulation environment like gazebo or similar)

You can configure the rosconsole format as you like by setting the ROSCONSOLE_FORMAT environment variable.

```<code>export ROSCONSOLE_FORMAT='[${severity}] [${time}]: ${message}'
</code>```

have a look at http://wiki.ros.org/rosconsole (Section 6. Console Output Formatting) for the complete list of available variables

happy logging!

Body:
The first number is the walltime (sec seconds and nsec nanoseconds since January 1, 1970 UTC).

The second number is the simulated time (also sec.nsec) since you started your simulation environment. (I assume you use some simulation environment like gazebo or similar)

You can configure the rosconsole format as you like by setting the ROSCONSOLE_FORMAT environment variable.

export ROSCONSOLE_FORMAT='[${severity}] [${time}]: ${message}'

have a look at http://wiki.ros.org/rosconsole (Section 6. Console Output Formatting) for the complete list of available variables

happy logging!


问:
How do I assign the origin of the frames in the Denavit Hartenberg convention?
I am studying the Denavit Hartenberg and to practice I am using the following manipulator:

I have been able to assign some of the frames almost correctly, but i did some mistakes I cannot understand. First of all, the correct solution for this manipulator is:

now, in my attemps, I did some mistakes, which are the following.

In the first place, I did not use the frame zero as the frame of the prismatic joint, which is the first joint, bit I decided to define a frame 1 for it, but this is wrong apparently, but why? , to me it seemed natural to define this additional frame.

Another mistake I have done is to not let coincede frame 1 with frame 2, but also in this case I defined an additional frame, which is wrong.

So, I have a problem understanding how to say when I can define two frames in the same point and when not.

How do I understand when to to this?

So, to be as clear as possible, my question is:

How do I assign the origin of the frames in the Denavit Hartenberg convention and how do I know if two frames have the same origin?

答:
There is not one correct DH Frame assignment, but there are many. Even if you do not get the same results, you might still have a correct frame assignment.

The first, base frame at the root of the robot, sometimes called robroot frame is the reference frame, relative to this are the TCP coordinates defined, this is why it is needed. A this is a frame relative to which the TCP coordinates are defined, in many cases it is application dependent, not necessarily DH dependent. If the application does not specify it, then you are free to define it as you prefer. In this case the best practice is at the base of the robot, with z pointing up, against gravity.

Adding a frame with no joint variable is generally not practiced in DH. An additional atomic transformation, included in an additional frame, can be also added as an offset to the joint variable and the effect would be the same. E.g. a pure transformation matrix on the z axis is the same as adding a z offset to the next DH frame. Generally an absolute minimum amount of matrices and as many 0 valued DH parameters as possible should be preferred.

Generally, when you can express two frames with the same origin it is preferred. If you can do this or not depends on the joint configuration. If the joints are collinear you probably can express them using the same origin.


答:
Chris is right, the problem is that the mechanical contact of the switch is bouncing. However, I disagree with his statement that the most elegant solution is polling. Polling is very inefficient for the task of counting how many times a button was pressed, and so I decided to post my own answer for clarification:

Interrupts are what you want. Polling is used when you want to constantly capture information, for example from a range sensor. Interrupts give you time sensitive control, and only performs processing at the exact time you need it.

For a more thorough answer...

This design problem is commonly referred to as "switch debouncing." As Chris correctly mentions, you can do this in hardware or software. The optimal method of implementation with respect to count accuracy and CPU load is to use hardware to smooth out the signal and connect your button to an interrupt pin on the Arduino. You can then use the built in attachInterrupt() function to eliminate the need to poll constantly. Polling is a waste of CPU power. The attachInterrupt() function will assign an interrupt handler that will be called whenever the interrupt pin sees a RISING or FALLING edge (this depends on how you have your switch electrically connected), or even more generally on a CHANGE. This would look similar to the following:

```<code>attachInterrupt(0, buttonHandler, RISING);

void buttonHandler() {
   // turn LED on:
  digitalWrite(ledPin, HIGH);
  count = count + 1;
  Serial.print(count);
}
</code>```

This page explains the problem of switch debouncing and shows some example hardware circuits you can easily implement: http://www.elexp.com/t_bounc.htm

Note: You probably won't be able to see the led go HIGH because it will be handled so quickly. This code will increment the count once every time the button is pressed or released, depending on whether you attach the interrupt to the rising or falling edge of the signal.

Edit for OP's comment:
If you want the serial.print to say that the button is pressed the entire time, then add a while loop in the interrupt:

```<code>void buttonHandler() {
   // turn LED on:
  digitalWrite(ledPin, HIGH);
  count = count + 1;
  Serial.print(count);
  while(digitalRead(buttonPin)) //This will loop until the button is released
    Serial.print("ON");
}
</code>```

Please note that this will cause your program to stay inside the interrupt handler, and your main function will not execute. In your main function, you can then have a Serial.print("OFF").

You may also be able to use a boolean that is set to true in your interrupt handler:

```<code>void buttonHandler() {
   // turn LED on:
  digitalWrite(ledPin, HIGH);
  count = count + 1;
  Serial.print(count);
  buttonWasPressed = true;
}
</code>```

That way, in your main you can have an if statement to prevent your code from having to loop in the interrupt handler:

```<code>void loop() {
  if(buttonWasPressed) {
    while(digitalRead(buttonPin)) {
      Serial.print("ON");
    }
    buttonWasPressed = false;
  }
  else
    Serial.print("OFF");
}
</code>```

Title: Unable to read pushbutton press properly in Arduino

Body:
I am trying to use a push button in order to know and print number of time a push button is pressed using a counter.But everytime i press the button , counter get incremented to sometime 3 and sometime 5 and some time counter does start >100 and continue.

I had preferred the this link for wiring PUSH button with arduino.

and here is my code

```<code>const int buttonPin = 2;     // the number of the pushbutton pin
const int ledPin =  13;      // the number of the LED pin

// variables will change:
int buttonState = 0;         // variable for reading the pushbutton status
int count = 0;
void setup() {
  // initialize the LED pin as an output:
  pinMode(ledPin, OUTPUT);
  // initialize the pushbutton pin as an input:
  pinMode(buttonPin, INPUT);
  pinMode(buttonPin, INPUT);
  Serial.begin(9600);
    buttonState = digitalRead(buttonPin);
  digitalWrite(ledPin,LOW);
}

void loop(){
    // read the state of the pushbutton value:
  buttonState = digitalRead(buttonPin);

  // check if the pushbutton is pressed.
  // if it is, the buttonState is HIGH:
  if (buttonState == HIGH) {

    // turn LED on:
    digitalWrite(ledPin, HIGH);
    count = count + 1;
    Serial.print(count);
  }
  else {
    // turn LED off:
    digitalWrite(ledPin, LOW);
  }
}
</code>```

I dont know why count is coming absurdly and unevenly.

Body:
Chris is right, the problem is that the mechanical contact of the switch is bouncing. However, I disagree with his statement that the most elegant solution is polling. Polling is very inefficient for the task of counting how many times a button was pressed, and so I decided to post my own answer for clarification:

Interrupts are what you want. Polling is used when you want to constantly capture information, for example from a range sensor. Interrupts give you time sensitive control, and only performs processing at the exact time you need it.

For a more thorough answer...

This design problem is commonly referred to as "switch debouncing." As Chris correctly mentions, you can do this in hardware or software. The optimal method of implementation with respect to count accuracy and CPU load is to use hardware to smooth out the signal and connect your button to an interrupt pin on the Arduino. You can then use the built in attachInterrupt() function to eliminate the need to poll constantly. Polling is a waste of CPU power. The attachInterrupt() function will assign an interrupt handler that will be called whenever the interrupt pin sees a RISING or FALLING edge (this depends on how you have your switch electrically connected), or even more generally on a CHANGE. This would look similar to the following:

attachInterrupt(0, buttonHandler, RISING);

void buttonHandler() {
   // turn LED on:
  digitalWrite(ledPin, HIGH);
  count = count + 1;
  Serial.print(count);
}

This page explains the problem of switch debouncing and shows some example hardware circuits you can easily implement: http://www.elexp.com/t_bounc.htm

Note: You probably won't be able to see the led go HIGH because it will be handled so quickly. This code will increment the count once every time the button is pressed or released, depending on whether you attach the interrupt to the rising or falling edge of the signal.

Edit for OP's comment:
If you want the serial.print to say that the button is pressed the entire time, then add a while loop in the interrupt:

void buttonHandler() {
   // turn LED on:
  digitalWrite(ledPin, HIGH);
  count = count + 1;
  Serial.print(count);
  while(digitalRead(buttonPin)) //This will loop until the button is released
    Serial.print("ON");
}

Please note that this will cause your program to stay inside the interrupt handler, and your main function will not execute. In your main function, you can then have a Serial.print("OFF").

You may also be able to use a boolean that is set to true in your interrupt handler:

void buttonHandler() {
   // turn LED on:
  digitalWrite(ledPin, HIGH);
  count = count + 1;
  Serial.print(count);
  buttonWasPressed = true;
}

That way, in your main you can have an if statement to prevent your code from having to loop in the interrupt handler:

void loop() {
  if(buttonWasPressed) {
    while(digitalRead(buttonPin)) {
      Serial.print("ON");
    }
    buttonWasPressed = false;
  }
  else
    Serial.print("OFF");
}


问:
Complete kinematics for parallel mechanisms
I am aware of various different approaches of kinematic modelling of parallel mechanisms. I have currently employed the vector loop closure equations to solve the inverse kinematics problem.
The problem is, the vector loop closure method eliminates the passive joint variables. I need to see how these are behaving while I proceed with dynamic modelling.
Are there any textbook methods that are capable of estimating all the joint variables? (active + passive) </p>

答:
After you calculate the joint angles, you can go back to each kinematic chain connecting the platforms ans solve the "inverse kinematic problem" of the kinematic chain only.

The base frame of the chain has known coordinates, the end of the chain should have known coordinates and the already calculated active joint angle can be used to reduce the number of unknowns.


答:
I hope introducing yet another variation on the notation does not confuse things but I strongly prefer the following layout for a transform because it makes chaining transformations the wrong way around pretty much impossible.

${}^aT_b$ describes the position of b in frame a.

${}^aT_{b} * {}^bT_c$ -> correct chaining

${}^aT_b * {}^cT_b$ -> clearly wrong

Your transformations:

T06 -> ${}^0T_6$ frame 6 described in frame 0

TB0 -> ${}^BT_0$ frame 0 described in frame 'base'

target -> ${}^BT_{tool}$ frame 'tool' described in frame 'base'

TWT -> ${}^{wrist}T_{tool}$ frame 'tool' described in frame 'wrist'

T6W -> ${}^{6}T_{wrist}$ frame 'wrist' described in frame 6

(btw you refer to 'frame 6 wrist(T6T)' first in the description and and then 'T6W' in the equation - is this intentional?)

So if the above is all correct, then we have:

${}^0T_6 = ({}^BT_0)^{-1} * {}^BT_{tool} * ({}^{wrist}T_{tool})^{-1} * ({}^{6}T_{wrist})^{-1}$

Let's flip the frames on the inverses to make it more readable

${}^0T_6 = {}^0T_B * {}^BT_{tool} * {}^{tool}T_{wrist} * {}^{wrist}T_{6}$

Which we can easily see now looks correct so I don't think there is a problem with this part (assuming those transformations are all set up correctly).

I assume you are taking y and x from this ${}^0T_6$? In which case, based on the layout of this robot and the particular configuration you show in the picture, I would indeed expect $\theta_1 = \tan^{-1}(y/x)$.

Note that this equation will not always be true for ${}^0T_6$ but should always be true for ${}^0T_5$ (or ${}^0T_{wrist}$).

However, don't forget that there may be two solutions! They will be $\pi$ apart.

If, as per your question, it does not, I think there is probably something wrong with one of your transformations.

How wrong is the answer you get?

To debug this I would set the joint angles to known values (as I guess you've already done as per the picture), then use forward kinematics to calculate the position of every frame, ${}^BT_0$, ${}^0T_1$, ${}^1T_2$ etc. If you have a way to visualise them that's ideal.
Now work along each frame from the base and check that each one is where you expect.


问:
Doubt about axis\angle representation
I am trying to understand the axis/angle representation.

So far, I have understood that the main idea is that every rotation can be represented as a rotation around an arbitrary axis of an angle $\theta $.

So, for example, if I have a rotation matrix $R$ I can find an axis $r$ and an angle $\theta$.

what I have not clear is the following:

Suppose I have a rotation matrix which represents the orientation of a frame, $R ^{i}_0$ for where $i$ is used to express the fact that this is an initial frame.

Now, suppose I want to change reference frame, and so I can obtain a final reference frame, for example applying Roll Pitch Yaw sequence, and so I obtain $R_{f}^{i}$, where $f$ stands for final frame. So I end up with the orientation of a final frame with respect to the initial frame.

What I don't understand is: In which frame is the unit vector around which the rotation happens expressed?

答:
Depending on which matrix you convert to an axis angle representation.

If you do it successively, you end up with 3 axis with 3 angles in 3 different frames ($R_0$, $R_1$ and $R_2$). It is trivial to notice, that in this the axes will coincide with axes of the successive frames.

If you choose to take the result of the multiplication and write it in axis angle form you will end up with 1 axis and 1 angle in the $R_0$ Frame and the axis-angles expresses the effect of all 3 rotations.

Title: Doubt about axis\angle representation


问:
How do I compute the derivative of the Jacobian with Matlab?
I am studying robotics, and I am trying to write a Matlab code for computing the derivative of the jacobian matrix.

The formula for computing the derivative of the Jacobian is the following:

so it has been done a derivation with respect to time and it has been applied the chain rule.

I know how to do this computation by hand, but it takes really a long time if the Jacobian is large, and moreover if then I have to do some calculations with the derivative of the Jacobian it is hard without the code.

I have found this question here on Stack Exchange Robotics where a code has been posted:

Derivative of a Jacobian matrix

which is similar(it is the same, I copied it but I changed T with q) to:

```<code>clear all
clc
syms q1 q2 q3 t;

q1(t) = symfun(sym('q1(t)'), t);
q2(t) = symfun(sym('q2(t)'), t);
q3(t) = symfun(sym('q3(t)'), t);

J11 = -sin(q1(t))*(a3*cos(q2(t) + q3(t)) + a2*cos(q2(t)))

dJ11dt = diff(J11,t)
</code>```

but if I use this code I get an error which says:

Error using sym>convertChar (line 1537)
Character vectors and strings in the first argument can only specify a variable or number. To evaluate
character vectors and strings representing symbolic expressions, use 'str2sym'.

can somebody please help me?

答:
To answer your solution, specifically, all you need to do is:

```<code>clear all
clc
syms q1 q2 q3 t a3 a2;

q1(t) = symfun(str2sym('q1(t)'), t);
q2(t) = symfun(str2sym('q2(t)'), t);
q3(t) = symfun(str2sym('q3(t)'), t);

J11 = -sin(q1(t))*(a3*cos(q2(t) + q3(t)) + a2*cos(q2(t)))

dJ11dt = diff(J11,t)
</code>```

But if you're interested, there is a paper you might want to check that solves for the Jacobian Derivative $\dot{J}(q)$ analytically, (Rhee, J. Y., & Lee, B., 2017). The authors compare the method you presented with their own in terms of computation time. It is significantly faster.

The method they presented is summarized in the following algorithm.

$$
\overline{\underline{\bf{\text{Algorithm 1}}\textrm{ Jacobian and Jacobian Differentiation}}}\\
\begin{array}{lll}
1: & \omega_0^0 \leftarrow\left[0,0,0\right]^T & \\
2: & z_0^0 \leftarrow\left[0,0,1\right]^T & \\
3: & \pmb{J}_1 \leftarrow \left[\begin{matrix}
z_0^0\times d_n^0 \\
z_0^0\end{matrix}\right] & \\
4: & \bf{\text{for }} \textrm{i = 2 to n } \bf{\text{do}} & \triangleright \textrm{ Jacobian Computation loop} \\
5: & \quad \pmb{J}_i = \left[\begin{matrix}
z_{i-1}^0 \times d_{i-1,n}^0 \\
z_{i-1}^0\end{matrix}\right] &  \\
6: & \quad \omega_{i-1}^0 \leftarrow w_{i-2}^0 + z_{i-2}^0\dot{q}_{i-1} \\
7: & \bf{\text{end for}} & \\
8: & \beta \leftarrow J_{v,i}\dot{q}_i & \\
9: & \bf{\text{for }} \textrm{i = n to 2 } \bf{\text{do}} & \triangleright
 \textrm{ Jacobian Differentiation loop}\\
10: & \quad \dot{z}_{i-1}^0 \leftarrow \omega_{i-1}^0 \times z_{i-1}^0 & \\
11: & \quad \alpha \leftarrow \left[0,0,0\right]^T & \\
12: & \quad \bf{\text{for }} \textrm{j = 1 to i-1 } \bf{\text{do}} \\
13: & \qquad \alpha \leftarrow \alpha + z_{j-1}^0 \times \left(d_n^0 - d_{i-1}^0\right)\dot{q}_j & \\
14: & \quad \bf{\text{end for}} & \\
15: & \quad \dot{J}_i = \left[\begin{matrix}
\left(\dot{z}_{i-1}^0\right) \times (d_{n}^0- d_{i-1,n}^0) + z_{i-1}^0 \times\left(\alpha + \beta\right) \\
\dot{z}_{i-1}^0 \end{matrix}\right] & \\
16: & \quad \beta \leftarrow \beta + J_{v,i-1}\dot{q}_{i-1} & \\
17: & \bf{\text{end for}} & \\
18: & \dot{J}_1 = \left[\begin{matrix}
z_0^0 \times \beta \\
\left[0,0,0\right]^T \end{matrix}\right] & \\
\end{array} \\
$$

Body:
To answer your solution, specifically, all you need to do is:

clear all
clc
syms q1 q2 q3 t a3 a2;

q1(t) = symfun(str2sym('q1(t)'), t);
q2(t) = symfun(str2sym('q2(t)'), t);
q3(t) = symfun(str2sym('q3(t)'), t);

J11 = -sin(q1(t))*(a3*cos(q2(t) + q3(t)) + a2*cos(q2(t)))

dJ11dt = diff(J11,t)

But if you're interested, there is a paper you might want to check that solves for the Jacobian Derivative $\dot{J}(q)$ analytically, (Rhee, J. Y., & Lee, B., 2017). The authors compare the method you presented with their own in terms of computation time. It is significantly faster.

The method they presented is summarized in the following algorithm.

$$
\overline{\underline{\bf{\text{Algorithm 1}}\textrm{ Jacobian and Jacobian Differentiation}}}\\
\begin{array}{lll}
1: & \omega_0^0 \leftarrow\left[0,0,0\right]^T & \\
2: & z_0^0 \leftarrow\left[0,0,1\right]^T & \\
3: & \pmb{J}_1 \leftarrow \left[\begin{matrix}
z_0^0\times d_n^0 \\
z_0^0\end{matrix}\right] & \\
4: & \bf{\text{for }} \textrm{i = 2 to n } \bf{\text{do}} & \triangleright \textrm{ Jacobian Computation loop} \\
5: & \quad \pmb{J}_i = \left[\begin{matrix}
z_{i-1}^0 \times d_{i-1,n}^0 \\
z_{i-1}^0\end{matrix}\right] &  \\
6: & \quad \omega_{i-1}^0 \leftarrow w_{i-2}^0 + z_{i-2}^0\dot{q}_{i-1} \\
7: & \bf{\text{end for}} & \\
8: & \beta \leftarrow J_{v,i}\dot{q}_i & \\
9: & \bf{\text{for }} \textrm{i = n to 2 } \bf{\text{do}} & \triangleright
 \textrm{ Jacobian Differentiation loop}\\
10: & \quad \dot{z}_{i-1}^0 \leftarrow \omega_{i-1}^0 \times z_{i-1}^0 & \\
11: & \quad \alpha \leftarrow \left[0,0,0\right]^T & \\
12: & \quad \bf{\text{for }} \textrm{j = 1 to i-1 } \bf{\text{do}} \\
13: & \qquad \alpha \leftarrow \alpha + z_{j-1}^0 \times \left(d_n^0 - d_{i-1}^0\right)\dot{q}_j & \\
14: & \quad \bf{\text{end for}} & \\
15: & \quad \dot{J}_i = \left[\begin{matrix}
\left(\dot{z}_{i-1}^0\right) \times (d_{n}^0- d_{i-1,n}^0) + z_{i-1}^0 \times\left(\alpha + \beta\right) \\
\dot{z}_{i-1}^0 \end{matrix}\right] & \\
16: & \quad \beta \leftarrow \beta + J_{v,i-1}\dot{q}_{i-1} & \\
17: & \bf{\text{end for}} & \\
18: & \dot{J}_1 = \left[\begin{matrix}
z_0^0 \times \beta \\
\left[0,0,0\right]^T \end{matrix}\right] & \\
\end{array} \\
$$


问:
Why with the pseudo-inverse it is possible to invert the Jacobian matrix even in a singular configuration?
Consider the following situation:

I have a robotic arm manipulator with all revolute joints, for example consider a 2R planar manipulator. We know that if we compute the direct kinematics of it, anf the from the direct kinematics we derive the Jacobian matrix (for example only the linear part), we have that:

$\dot{p}=J(q)\dot{q}$

and if we want to do the inverse differential kinematics and obtain the joint velocities we just need to do the following:

$\dot{q}=J^{-1}(q)\dot{p}$

which is only possible if the Jacobian matrix is invertible, so if the robot is not in a singular configuration.

Now, consider the case where the robot is in a singular configuration. We have that we cannot do anymore what we have done before, because the Jacobian is not invertible.

So, studying I have seen that we can use a trick for inverting the jacobian and so finding the joint velocity anyway, and this is done by using the pseudoinverse:

$\dot{q}=J^{*}(q)\dot{p}$

where I have used $*$ to imply the pesudoinverse (sorry but I don't know why it does not let me use #).

I have seen that by using the pesudoinverse it is minimized the leaast square error and so the motion is possible to realize also from a singular configuration, but I have not understood this concept.

This should me the minimum norm solution.

Can someone please explain to me this concept?

答:
The pseudoinverse gives a “least squared error, minimum-norm” solution: Out of all $\dot{q}$ vectors at your current $q$, the vector

$$\dot{q}_{s} = J^{+}(q)\dot{p}_{\text{in}}$$

satisfies two conditions:

Least squared error: There is no $\dot{q}$ vector which will get closer to $\dot{p}$ when passed through $J(q)$. (I.e. for $\dot{p}_{s} = J(q)\dot{q}_{s}$, the normed difference $\lvert \dot{p}_{\text{in}}-\dot{p}_{s} \rvert$ is smaller than any other $\lvert \dot{p}_{\text{in}} - \dot{p} \rvert$.

Minimum-norm: Out of all $\dot{q}$ that satisfy the first condition, $\dot{q}_{s}$ is the smallest of those vectors. (I.e. $\lvert \dot{q_{s}}\rvert$ is smaller than $\lvert \dot{q} \rvert$ for all $\dot{q}$ that minimize error in $\dot{p}$.

At the singularity, the set of achievable velocities loses one dimension (so that there are $\dot{p}$ that cannot be exactly produced by a $\dot{q}$), but for any $\dot{p}_{\text{in}}$ there is a set of $\dot{q}$ that all come equally close to producing $\dot{p}_{\text{in}}$, and the pseudoinverse picks the smallest (in $\dot{q}$-space) of these vectors.


问:
HTTP requestes for Robot Teleoperation
I'm new to this forum so I apologise if I disrespect some rules. I am considering using HTTP requests to control a robot remotely. The design problem I am trying to solve is that I would like to operate my robot using a wireless controller. The controller communicates to my pc, which then transmits data about its joystick to the robot. I am thinking of sending this data by means of HTTP requests. Based on the design problem, the communication will happen frequently.

Are there any glaring issues with this approach?

Are there any alternative communication protocols that might be better?

Thanks!

答:
HTTP is a request reply communication. This is inherently limited to only one of the parties (the client) to initiate the exchange of messages. Furthermore as it is defined at the upper layers of the OSI stack it has more overhead, then protocols on lower layers.

One alternative a protocol on the transport layer, like TCP, SCTP or UDP. TCP opens a communication channel and until this channel is open messages can be exchanged and messages are guaranteed to arrive. UDP does not offer this guarantee, but in a small network messages should not get lost. SCTP is a newer portocoll and it is meant for streaming applications, so it could fit your application well.

Another option to consider is MQTT or similar streaming protocols (similar is RabbitMQ, zeroMQ). MQTT uses a message broker and instead of request-reply it uses publish-subscribe pattern. It is at a higher layer then TCP, UDP or SCTP (This usually means that it is more convenient to use but has more overhead). The robot can publish its measured coordinates and the joystick can publish its reference position. The robot subscribes to the joystick reference positions and a user interface can subscribe to both reference and measured coordinates in order to display them.

All above solutions are supported by libraries for all popular programing languages and cross platform use is also possible.

If you prefer a browser based interaction HTTP would be the easiest to implement. For native apps all above is similarly complex and plenty tutorials can be found.

Please not that none of the above methods will reach a cycle time suitable for using them inside a closed control loop, MQTT, SCTP, TCP or UDP will all be ok for supplying reference values for control loops. I would expect HTTP to be a bit slower the the others.

Title: HTTP requestes for Robot Teleoperation


问:
Reset coordinate system of robot while maintaining relationship to the previous
I am currently estimating a robot's pose (3d position and rotation matrix) with an IMU and I want to reset the localizer in the middle of the run while keeping the relationship to the pose it had when the reset occurs.

Basically, at time t, I have the position and velocity in the world frame of the robot as well as the rotation matrix from the body-fixed frame to the world frame. If, I then reset the localizer at time t and set the position to the origin and the rotation matrix to the identity; how should the velocity be rotated such that I can relate the new estimated trajectory to the pose the vehicle had when it reset at time t by:

$$ \mathbf{p}^{world}_{t+n} = \mathbf{R}_{t}\mathbf{p}_{t+n}+\mathbf{p}_{t}$$
$$ \mathbf{R}^{world}_{t+n} = \mathbf{R}_{t}\mathbf{R}_{t+n} $$

where n is any index of the estimated positions after the reset at time t.

答:
Instead of just considering it a reset, you should consider it a coordinate system transformation:

You set the position to zero, that means that you create a transformation matrix which brings your current position to zero.

You set the rotation matrix to the identity matrix, so you create a transformation matrix, which transforms your current rotation state to zero rotation.

You can build these matrices as the inverse of the transformation matrices you already have. These matrices can then be used to convert the velocity vector (or even the whole previous trajectory) to the new frame.

Do not do a reset by attributing a new position and rotation value, find the coordinate transformation which brings the current frame to the desired frame and use that transformation to transform all elements of the original frame.

Algorithmically, instead of

```<code>newPos = defaultPos
</code>```

use

```<code>newPos = getTMat(oldPos, defaultPos) * oldPos
</code>```

If you have a velocity vector in the previous coordinate frame, oldVel = (xvel, yvel, zvel) and you want to convert it to the new coordinate frame:

```<code>newVel = R * oldVel
</code>```

Where R is the rotation part (3x3) of the T transformation matrix (4x4) between the two frames. Please note, that if the velocity is expresses in a different frame (e.g. the mobile robots coordinates) you will need the rotation matrix between the robot and the old frame first:

```<code>newVel = R_new, old * R_old, robot * oldVel
</code>```

Body:
Instead of just considering it a reset, you should consider it a coordinate system transformation:

You set the position to zero, that means that you create a transformation matrix which brings your current position to zero.

You set the rotation matrix to the identity matrix, so you create a transformation matrix, which transforms your current rotation state to zero rotation.

You can build these matrices as the inverse of the transformation matrices you already have. These matrices can then be used to convert the velocity vector (or even the whole previous trajectory) to the new frame.

Do not do a reset by attributing a new position and rotation value, find the coordinate transformation which brings the current frame to the desired frame and use that transformation to transform all elements of the original frame.

Algorithmically, instead of

newPos = defaultPos

use

newPos = getTMat(oldPos, defaultPos) * oldPos

If you have a velocity vector in the previous coordinate frame, oldVel = (xvel, yvel, zvel) and you want to convert it to the new coordinate frame:

newVel = R * oldVel

Where R is the rotation part (3x3) of the T transformation matrix (4x4) between the two frames. Please note, that if the velocity is expresses in a different frame (e.g. the mobile robots coordinates) you will need the rotation matrix between the robot and the old frame first:

newVel = R_new, old * R_old, robot * oldVel


问:
How do I find the minimum time for computing a trajecotry for a robotic arm?
I am studying robotics and I am focusing on trajectory planning.

I have been struggling with this topic for a while, and I still have some doubts that I cannot solve. One of them is the following:

I was solving a problem for training which required this:

A robot joint should perform a rest-to-rest rotation $\Delta \theta $ in a total time $T$ by using a bang-coast-bang
  acceleration profile with symmetric acceleration and deceleration phases, each of duration $Ts = \frac{T}{4}$.
  Given a maximum joint velocity $V_{max} > 0$ and a maximum bound $A_{max} > 0$ for the absolute value of the
  joint acceleration, find the minimum time $T_{min}$ in this class of trajectories such that the motion is feasible.

I think that for let understand my doubts, I have to first say how I thought to tackle the problem, even if it is clearly the wrong way.

My idea for solving this proble was to find the maximum velocity, which is:

$V_{max}=\frac{T}{4}$

and from it derive the time $T$, which in my opinion was the minimum motion of time because it is referred to the maximum velocity. So, I thought the minumum time velocity to be:

$T = \frac{4V_{max}}{A}$

but this is obviusly wrong. Infact, when I went looking for the solution that my professor proposed for this exercise, the correct way was the following:

where there are also numercal values for the quantities, which I did not included in the question.

After seeing the solution, most concept where clarified. For example, I have understood that I should have calculated the minimum motion time for completing the otion(so for completing the total displacement) on the displacement from zero to $T$.

But there is still something I don't understand. In the solution, my professor derived a velocity $V$ and a displacement $\Delta \theta _s$ where this displacement is the displacement from zero to $\frac{T}{4}$ and it is equal to the displacement from $\frac{3T}{4}$ to $T$.

So, with this quantities he has computed the total displacement as :

$\Delta \theta = 2\Delta \theta _s+V\frac{T}{2}$

and from it he computed the acceleration and the velocity. Then he substitute in these the limit velocity and acceleration and then computed the minimum time trajectory.

But why he needed to compute both acceleration and velocity form the total displacement?

And what is $V=\frac{T}{4}$ that he computed in the beginning? Shoudn't this be the maximum velocity?

I have a lot of doubts on this, sice I do not understand also what are  $A=\frac{16\Delta \theta }{3T^{2}}$ and $V=\frac{4\Delta \theta }{3T}$

And also, why there is the need to compute the time both from the acceleration and form the velocity?

Can somebody please help me understand?

答:
Your problem statement is:

using a bang-coast-bang acceleration profile with symmetric acceleration and deceleration phases, each of duration $T_s=\frac{T}{4}$.

So, right off, you know the time spent in acceleration is $T/4$. The peak velocity is not $T/4$.

I'll agree that your professor's notation is hard to read (for me at least), but it looks like they're using functions for the notation.

The "bang-coast-bang" is also called a trapezoidal speed profile and it's super common with my industry's speed-regulated motors.

So, if your speed $\dot{\theta}$ is a function of time, and you ramp up to full speed $V$ at time $t=T/4$, then you can express that data point by showing your speed, as a function of time $\dot{\theta}\left(t\right)$:

$$
\dot{\theta}\left(\frac{T}{4}\right) = V \\
$$

This might be more useful if you replace $V$ with $\dot{\theta}_{max}$:

$$
\dot{\theta}\left(\frac{T}{4}\right) = \dot{\theta}_{max} \\
$$

and again, it's just saying that your angular speed $\dot{\theta}$ reaches your maximum speed at $t = \frac{T}{4}$. Remember that "big T", $T$, is the total time elapsed for the move, or the period for this operation.

Now, the other way you can express a speed, generically speaking, is:

$$
v = \int\left(a\right)dt \\
$$

which, for a constant acceleration, is:

$$
v = at \\
$$

Speed is acceleration times time. If you are using a constant acceleration, then you could also say that

$$
\dot{\theta}\left(t=\frac{T}{4}\right) = A\frac{T}{4} \\
$$

So now put those two statements above together and see that:

$$
\boxed{\dot{\theta}\left(\frac{T}{4}\right) = V = A\frac{T}{4}} \\
$$

This is a slight re-arranging of your professor's statement $
V = \dot{\theta}\left(\frac{T}{4}\right) = A\frac{T}{4} \\
$, but I find the professor's form more confusing because then it looks like $V$ is a function of $\dot{\theta}(t)$ and not the other way around.

The next line goes on to continue the "classic" constant-acceleration position formula:

$$
x(t) = x_0 + v_0 t + \frac{1}{2}at^2 \\
$$

or, in your angular case:

$$
\theta(t) = \theta_0 + \dot{\theta}_0 t + \frac{1}{2} A t^2 \\
$$

And here you can move your $\theta_0$ and assume  $\dot{\theta}_0 = 0$ and so you wind up with:

$$
\theta(t) - \theta_0 = \frac{1}{2}A t^2 \\
$$

and, since $\theta(t) - \theta_0 = \Delta \theta(t)$, you can re-write that as:

$$
\Delta \theta(t) = \frac{1}{2}A t^2 \\
$$

Now again you're trying to find out how much the angle shifted at the end of the starting acceleration period $(\Delta \theta_s)$, and the acceleration period ends at $t = \frac{T}{4}$, so:

$$
\boxed{
\Delta \theta_s = \Delta\theta\left(t = \frac{T}{4}\right) = \frac{1}{2}A\left(\frac{T}{4}\right)^2} \\
$$

And now, finally, your speed profile states you accelerate for $t = \frac{T}{4}$, you move at constant speed $V$ for $t = \frac{T}{2}$, and you decelerate for $t = \frac{T}{4}$. Since your acceleration and deceleration rates are the same, the total angle traversed during that transient is the same and so:

$$
\Delta \theta\left(t = T\right) = \Delta \theta_s + V\frac{T}{2} + \Delta \theta_s \\
$$

Or, as your professor stated it,

$$
\boxed{\Delta \theta\left(t = T\right) = 2\Delta \theta_s + V\frac{T}{2}} \\
$$

So, what is $V$? That's from the first boxed equation here:

$$
\dot{\theta}\left(\frac{T}{4}\right) = V = A\frac{T}{4} \\
$$

and you can see:

$$
V = A\frac{T}{4} \\
$$

And, what is $\Delta \theta_s$? From the second boxed equation:

$$
\Delta \theta_s = \Delta\theta\left(t = \frac{T}{4}\right) = \frac{1}{2}A\left(\frac{T}{4}\right)^2 \\
$$

so:

$$
\Delta \theta_s = \frac{1}{2}A\left(\frac{T}{4}\right)^2 \\
$$

And finally you can plug that all into the third boxed equation:

$$
\Delta \theta\left(t = T\right) = 2\Delta \theta_s + V\frac{T}{2} \\
$$

And get:

$$
\Delta \theta\left(t = T\right) = 2\left(\frac{1}{2}A\left(\frac{T}{4}\right)^2\right) + \left(A\frac{T}{4}\right)\frac{T}{2} \\
\Delta \theta\left(T\right) = \left(A\left(\frac{T}{4}\right)^2\right) + \left(A\frac{T}{4}\right)\frac{T}{2} \\
\Delta \theta\left(T\right) = A\left(\frac{T}{4}\right)\left(\frac{T}{4}\right) + A\left(\frac{T}{4}\right)\left(\frac{T}{2}\right) \\
\Delta \theta\left(T\right) = A\left(\frac{T^2}{16}\right) + A\left(\frac{T^2}{8}\right) \\
\Delta \theta\left(T\right) = A\left(\frac{T^2}{16}\right) + A\left(\frac{2T^2}{16}\right) \\
\boxed{\Delta \theta\left(T\right) = A\left(\frac{3T^2}{16}\right)}\\
$$

And, realizing that the total angle change $\Delta\theta$ is defined to be $\Delta\theta(T)$, you can solve the boxed equation on the line above as:

$$
\Delta \theta = A\left(\frac{3T^2}{16}\right)\\
\Delta \theta\frac{16}{3T^2} = A \\
\boxed{A = \frac{16\Delta\theta}{3T^2}} \\
$$

You can solve for $V$ the same way with your expression $V = A\frac{T}{4}$, solving for $A = V\frac{4}{T}$, and then substituting into the above boxed formula:

$$
A = V\frac{4}{T} = \frac{16\Delta\theta}{3T^2} \\
\boxed{V = \frac{4\Delta\theta}{3T}} \\
$$

Hope this helps!