问:
Raspberry Pi & L298N & Johnny Five, motor turns when instantiated
I'm a newcomer to this kind of tinkering so please bear with me here. I've searched high and low for an answer but have been unable to find one. I have a Raspberry Pi Zero W hooked up to a L298N motor controller and want to turn a motor forward or backward using Johnny Five. Oddly enough, when I do a simple trial of simply instantiating the motor via script, it begins turning without instruction and won't stop until I reboot the Pi; even killing the script doesn't stop the motor once it's started. Here's the code I'm using which starts the motor:

```<code>
const raspi = require('raspi-io').RaspiIO
const five = require('johnny-five')
const board = new five.Board({
  io: new raspi()
})

board.on('ready', function () {
  const motor = new five.Motor(['GPIO23', 'GPIO24'])
})

</code>```

That's it. the new five.Motor call is the one that starts the motor going.

Script/lib info:

```<code>NodeJS: v10.15.0
"johnny-five": "^1.4.0"
"raspi-io": "^11.0.0"
</code>```

And, an image describing my wiring:

I have the L298N using 5v power from the Pi board, ground connected, and the in1/in2 is hooked to GPIO23 and GPIO24.

If I run the script with GPIO23 connected but not GPIO24, the motor does not run. If the opposite is true (GPIO24 connected but not GPIO23), then the motor runs on startup.

It seems like what I'm trying to do is very simple, but I also understand that I'm at the mercy of two libraries (raspi-io and johnny-five) that I may not be using properly. I've tried my best to follow all examples I could find (especially This one and pared my code back to the bare minimum to still create this effect. Any help would be greatly appreciated.

答:
In reading through the Johnny Five examples, it seems like the basic Motor command is supposed to be used if you're driving a motor directly. You're not, though - you're using an H-Bridge configuration. There is an example for that as well that gives the following for the motor command:

```<code>const motor = new Motor({
    pins: {
      pwm: 3,
      dir: 12
    },
    invertPWM: true
    });
</code>```

It's not immediately clear to me if you need the invertPWM: true line or not, but try it and if it doesn't work then remove it. It just flips between a default of "low" or "high," so having it one way will be stopped at 0 and running at 255 and having it wrong will make it run at 0 and stop at 255.

Anyways, looks like once you've got the motor configured, run it with motor.forward() or motor.reverse() and put your desired speed (0-255) in the parenthesis.

You need to make sure your GPIO pins are able to do PWM, though.

Body:
In reading through the Johnny Five examples, it seems like the basic Motor command is supposed to be used if you're driving a motor directly. You're not, though - you're using an H-Bridge configuration. There is an example for that as well that gives the following for the motor command:

const motor = new Motor({
    pins: {
      pwm: 3,
      dir: 12
    },
    invertPWM: true
    });

It's not immediately clear to me if you need the invertPWM: true line or not, but try it and if it doesn't work then remove it. It just flips between a default of "low" or "high," so having it one way will be stopped at 0 and running at 255 and having it wrong will make it run at 0 and stop at 255.

Anyways, looks like once you've got the motor configured, run it with motor.forward() or motor.reverse() and put your desired speed (0-255) in the parenthesis.

You need to make sure your GPIO pins are able to do PWM, though.


问:
Simulating sensor readings based on an existing Alan Variance Analysis
My goal is to model an accelerometer and a gyroscope based on real hardware. I understand deterministic errors like bias and scaling but the different types of non-deterministic errors are difficult.

After a bit of research, I found a tutorial provided in the matlab documentation.

$\sigma^2(\tau) = \frac{1}{2\tau^2(n-2m)}\sum_{k=1}^{n-2m}(\theta_{k+2m} - 2\theta_{k+m} + \theta_{k})^2$

$\sigma(t) = \sqrt{\sigma^2(t)}$

Since I have no access to matlab, I implemented the function using python:

def calculate_avar(theta, t0, max_num_m):
    n = theta.size # number of samples
    max_m = 2**int(np.log2(n/2)) # maximum cluster size
    m = np.logspace(np.log10(1), np.log10(max_m), max_num_m) # cluster sizes
    m = np.ceil(m).astype(int) # m must be an integer.
    m = np.unique(m) # Remove duplicates.

    tau = m*t0
    result = np.empty_like(m)
    for i in range(m.size):
        result[i] = np.sum((theta[2*m[i]:n] - 2*theta[m[i]:n-m[i]] + theta[:n-2*m[i]])**2)
    result = result / (2*tau**2 * (n - 2*m))

    return tau, result
```

<p>Using this function I calculated:</p>

<ul>
<li>Noise density/Angle Random Walk/Velocity random walk (slope = <span class="math-container">$-\frac{1}{2}$</span>)</li>
<li>Bias (in)stability (slope = 0)</li>
<li>(Rate) Random walk (slope = <span class="math-container">$+\frac{1}{2}$</span>)</li>
</ul>

<pre class="lang-py prettyprint-override"><code># find y intersection given a particular slope
def get_y_intersect(slope, tau, adev):
    logtau = np.log10(tau)
    logadev = np.log10(adev)
    dlogadev = np.diff(logadev) / np.diff(logtau)
    i = np.argmin(np.abs(dlogadev - slope))

    return logadev[i] - slope*logtau[i], i

# noise density N
slope = -0.5
b, _ = get_y_intersect(slope, tau, adev)
logN = slope*np.log10(1) + b
N = 10**logN

# rate random walk R
slope = 0.5
b, _ = get_y_intersect(slope, tau, adev)
logK = slope*np.log10(3) + b
K = 10**logK

# bias (in)stability B
slope = 0
b, i = get_y_intersect(slope, tau, adev)
scfB = np.sqrt(2*np.log(2)/np.pi)
logB = b - np.log10(scfB)
B = 10**logB
</code>```

The problem is that I don't have access to Matlab and don't understand how to simulate the resulting parameters N, K and B. I've seen various names for these parameters (pink noise, white noise Brownian noise, second-order random walk, etc...) which is even more confusing to me. Everything I've found so far was either too shallow or requires in-depth knowledge about signal processing.

fs = 100 # sample rate
max_num_m = 1000 # number of clusters
n = 12*60*60*fs # number of samples (12hours of 100Hz samples)
t0 = 1/fs # sample time
theta = np.zeros(n, dtype=np.float64) # ideal samples

##### I can't figure out this part #####
theta += np.random.normal(0, N, n) # noise density !?
theta += np.random.normal(0, B, n).cumsum() # bias (in)stability !?
# theta += ??? # rate random walk
########################################

tau, avar = calculate_avar(theta, t0, max_num_m)
adev = np.sqrt(avar)

## plotting everything
fig, ax = plt.subplots(1, 1, figsize=(16,9))
ax.loglog(tau, adev, "-")
ax.grid(True)
ax.set_title("Allan Deviation")
ax.axis("equal")
ax.set_xlabel("$\\tau$")
ax.set_ylabel("$\\sigma(\\tau)$")
fig.show()
```

I'm not sure if the last bit of code is correct (probably not) but I'm kind of stuck here... can someone help?

答:
From the same documentation you linked,

The three noise parameters N (angle random walk), K (rate random walk), and B (bias instability) are estimated using data logged from a stationary gyroscope.

You shouldn't need to simulate these; they're typically terms provided to you on datasheets or, if you have a physical device, you can calculate them from the test device.

ContentLicense: CC BY-SA 4.0


答:
How are you calling the function? The documentation says:

And the function can also compute the fundamental matrix F:

```<code>F = cameraMatrix2^{-T} E cameraMatrix1^{-1}
</code>```

And checking this doesn't give a correct/matching number either, so I'm wondering if maybe you've left an argument out and it's causing and off-by-one error that's failing to send the right parameter to the correct output.

Title: Computing Essential Matrix from Fundamental Matrix

Body:
I have calibrated my cameras using OpenCV.
I used cv.stereoCalibrate to get the E, and F matrices. I wanted to do a sanity check to see if the E matrix obtained is the same from what is manually computed, but I did not get the same results.

I am using E = np.dot(np.dot(K2.T, F), K1)

K2 is the Second (Right) Camera Matrix & K1 is the First (Left) Camera Matrix.

The values obtained from cv.stereoCalibrate are -

```<code>K1 = array([[934.06230207,   0.        , 959.99754346],
           [  0.        , 933.44096222, 539.98442119],
           [  0.        ,   0.        ,   1.        ]])
</code>```

```<code>K2 = array([[935.93262747,   0.        , 960.00167839],
            [  0.        , 936.1379955 , 539.98789096],
            [  0.        ,   0.        ,   1.        ]])
</code>```

```<code>E = array([[-4.27815162e-05, -6.90641692e-02,  3.35371623e-02],
          [ 5.78660464e-02,  1.37740833e-02,  1.20282641e+01],
          [-3.54642135e-02, -1.20281991e+01,  1.37524047e-02]])
</code>```

```<code>F = array([[-1.45461342e-09, -2.34981243e-06,  2.33536635e-03],
           [ 1.96707058e-06,  4.68541244e-07,  3.79780838e-01],
           [-2.18936224e-03, -3.81020843e-01,  1.00000000e+00]])
</code>```

If I compute E = np.dot(np.dot(K2.T, F), K1)
I get

```<code>E = array([[-1.27165124e-03, -2.05288509e+00,  9.96869161e-01],
          [ 1.72002857e+00,  4.09425185e-01,  3.57531907e+02],
           [-1.05414944e+00, -3.57529974e+02,  4.08780803e-01]])
</code>```

This is not equal to the E I got from OpenCV.
enter code here

Body:
How are you calling the function? The <a href="https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#double%20stereoCalibrate(InputArrayOfArrays%20objectPoints,%20InputArrayOfArrays%20imagePoints1,%20InputArrayOfArrays%20imagePoints2,%20InputOutputArray%20cameraMatrix1,%20InputOutputArray%20distCoeffs1,%20InputOutputArray%20cameraMatrix2,%20InputOutputArray%20distCoeffs2,%20Size%20imageSize,%20OutputArray%20R,%20OutputArray%20T,%20OutputArray%20E,%20OutputArray%20F,%20TermCriteria%20criteria,%20int%20flags)" rel="nofollow noreferrer">documentation says:

And the function can also compute the fundamental matrix F:

F = cameraMatrix2^{-T} E cameraMatrix1^{-1}

And checking this doesn't give a correct/matching number either, so I'm wondering if maybe you've left an argument out and it's causing and off-by-one error that's failing to send the right parameter to the correct output.


问:
My Raspberry Pi is losing power in a surge
I have an RC car. The battery provides power to the ESC and then the ESC provides 6 V back out to the receiver. Instead of the receiver I have a Raspberry Pi, which uses the 6 V, steps it down to 5 V and provides power to the Raspberry Pi.

The problem

Every time we go full power*, there is a lack of voltage and the Raspberry Pi seems to hard reset.

* By full power we mean direct to 100% and not ranging from 0-100

I am not an expert in electrical circuits, but some of the suggestions are to use a capacitor to provide the missing 5 V in the interim. How do I prevent the Raspberry Pi from dying in the event of full power?

答:
Since you're running directly from a battery I would say it's safe to just add as much decoupling (in other words caps across your input power) as possible, since the only real downside (that I think is relevant to your setup) to adding a lot of capacitance is increased in-rush current (since the capacitor naturally acts as a short-circuit during charge-up).

In certain circumstances however the high current draw associated with excessive decoupling during initial start-up should be avoided. An example is when you use a switching converter to step a voltage up/down. If the converter has built in over current protection (and no slow start feature), the short-circuit caused by the uncharged caps will cause the converter to stagger (starting up, over current and starting up again), and never fully reach its target voltage. However, since you're running directly from a battery this shouldn't be a problem, since a battery can be driven way above it's rated current capacity (for short periods).

Another thing to also remember is that since there exists a large energy store across your power rails (the capacitors), the system might take a while to discharge (after being powered off). In other words, your Pi will probably run for another 30 seconds (depending on how much capacitance you add) or so after you disconnect your main battery.

Finally, always try to add capacitors that's rated at at least twice your operating voltage (for instance, if you have 6V batteries try to get 16V caps). Motors that reverses their direction very quickly may induce sufficiently large voltage spikes back into your system, and cause your caps to explode (hopefully your motor driver has sufficient clamping diodes).

I would say a single 1000 uF electrolytic cap would be more than enough decoupling. If your Pi continues to brown-out, I guess the more appropriate cause would be your batteries not being able to supply the required current. Remember, the reason you Pi is restarting (or browning out) is due to the supply voltage dipping because the batteries are unable to supply the current the motors require. Adding capacitors will help with surges in currents (such as motors accelerating), but obviously won't solve long term high current draw.

ContentLicense: CC BY-SA 3.0


问:
Why my robot is lying down in gazebo simulator?
this is my first post about ROS and Gazebo also I am new in ROS.
I spawn KUKA robot in Gazebo using urdf file from KUKA github repository
following answer from this question I spawned my urdf file in gazebo simulator and KUKA robot appeared in simulator but problem is that is lying down like is attached on floor for some reason.
Here is picture in Gazebo:

This is my launch file:

```<code><launch>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="<span class="math-container">$(find gazebo_ros)/launch/empty_world.launch">
  <arg name="paused" value="false"/>
  <arg name="use_sim_time" value="true"/>
  <arg name="gui" value="true"/>
  <arg name="headless" value="false"/>
  <arg name="debug" value="false"/>
</include>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $</span>(find kuka_kr210_support)/urdf/kr210l150.urdf -urdf -z 1 -model baxter" />
</launch>
</code>```

EDIT:
I fixed base link for the world, added transmission, controllers for each axis and gazebo plugin. now my urdf file looks like this:

```<code><?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from kr16_2.xacro                   | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--Generates a urdf from the macro in kr16_2_macro.xacro -->
<robot name="kuka_kr16_2" xmlns:xacro="http://wiki.ros.org/xacro">
  <!-- LINKS -->

  <!-- base link -->
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/base_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/base_link.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- link 1 (A1) -->
  <link name="link_1">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_1.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- link 2 -->
  <link name="link_2">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_2.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- link 3 -->
  <link name="link_3">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_3.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_3.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- link 4 -->
  <link name="link_4">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_4.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_4.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- link 5 -->
  <link name="link_5">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_5.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_5.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- link 6 -->
  <link name="link_6">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_6.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_6.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- tool 0 -->
  <!-- This frame corresponds to the $FLANGE coordinate system in KUKA KRC controllers.
  <link name="tool0"/> -->

  <!-- END LINKS -->
  <!-- JOINTS -->
  <!-- joint 1 (A1) -->
  <joint name="joint_a1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.675"/>
    <parent link="base_link"/>
    <child link="link_1"/>
    <axis xyz="0 0 -1"/>
    <limit effort="0" lower="-3.22885911619" upper="3.22885911619" velocity="2.72271363311"/>
  </joint>

   <transmission name="trans_joint_a1">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_a1">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_joint_a1">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </actuator>
   </transmission>

  <!-- joint 2 (A2) -->
  <joint name="joint_a2" type="revolute">
    <origin rpy="0 0 0" xyz="0.26 0 0"/>
    <parent link="link_1"/>
    <child link="link_2"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-2.70526034059" upper="0.610865238198" velocity="2.72271363311"/>
  </joint>

   <transmission name="trans_joint_a2">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_a2">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_joint_a2">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </actuator>
   </transmission>

  <!-- joint 3 (A3) -->
  <joint name="joint_a3" type="revolute">
    <origin rpy="0 0 0" xyz="0.68 0 0"/>
    <parent link="link_2"/>
    <child link="link_3"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-2.26892802759" upper="2.68780704807" velocity="2.72271363311"/>
  </joint>

   <transmission name="trans_joint_a3">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_a3">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_joint_a3">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </actuator>
   </transmission>

  <!-- joint 4 (A4) -->
  <joint name="joint_a4" type="revolute">
    <origin rpy="0 0 0" xyz="0.67 0 -0.035"/>
    <parent link="link_3"/>
    <child link="link_4"/>
    <axis xyz="-1 0 0"/>
    <limit effort="0" lower="-6.10865238198" upper="6.10865238198" velocity="5.75958653158"/>
  </joint>

   <transmission name="trans_joint_a4">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_a4">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_joint_a4">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </actuator>
   </transmission>

  <!-- joint 5 (A5) -->
  <joint name="joint_a5" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="link_4"/>
    <child link="link_5"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-2.26892802759" upper="2.26892802759" velocity="5.75958653158"/>
  </joint>

   <transmission name="trans_joint_a5">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_a5">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_joint_a5">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </actuator>
   </transmission>

  <!-- joint 6 (A6) -->
  <joint name="joint_a6" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="link_5"/>
    <child link="link_6"/>
    <axis xyz="-1 0 0"/>
    <limit effort="0" lower="-6.10865238198" upper="6.10865238198" velocity="10.7337748998"/>
  </joint>

   <transmission name="trans_joint_a6">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_a6">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_joint_a6">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </actuator>
   </transmission>

  <!-- tool frame - fixed frame
  <joint name="joint_a6-tool0" type="fixed">
    <parent link="link_6"/>
    <child link="tool0"/>
    <origin rpy="0 1.57079632679 0" xyz="0.158 0 0"/>
  </joint>

   <transmission name="trans_joint_a6-tool0">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_a6-tool0">
        <hardwareInterface>EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_joint_a6-tool0">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>EffortJointInterface</hardwareInterface>
      </actuator>
   </transmission> -->

  <!-- END JOINTS -->
  <!-- ROS base_link to KUKA $ROBROOT coordinate system transform -->
  <link name="base"/>
  <joint name="base_link-base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="base"/>
  </joint>

  <!-- Gazebo world joint -->
  <!-- Used for fixing robot to Gazebo 'base_link' -->
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/kr16_2_gazebo</robotNamespace>
  </plugin>
</gazebo>

</robot>
</code>```

This is controller yaml:

```<code># Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

# Position Controllers ---------------------------------------
joint1_position_controller:
  type: effort_controllers/JointPositionController
  #joint: base_link__link_01
  joint: joint_a1
  pid: {p: 100, i: 100, d: 500.0}
joint2_position_controller:
  type: effort_controllers/JointPositionController
  #joint: link_01__link_02
  joint: joint_a2
  pid: {p: 100.0, i: 100, d: 2000.0}
joint3_position_controller:
  type: effort_controllers/JointPositionController
  #joint: link_02__link_03
  joint: joint_a3
  pid: {p: 100.0, i: 50, d: 1000.0}
joint4_position_controller:
  type: effort_controllers/JointPositionController
  #joint: link_03__link_04
  joint: joint_a4
  pid: {p: 100.0, i: 50, d: 200.0}
joint5_position_controller:
  type: effort_controllers/JointPositionController
  #joint: link_04__link_05
  joint: joint_a5
  pid: {p: 100.0, i: 50, d: 70.0}
joint6_position_controller:
  type: effort_controllers/JointPositionController
  #joint: link_05__link_06
  joint: joint_a6
  pid: {p: 100.0, i: 50, d: 70.0}
tool0_position_controller:
  type: effort_controllers/JointPositionController
  joint: link_06__tool0
  #joint: joint1
  pid: {p: 100.0, i: 50, d: 70.0}
</code>```

and this is my launch file:

```<code><?xml version="1.0"?>
<launch>

  <arg name="paused" default="false"/>
  <arg name="gazebo_gui" default="true"/>
  <arg name="urdf_path" default="$(find kuka_kr16_support)/urdf/kr16_2_gazebo.urdf"/>

  <!-- startup simulated world -->
  <include file="<span class="math-container">$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="worlds/empty.world"/>
    <arg name="paused" value="$</span>(arg paused)"/>
    <arg name="gui" value="$(arg gazebo_gui)"/>
  </include>

  <!-- send robot urdf to param server -->
  <param name="robot_description" textfile="$(arg urdf_path)" />

  <!-- Load controllers -->
  <rosparam ns="/kr16_2_gazebo" command="load" file="$(find kuka_kr16_moveit_gazebo)/config/joints.yaml" />

  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
    respawn="false" output="screen" />

  <!-- Controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner"
    respawn="false" output="screen" ns="/kr16_2_gazebo"
    args="--namespace=/kr16_2_gazebo
    joint_state_controller
    joint1_position_controller
    joint2_position_controller
    joint3_position_controller
    joint4_position_controller
    joint5_position_controller
    joint6_position_controller
    --timeout 60">
  </node>

</launch>
</code>```

And when I launch it in terminal this is output:

WARNING: Package name "KUKA_KR16_moveit_Luka1" does not follow the
  naming conventions. It should start with a lower case letter and only
  contain lower case letters, digits, underscores, and dashes. ...
  logging to
  /home/gazebo/.ros/log/133adcf4-5e8d-11e9-8069-000c2911adc8/roslaunch-ubuntu-3085.log
  Checking log directory for disk usage. This may take awhile. Press
  Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

WARNING: Package name "KUKA_KR16_moveit_Luka1" does not follow the
  naming conventions. It should start with a lower case letter and only
  contain lower case letters, digits, underscores, and dashes. started
  roslaunch server http://ubuntu:35047/

SUMMARY

PARAMETERS  * /gazebo/enable_ros_network: True  *
  /kr16_2_gazebo/joint1_position_controller/joint: joint_a1  *
  /kr16_2_gazebo/joint1_position_controller/pid/d: 500.0  *
  /kr16_2_gazebo/joint1_position_controller/pid/i: 100  *
  /kr16_2_gazebo/joint1_position_controller/pid/p: 100  *
  /kr16_2_gazebo/joint1_position_controller/type: effort_controller...
  * /kr16_2_gazebo/joint2_position_controller/joint: joint_a2  * /kr16_2_gazebo/joint2_position_controller/pid/d: 2000.0  *
  /kr16_2_gazebo/joint2_position_controller/pid/i: 100  *
  /kr16_2_gazebo/joint2_position_controller/pid/p: 100.0  *
  /kr16_2_gazebo/joint2_position_controller/type: effort_controller...
  * /kr16_2_gazebo/joint3_position_controller/joint: joint_a3  * /kr16_2_gazebo/joint3_position_controller/pid/d: 1000.0  *
  /kr16_2_gazebo/joint3_position_controller/pid/i: 50  *
  /kr16_2_gazebo/joint3_position_controller/pid/p: 100.0  *
  /kr16_2_gazebo/joint3_position_controller/type: effort_controller...
  * /kr16_2_gazebo/joint4_position_controller/joint: joint_a4  * /kr16_2_gazebo/joint4_position_controller/pid/d: 200.0  *
  /kr16_2_gazebo/joint4_position_controller/pid/i: 50  *
  /kr16_2_gazebo/joint4_position_controller/pid/p: 100.0  *
  /kr16_2_gazebo/joint4_position_controller/type: effort_controller...
  * /kr16_2_gazebo/joint5_position_controller/joint: joint_a5  * /kr16_2_gazebo/joint5_position_controller/pid/d: 70.0  *
  /kr16_2_gazebo/joint5_position_controller/pid/i: 50  *
  /kr16_2_gazebo/joint5_position_controller/pid/p: 100.0  *
  /kr16_2_gazebo/joint5_position_controller/type: effort_controller...
  * /kr16_2_gazebo/joint6_position_controller/joint: joint_a6  * /kr16_2_gazebo/joint6_position_controller/pid/d: 70.0  *
  /kr16_2_gazebo/joint6_position_controller/pid/i: 50  *
  /kr16_2_gazebo/joint6_position_controller/pid/p: 100.0  *
  /kr16_2_gazebo/joint6_position_controller/type: effort_controller...
  * /kr16_2_gazebo/joint_state_controller/publish_rate: 50  * /kr16_2_gazebo/joint_state_controller/type: joint_state_contr...  *
  /kr16_2_gazebo/tool0_position_controller/joint: link_06__tool0  *
  /kr16_2_gazebo/tool0_position_controller/pid/d: 70.0  *
  /kr16_2_gazebo/tool0_position_controller/pid/i: 50  *
  /kr16_2_gazebo/tool0_position_controller/pid/p: 100.0  *
  /kr16_2_gazebo/tool0_position_controller/type: effort_controller...  *
  /robot_description:

NODES   /kr16_2_gazebo/
      controller_spawner (controller_manager/spawner)   /
      gazebo (gazebo_ros/gzserver)
      gazebo_gui (gazebo_ros/gzclient)
      spawn_gazebo_model (gazebo_ros/spawn_model)

auto-starting new master process[master]: started with pid [3096]
  ROS_MASTER_URI=http://localhost:11311

setting /run_id to 133adcf4-5e8d-11e9-8069-000c2911adc8 WARNING:
  Package name "KUKA_KR16_moveit_Luka1" does not follow the naming
  conventions. It should start with a lower case letter and only contain
  lower case letters, digits, underscores, and dashes.
  process[rosout-1]: started with pid [3107] started core service
  [/rosout] process[gazebo-2]: started with pid [3110]
  process[gazebo_gui-3]: started with pid [3119]
  process[spawn_gazebo_model-4]: started with pid [3124]
  process[kr16_2_gazebo/controller_spawner-5]: started with pid [3125]
  WARNING: Package name "KUKA_KR16_moveit_Luka1" does not follow the
  naming conventions. It should start with a lower case letter and only
  contain lower case letters, digits, underscores, and dashes. WARNING:
  Package name "KUKA_KR16_moveit_Luka1" does not follow the naming
  conventions. It should start with a lower case letter and only contain
  lower case letters, digits, underscores, and dashes. WARNING: Package
  name "KUKA_KR16_moveit_Luka1" does not follow the naming conventions.
  It should start with a lower case letter and only contain lower case
  letters, digits, underscores, and dashes. WARNING: Package name
  "KUKA_KR16_moveit_Luka1" does not follow the naming conventions. It
  should start with a lower case letter and only contain lower case
  letters, digits, underscores, and dashes. [INFO] [1555229566.121403,
  0.000000]: Controller Spawner: Waiting for service /kr16_2_gazebo/controller_manager/load_controller [ INFO]
  [1555229567.920658309]: Finished loading Gazebo ROS API Plugin. [
  INFO] [1555229567.922362082]: waitForService: Service
  [/gazebo_gui/set_physics_properties] has not been advertised,
  waiting... [ INFO] [1555229567.927517546]: Finished loading Gazebo ROS
  API Plugin. [ INFO] [1555229567.928969037]: waitForService: Service
  [/gazebo/set_physics_properties] has not been advertised, waiting...
  [INFO] [1555229569.090114, 0.000000]: Loading model XML from ros
  parameter robot_description [INFO] [1555229569.100970, 0.000000]:
  Waiting for service /gazebo/spawn_urdf_model [INFO]
  [1555229569.708130, 0.000000]: Calling service
  /gazebo/spawn_urdf_model [ INFO] [1555229569.740163120, 0.023000000]:
  waitForService: Service [/gazebo/set_physics_properties] is now
  available. [INFO] [1555229570.489737, 0.159000]: Spawn status:
  SpawnModel: Successfully spawned entity [ INFO] [1555229570.502446729,
  0.159000000]: Physics dynamic reconfigure ready. [ INFO] [1555229570.679725950, 0.159000000]: Loading gazebo_ros_control plugin
  [ INFO] [1555229570.680028318, 0.159000000]: Starting
  gazebo_ros_control plugin in namespace: /kr16_2_gazebo [ INFO]
  [1555229570.680620454, 0.159000000]: gazebo_ros_control plugin is
  waiting for model URDF in parameter [/robot_description] on the ROS
  param server. [spawn_gazebo_model-4] process has finished cleanly log
  file:
  /home/gazebo/.ros/log/133adcf4-5e8d-11e9-8069-000c2911adc8/spawn_gazebo_model-4*.log
  [ INFO] [1555229570.868346785, 0.159000000]: Loaded
  gazebo_ros_control. [INFO] [1555229570.971088, 0.255000]: Controller
  Spawner: Waiting for service
  /kr16_2_gazebo/controller_manager/switch_controller [INFO]
  [1555229570.980829, 0.257000]: Controller Spawner: Waiting for service
  /kr16_2_gazebo/controller_manager/unload_controller [INFO]
  [1555229570.987506, 0.263000]: Loading controller:
  joint_state_controller [INFO] [1555229571.017423, 0.294000]: Loading
  controller: joint1_position_controller [INFO] [1555229571.106571,
  0.377000]: Loading controller: joint2_position_controller [INFO] [1555229571.162822, 0.424000]: Loading controller:
  joint3_position_controller [INFO] [1555229571.229486, 0.475000]:
  Loading controller: joint4_position_controller [INFO]
  [1555229571.288503, 0.515000]: Loading controller:
  joint5_position_controller [INFO] [1555229571.361675, 0.563000]:
  Loading controller: joint6_position_controller [INFO]
  [1555229571.418728, 0.607000]: Controller Spawner: Loaded controllers:
  joint_state_controller, joint1_position_controller,
  joint2_position_controller, joint3_position_controller,
  joint4_position_controller, joint5_position_controller,
  joint6_position_controller [INFO] [1555229571.426332, 0.612000]:
  Started controllers: joint_state_controller,
  joint1_position_controller, joint2_position_controller,
  joint3_position_controller, joint4_position_controller,
  joint5_position_controller, joint6_position_controller

From output I can see that all controllers are loaded,to confirm it here is output of rostopic:

```<code>/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/kr16_2_gazebo/joint1_position_controller/command
/kr16_2_gazebo/joint1_position_controller/pid/parameter_descriptions
/kr16_2_gazebo/joint1_position_controller/pid/parameter_updates
/kr16_2_gazebo/joint1_position_controller/state
/kr16_2_gazebo/joint2_position_controller/command
/kr16_2_gazebo/joint2_position_controller/pid/parameter_descriptions
/kr16_2_gazebo/joint2_position_controller/pid/parameter_updates
/kr16_2_gazebo/joint2_position_controller/state
/kr16_2_gazebo/joint3_position_controller/command
/kr16_2_gazebo/joint3_position_controller/pid/parameter_descriptions
/kr16_2_gazebo/joint3_position_controller/pid/parameter_updates
/kr16_2_gazebo/joint3_position_controller/state
/kr16_2_gazebo/joint4_position_controller/command
/kr16_2_gazebo/joint4_position_controller/pid/parameter_descriptions
/kr16_2_gazebo/joint4_position_controller/pid/parameter_updates
/kr16_2_gazebo/joint4_position_controller/state
/kr16_2_gazebo/joint5_position_controller/command
/kr16_2_gazebo/joint5_position_controller/pid/parameter_descriptions
/kr16_2_gazebo/joint5_position_controller/pid/parameter_updates
/kr16_2_gazebo/joint5_position_controller/state
/kr16_2_gazebo/joint6_position_controller/command
/kr16_2_gazebo/joint6_position_controller/pid/parameter_descriptions
/kr16_2_gazebo/joint6_position_controller/pid/parameter_updates
/kr16_2_gazebo/joint6_position_controller/state
/kr16_2_gazebo/joint_states
/rosout
/rosout_agg
</code>```

and robot is just like hanging on base link, because base link is only one which has type=fixed and all other have type=revolute.

here is picture of hanging robot:

When I lift and release robot in gazebo using mouse, robot just fall down and axis are just shaking like there is no motor or any force which keeps them fixed.

答:
A. It seems your robot is not fixed to gazebo world so you have to add a joint to fix in gazebo environment.
Add below content into your urdf file and child link should be your base link name.

B. Fine tune damping and friction values of dynamics tag for all joints in urdf file.

C. if more of your robot start vibrating than fine tune PID gain values from your either controller.yaml file or joint_parameter.yaml file.


问:
Raspberry pi libraries for fortran
I'm quite a fan of fortran and I was wondering if there were any libraries to control the GPIO pins in a raspberry pi with it. I find python to be quite slow so I think it is the best option for making it fast.</p>

答:
Turns out there is in fact a very simple library called fortran-gpio for exactly that.

Another option is to access GPIO pins through device files, see here for details.

Title: Raspberry pi libraries for fortran


问:
Setting up rewards to account for UAV crashes (Reinforcement Learning)
I am working on a project to implement a collision avoidance algorithm on a real UAV. I'm interested in understanding the process to set up a negative reward to account for scenarios wherein there is a UAV crash. This can be done very easily during simulation(if the UAV touches any object, the episode stops giving a negative reward).
Any ideas will be highly appreciated.</p>

答:
From what I understand this is quite easy and you don't have to use reinforcement   learning. You can also use standard ANNs.

Algorithm:

For as long as you want:

  If Touch (sensor. Maybe a button???) == 1

  Update network (the network should output the probability of crashing in the       next step. You can also use markov chains for multiple steps (just an idea))

I hope this answers your question.


问:
Absolute positioning without GPS
Using an IMU a robot can estimate its current position relative to its starting position, but this incurs error over time. GPS is especially useful for providing position information not biased by local error accumulation. But GPS cannot be used indoors, and even outdoors it can be spotty.

So what are some methods or sensors that a robot can use to localize (relative to some frame of reference) without using GPS?

答:
I understand you problem is to find different means to GPS to find your position within a given reference frame. This problem in isolation is called localization, and there are many ways to perform that. Firstly you will have to differentiate between relative methods, so measurements which provide a change in position to a previously known position. This method has the problem, that any errors are obviously accumulated, and will grow unbounded.

Dead reckoning is likely one of the oldest ways of relative localization. If you use heading, speed and time (so estimating the distance traveled) you can sum up you position changes from a starting position.

In addition to using dead reckoning you can also not landmarks and track them in a map. Finding those landmarks again will allow you to reduce your relative position error. This is the Simultaneous Localization and Mapping (SLAM) problem. It is still relative navigation.

Now coming to your actual question on absolute navigation. All that GPS does is provide you with distance estimations to Landmarks with a known position information in your reference frame (in this case geocentric). GPS receivers will take these information and generate a position solution, which also has an error. The good thing though is that this error is bounded within your frame of reference. This it what makes it an absolute positioning system. So whether its indoors or outdoors and regardless of your desired reference frame, all you need for absolute positioning systems are measurements that put you in relation to some known landmark position within your reference frames. Some of those methods have been given in a previous answer. Although, as I said, SLAM is not an absolute method.

The simplest form is direct landmark recognition. If you see an Eiffel-Tower you should have a good notion of your absolute position (at least with an absolute error bound) within the earth fixed frame (if you know the position of the Eiffel-Tower). You may have to do some disambiguation, though.

If you want to improve your absolute position error, you can use multiple landmarks at the same time. Classical Triangulation is such an example. Another one is using craters for a lunar descent vehicle. Landmarks don't have to be visual, and you can use things like RF signal strength for known signals like in WiFi or Cellular localization.

All of the above methods needed landmarks, which need to be identified and uniquely associated. If this is a problem, you can also use different methods, like the terrain profile. This have for example been applied for early cruise missile navigation. I've also used this method for localization on an elevation map without visual or range sensing.

With all of the above methods: as long as any of your map material has Geo-referenced information associated, you can obviously Geo-reference yourself without the use of GPS. The most important factor to differentiate the methods is their error characteristics.

Title: Absolute positioning without GPS


答:
It sounds like you're experiencing a "brown out" caused when the excessive current draw from the battery causes a drop in the supply voltage.  This is due to the fact that batteries have internal resistance (a.k.a output impedance).

In this example, if the load drops to $0.2\Omega$, the internal resistance of the battery will cause the output voltage to be divided equally with the external resistance -- the load will only see 5V.  It's possible that your 6V supply is dropping to 3V (or less) under load, in much the same way.

A capacitor will delay this effect, but what you really need is a voltage regulator -- many are available in an integrated circuit (IC) package.  Some voltage regulators will step up voltage when insufficient, but most simply lower a supply voltage to the voltage needed by a component like your RPi (in which case, your battery voltage would have to be increased so that it never dipped below 5V under full motor load).

Alternatively, you could use a separate battery pack for the RPi.  This is a common solution for mobile robots, because it ensures that when the robot is immobilized due to lack of power, radio communication with the onboard PC will not be lost.

Title: My Raspberry Pi is losing power in a surge

Body:
I have an RC car. The battery provides power to the ESC and then the ESC provides 6 V back out to the receiver. Instead of the receiver I have a Raspberry Pi, which uses the 6 V, steps it down to 5 V and provides power to the Raspberry Pi.

The problem

Every time we go full power*, there is a lack of voltage and the Raspberry Pi seems to hard reset.

* By full power we mean direct to 100% and not ranging from 0-100

I am not an expert in electrical circuits, but some of the suggestions are to use a capacitor to provide the missing 5 V in the interim. How do I prevent the Raspberry Pi from dying in the event of full power?


问:
how do I decide what is the order of the polynomial I need to use in a polynomial iterpolation for a trajectory?
I am a student studying robotics, and I am focusing on trajectory planning for a robotic arm. In particular, I am studying polynomial interpolation for defining the trajectories, and I am having some doubts.

I have understand that ny using a polynomial interpolation for defining the timing law of a trajectory I have a smooth interpolation. Also, I have seen in many examples, that usually, if I don't need to include acceleration in my profile, I can choose a cubic polynomial and if I have to include also acceleration, I have to use a quintic polynomial (this concept is not really clear to me, I am just using it automatically).

As far as I have understood this concept is relate to the number of parameters I have to use, but is this a fixed rue?

What I mean is, how do I decide what is the order of the polynomial I need to use?

In many example I see it is said that it is sufficient a second order polynomial, or sometimes a third order polynomial, but I have not clear why.

Can somebody please help me?

答:
This sounds exactly like cam design.  When designing a cam profile, you typically want the cam follower to be at certain displacements at certain times, and you can usually choose how it gets there.

For example:

As you point out, there are many curve options.  Curves are chosen to eliminate discontinuities in position, velocity, and acceleration.  And when possible, minimizing acceleration.

When I was a cam designer, this book: Cam Design by Clyde H. Moon was our bible.  It is very old, but still pretty useful.

In cam design, a velocity or acceleration discontinuity will make the follower jump off the cam or cause excess wear.  You don't really have that issue with a robot arm, but I still believe that having a smooth acceleration profile is desirable.


问:
Is it possible to use a LiPo charger as a lab bench power supply?
I recently thought about building a lab bench power supply, it comes in cheaper and I love to build things...

But then I also have a LiPo charger an iMax B6AC, that I had bought for my quadcopter, then came the idea of whether I can use the charger as a lab bench power supply...

My questions is, could this work and how could I make it work?

答:
This seems to be the only place I'm finding with information regarding this question. The imax b6ac has a power mode function that you can set voltage and amperage up to 20v 5a, those values can also be altered real time without need to reset the function.
I guess it's also worth noting I have the knock off version as it was maybe $20, but otherwise it's the same thing. </p>


问:
Estimation of pitch, roll, yaw angles: synthesis of an observer or IMU?
Dear Robotics Specialists, I want to contact you for advice.
I am developing a control system for complex parallel mechanisms like what I presented in this thread.

Formulas for calculating the passive joints angular velocities

I faced the problem of estimating the pitch, roll and yaw angles of a platform.

On the one hand, this problem can be solved by using IMU.

On the other hand, the use of mathematical tools, for example, the synthesis of an observer (a mathematical model of the mechanism in the state-space is known), is a rather attractive way.

But here another question arises: it is necessary to evaluate either the rotation angular velocities of the intermediate joints ($\dot{\psi_i}$ and $\dot{\xi_i}$), and then use them to solve the direct kinematics problem (which determines the multiplicity of solutions, rather than uniqueness), or, to evaluate these angular velocities and pitch, roll and yaw angles using an observer, taking them in the form of a single state vector. I’m not sure if this is possible. only motor shaft position and velocity is available for control and measurement ($[\dot{\theta_1},\dot{\theta_2},\dot{\theta_3}]$).

So:

IMU - automatically solves the problem and ensures the uniqueness of solutions, but it is expensive and causes the accumulation of errors, which is their disadvantage. Are there any alternatives to the IMU that have been spared this drawback?

An observer is a computational tool implemented in software. It allows you to evaluate some parameters. How to use it to evaluate such a large vector when only 1 measured and controlled joint is available and what type of observer is best used for this?? ($[\dot{\theta_1},\dot{\theta_2},\dot{\theta_3}]$).

答:
Assuming this is a platform attached to a robot of some kind, you can:

Measure the platform pose directly, as with an inclinometer, IMU, laser, camera, etc.,

Measure the platform pose indirectly, by putting rotary encoders on the joints and using kinematics to determine the platform pose for a given set of joint positions, or

Estimate the platform pose with observers/filters.

For an IMU, the Madgwick filter can fix roll and pitch angles. You would need a magnetometer (called "9-axis IMU," typically) to fix yaw angle as well. No IMU is capable of fixing position. You could filter with a GPS to fix position.

Regarding your question about an observer:

How to use it to evaluate such a large vector when only 1 measured and controlled joint is available and what type of observer is best used for this?

You need to check for observability. You had stated

a mathematical model of the mechanism in the state-space is known

so you build the observability matrix:

$$
O = \left[\begin{matrix}
C \\
CA \\
CA^2 \\
\vdots \\
CA^{n-1}\end{matrix}\right]
$$

where $n$ is the number of states of your system (number of rows/column of $A$) and then check that $\textrm{rank}\left(O\right) = n$.

If $\textrm{rank}\left(O\right) < n$ then the system is not observable and your states can't be estimated with the stated arrangement of feedback channel(s).


问:
Do mobile robots use variable transmissions?
I've built a simple robot that can move.  I'm using brushless motors.  I've realized that at slow speeds the motors pull a lot of current.  I didn't understand the power / current / efficiency charts when I started.  I see it in the charts, now, that the efficiency in electric motors is at high RPM and the low speed torque is awesome but comes at a price of high current.

For mobile robots, do people generally just provide high-current batteries for slow speeds, ignore slow speeds, or do they build robots with variable transmissions instead of fixed gearboxes?  When I look around I just see designs with fixed gearboxes but I feel like I'm missing something.

If I look beyond hobby type projects I see talk about CVT and the Inception drive but I'm wondering anyone at the home / hobbiest level do anything like that.

答:
For a simple answer: no, most mobile robots do not use variable transmissions.  I believe it is far easier to just design a motor and gearbox combination that works well in your narrow range of desired speeds, than deal the extra mechanical complexity and cost of a variable transmission.

The long answer is that it depends on what you consider a mobile robot.  Let's take a quick survey of the range of "mobile robots".  This can be a huge list, but I will try to span the space from very small to very large.

 Plum Geek Ringo Robot. Brushed motors, no transmission at all!

 Parallax BOT Bot. Brushed motors, no variable transmission.

 iRobot Roomba/Create. Brushed motors, no variable transmission.

 FLIR Packbot. Brushless motors, no variable transmission.

 DARPA LAGR Program Robot. Electric wheelchair motors, no variable transmission.

 Stanley.  Desiel engine with variable transmission.

 Tesla Model 3, brushless electric motor with variable transmission.

So it looks like variable transmissions are really only needed when you have gas engines.  Or very high performance electric motors.


问:
Plotting a Robot in App Designer
I've a robot created with Peter Corke toolbox and I must plot it inside an app (must be in App Designer - Matlab). The problem is that everytime I try to plot it, it makes an independent figure outside the app. I've got the UIAxes created but I don't know if I can make it 'Parent' with a SerialLink.plot.

Is it even possible to plot it with SerialLink.plot or there's another way to do it?

Thanks in advance

Edit.: The plot must show an animation of different states using inverse kinematics. I can't do the animation either but I think after the plot issue, it should be easier.

答:
Plot functions plot to the gca. gca is an abraviation for get current axis. It returns the handle to the currently selected axis, which will recieve the next plot.

You can take a look at the source code of the plot function you mentioned here:
https://github.com/petercorke/robotics-toolbox-matlab/blob/master/%40SerialLink/plot.m

If you follow its logic, it seems that if you create a new axis on the UI and set it as the currently active axis and if this axis does not have a tag which is set to 'RTB.plot' then the plot will be made in the current axis.

If you want to set your preferred axis as the current axis, you can do this by calling:

```<code>axes(handle_to_prefered_axis);
</code>```

Also, you might want to call hold on before plotting, as this will allow the next plot to be created in the same figure.

Title: Plotting a Robot in App Designer

Body:
Plot functions plot to the gca. gca is an abraviation for get current axis. It returns the handle to the currently selected axis, which will recieve the next plot.

You can take a look at the source code of the plot function you mentioned here:
https://github.com/petercorke/robotics-toolbox-matlab/blob/master/%40SerialLink/plot.m

If you follow its logic, it seems that if you create a new axis on the UI and set it as the currently active axis and if this axis does not have a tag which is set to 'RTB.plot' then the plot will be made in the current axis.

If you want to set your preferred axis as the current axis, you can do this by calling:

axes(handle_to_prefered_axis);

Also, you might want to call hold on before plotting, as this will allow the next plot to be created in the same figure.


问:
Encoder based speed control for Rover 5
I am trying to get precise control over the speed of rover 5 based robot. It has four PWM controlled motors and 4 Optical Quadrature Encoders. I am using 4-channel motor controller with rover 5 chassis. I am using arduino Nano for control. I am able to read encoder INT output and change PWM based on pulse width to control speed. But, as a result, I am getting heavy oscillations in the control output. That makes, the robot to move in steps, as PWM is changing constantly. I need an algorithm which can minimize this ringing and have a smooth moving robot. Here is my arduino code snippet.

```<code>void setup() {
    Serial.begin(9600);
    init_motors();
    init_encoders();
    req_speed[0] = 20;
    req_speed[1] = 20;
    req_speed[2] = 20;
    req_speed[3] = 20;
}

void loop() {
  update_encoders();
  update_motors();
}

void update_motors() {
  int i, err;
  unsigned long req_width;
  if(micros() - mtime > 2999) {
    mtime = micros();

    for(i=0; i<4; i++) {
      digitalWrite(pins_dir[i], req_speed[i]>0);
      if(mtime - change_time[i] > 50000ul && req_speed[i] != 0) {
        cur_pwm[i] += 5;
      }
      if(req_speed[i] > 0)
        cur_err[i] = req_speed[i]*10  - cur_speed[i];
      else
        cur_err[i] = (-req_speed[i]*10)  - cur_speed[i];
      if(cur_err[i] > 0 && cur_pwm[i] < 255) {
        cur_pwm[i]++;
      } else if(cur_err[i] < 0 && cur_pwm[i] > 0) {
        cur_pwm[i]--;
      }
      analogWrite(pins_pwm[i], cur_pwm[i]);
    }
  }
}

void update_encoders() {
  int i;
  unsigned long w;
  enc_new = PINC & B00001111;
  unsigned long etime = micros();
  for (i=0; i<4; i++) {
    if((enc_old & (1 << i)) < (enc_new & (1 << i)))
    {
      w = (unsigned long)(((etime - change_time[i])));
      pulse_width[i] = (w + pulse_width_h1[i] + pulse_width_h2[i])/3;
      pulse_width_h2[i] = pulse_width_h1[i];
      pulse_width_h1[i] = pulse_width[i];
      change_time[i]=etime;
      pulse_count[i]++;
      cur_speed[i] = (3200000ul / pulse_width[i]);
    }
  }
  enc_old=enc_new;
}
</code>```

Here req_speed is between -100 to 100, where sign indicates direction. Please consider all undefined variables as globals. I experimentally measured that, when motor is running at full speed, the pulse width is around 3200us.

Encoders' INT outputs (XOR of A and B) are connected to A0 thru A3. Motor PWM is connected to D3, D5, D6, D9. Please let me suggest any improvements to this code and advice me about what am I missing here.

答:
I'll be honest, I don't understand what some of your code is doing. Why is there a 4bit mask for the encoders? Is this the direction? What is pulse_width_h1 vs pulse_width_h2...please comment your code and I will be able to help you.

In addition, your controller is odd. It looks like you're simply adding 1 or -1 depending on the sign of the error. You should look into a PID controller. These are easy to implement and VERY commonly used to implement velocity control. You can start with just a P (proportional) term. The idea is to tune your gain K_p until the system behaves as desired.

To reiterate...please comment your code and I can edit this post to provide a better answer.

Title: Encoder based speed control for Rover 5


问:
What communication bus is used in state of the art systems to implement a synchronization signal?
I have a robotic system with many sensors that need to have very accurate localization associated with the data they gather.
We have a geolocalization system that acquire localization at high speed (up to 100Hz) and we want to broadcast a synchronization signal that we can associate with this localization data.

We develop the electronics for the whole system and we hesitate between a clock signal or a signal that delivers event such that we can have ids broadcasted that will serve as timestamp.
Our requirement is to be accurate with a drift of maximum 1ms.

The signal can have to traverse up to 4m of cables between the generator of the synchronization signal and the sensors.

If we can we would like to use CAN bus, but we have no experience with it as a synchronization bus.

We would like to ask the community what communication bus is used in state of the art systems to implement a synchronization signal ?

答:
There are many possible routes you can take.

You mentioned CAN. CANOpen is built on CAN and is used in many automation and robotics related devices. CANOpen specifies a SYNC mechanism exactly for the purpose you mention. More about this here. As for cycle times, the bandwidth and exact message size of CAN packets is knows, so cycle times are easy to calculate. CANOpen is widely used in the automation industry for millisecond cycle time applications.

PTP is a time synchronisation mechanism for networked devices. It can be implemented on different communication protocols. Most commonly it is used in ethernet-based networks with UDP implementation. More about this here. This mechanism does not trigger a measurement but makes sure that internal clocks on the network are synched (and then measurements can be self-triggerd cyclically).

Depending on the application you are working on, low-latency industrial ethernet-based bu systems may be an option, as an alternative to CAN (or to be more precise, as an alternative to CANOpen), you could look into sercos, Ethernet-IP, Profinet, DeviceNet, EtherCAT. Some (most?) of this are proprietary and will require that you select equipment already compatible with the communication bus. These are designed to work as you intend, low jitter, low latency, high speed.


问:
IR pattern projector
I am currently working on a robotics project in which I wish to integrated a depth/object scanner. Now my focus is currently on an IR camera in combination with some sort of IR pattern projector.

Now I am having a hard time finding any sort of IR projector which can project a pattern. I have found several topics suggesting that I use an IR-LED and create a pattern in front of it, but that would mean the projector would take up significant space.

Are there any projectors out on the market in a sensible form-factor for consumers? I came across the Structure Core, but that's only available to OEMs.

答: The projectors you are looking for are available with https://ams.com/3d-imaging</p>

Title: IR pattern projector


问:
Cartesian Impedance control Damping Design (Double Diagonalization)
In the literature, it is often seen that for a system:

\begin{align}
        \Lambda \ddot{e} + D_d \dot{e} + K_d e = F_{ext}
\end{align}

A dynamic damping matrix is required for a robot to take into account the structure and changes of $\Lambda$ during movement. Hence the following method called the "double diagonalization".

Given a symmetric and positive definite matrix $\Lambda \in \mathcal{R}^{n\times n}$ and a symmetric matrix $K_d \in \mathcal{R}^{n\times n}$, one can find a non singular matrix $Q \in \mathcal{R}^{n\times n}$ and a diagonal matrix $B_0 \in \mathcal{R}^{n\times n}$ such that:
\begin{align}
    \Lambda &= Q^\intercal Q\\
    K_d &= Q^\intercal B_0 Q
\end{align}
Where the diagonal elements of $B_0$ are the generalized eigenvalues of $K_d$

The design of the damping matrix becomes:
\begin{align}
    D_d = 2 Q^\intercal diag( \xi \sqrt(\lambda_{K,i}^\Lambda) Q)
\end{align}

Such that
\begin{align}
    Q^\intercal Q \ddot{x} + 2 Q^\intercal diag( \xi \sqrt(\lambda_{K,i}^\Lambda) Q) \dot{x} + Q^\intercal B_0 Q x = F_{ext}
\end{align}

where $\xi_i$ is the damping factor in the range of $[0,1]$, $\lambda_{K,i}^\Lambda$ is the i'th diagonal element of $B_0$

-
Yeah, I 'm pretty dumb (I'm guessing the solution is simple but I cannot see it). I cannot figure out how to solve for the matrices $Q$ and $B_0$, such that (in particular) $\Lambda = Q^\intercal Q$. What am I missing? Can elaborate for me?

How do I solve this?

Sources: (this article should be free and provide some context.)
(Albu-Schaffer, Alin, et al., 2003)

答:
A hint towards what the answer is given in the paper. Namely, one can use the generalized eigenvalue decomposition, which in this case can be formulated as finding eigenvalues $\lambda \in \mathbb{R}$ and eigenvectors $v \in \mathbb{R}^n$ such that

$$
(\lambda\,\Lambda - K_d)\,v = 0. \tag{1}
$$

Consider two distinct solutions $(\lambda_i,v_i)$ and $(\lambda_j,v_j)$ of $(1)$, which can also be written as

\begin{align}
\lambda_i\,\Lambda\,v_i &= K_d\,v_i, \tag{2a} \\
\lambda_j\,\Lambda\,v_j &= K_d\,v_j. \tag{2b}
\end{align}

By pre-multiplying $(2a)$ by $v_j^\top$ and $(2b)$ by $v_i^\top$ one gets

\begin{align}
\lambda_i\,v_j^\top \Lambda\,v_i &= v_j^\top K_d\,v_i, \tag{3a} \\
\lambda_j\,v_i^\top \Lambda\,v_j &= v_i^\top K_d\,v_j. \tag{3b}
\end{align}

When using that $M = M^\top$ and $K_d = K_d^\top$ it follows that when subtracting the transpose of $(3b)$ from $(3a)$ one gets

$$
(\lambda_i - \lambda_j)\,v_j^\top \Lambda\,v_i = 0. \tag{4}
$$

Thus when $\lambda_i \neq \lambda_j$ it follows that $v_j^\top \Lambda\,v_i = 0$, from which together with $(3a)$ it also follows that $v_j^\top K_d\,v_i = 0$. When $i \neq j$ but $\lambda_i = \lambda_j$ this orthogonality property between vectors $v_i$ and $v_j$, and matrices $\Lambda$ and $K_d$ does not immediately follow. It can be noted that $\lambda_i = \lambda_j = \lambda$ means that the kernel of $\lambda\,\Lambda - K_d$ has a dimension larger then one, such that for all $\alpha,\beta\in\mathbb{R}$ the vector $v = \alpha\,v_i + \beta\,v_j$ would satisfy $(1)$. However, most generalized eigenvalue problem solvers "choose" $\alpha$ and $\beta$ such that they do ensure that $v_j^\top \Lambda\,v_i = 0$ and $v_j^\top K_d\,v_i = 0$ whenever $\lambda_i = \lambda_j$.

When defining $V$ as the matrix whose columns are equal to $v_i\ \forall\,i=1,2,\cdots, n$ it follows from $v_j^\top \Lambda\,v_i = v_j^\top K_d\,v_i = 0$ that $V^\top \Lambda\,V$ and $V^\top K_d\,V$ are diagonal matrices. By also dividing each $i$th column of $V$ by $\sqrt{v_i^\top \Lambda\,v_i}$ one also obtains that $V^\top \Lambda\,V = I$ and $V^\top K_d\,V = \text{diag}(\lambda_1,\lambda_2,\cdots,\lambda_n)$. Solving this for $\Lambda$ and $K_d$ yields

\begin{align}
\Lambda &= V^{-\top} V^{-1}, \tag{5a} \\
K_d &= V^{-\top} \text{diag}(\lambda_1,\lambda_2,\cdots,\lambda_n)\,V^{-1}, \tag{5b}
\end{align}

which is equivalent to using $Q = V^{-1}$ and $B_0 = \text{diag}(\lambda_1,\lambda_2,\cdots,\lambda_n)$.

For more information of this derivation see section 3.3.2 from the book De Kraker, A. (2009). Mechanical vibrations. Shaker Publishing BV.


答: I looked at the PID controller basics and found that its the canonical solution for this kind of speed control problem. I used the arduino PID library and it worked pretty well. I had to spend some time figuring out the PID parameters (Kp, Ki, Kd) manually by trial and error. But,  with this, I am now able to get my motors running at the required speed with pretty much stable behaviour. You can take a look the new PID based code here</p>

Title: Encoder based speed control for Rover 5

Body:
I am trying to get precise control over the speed of rover 5 based robot. It has four PWM controlled motors and 4 Optical Quadrature Encoders. I am using 4-channel motor controller with rover 5 chassis. I am using arduino Nano for control. I am able to read encoder INT output and change PWM based on pulse width to control speed. But, as a result, I am getting heavy oscillations in the control output. That makes, the robot to move in steps, as PWM is changing constantly. I need an algorithm which can minimize this ringing and have a smooth moving robot. Here is my arduino code snippet.

```<code>void setup() {
    Serial.begin(9600);
    init_motors();
    init_encoders();
    req_speed[0] = 20;
    req_speed[1] = 20;
    req_speed[2] = 20;
    req_speed[3] = 20;
}

void loop() {
  update_encoders();
  update_motors();
}

void update_motors() {
  int i, err;
  unsigned long req_width;
  if(micros() - mtime > 2999) {
    mtime = micros();

    for(i=0; i<4; i++) {
      digitalWrite(pins_dir[i], req_speed[i]>0);
      if(mtime - change_time[i] > 50000ul && req_speed[i] != 0) {
        cur_pwm[i] += 5;
      }
      if(req_speed[i] > 0)
        cur_err[i] = req_speed[i]*10  - cur_speed[i];
      else
        cur_err[i] = (-req_speed[i]*10)  - cur_speed[i];
      if(cur_err[i] > 0 && cur_pwm[i] < 255) {
        cur_pwm[i]++;
      } else if(cur_err[i] < 0 && cur_pwm[i] > 0) {
        cur_pwm[i]--;
      }
      analogWrite(pins_pwm[i], cur_pwm[i]);
    }
  }
}

void update_encoders() {
  int i;
  unsigned long w;
  enc_new = PINC & B00001111;
  unsigned long etime = micros();
  for (i=0; i<4; i++) {
    if((enc_old & (1 << i)) < (enc_new & (1 << i)))
    {
      w = (unsigned long)(((etime - change_time[i])));
      pulse_width[i] = (w + pulse_width_h1[i] + pulse_width_h2[i])/3;
      pulse_width_h2[i] = pulse_width_h1[i];
      pulse_width_h1[i] = pulse_width[i];
      change_time[i]=etime;
      pulse_count[i]++;
      cur_speed[i] = (3200000ul / pulse_width[i]);
    }
  }
  enc_old=enc_new;
}
</code>```

Here req_speed is between -100 to 100, where sign indicates direction. Please consider all undefined variables as globals. I experimentally measured that, when motor is running at full speed, the pulse width is around 3200us.

Encoders' INT outputs (XOR of A and B) are connected to A0 thru A3. Motor PWM is connected to D3, D5, D6, D9. Please let me suggest any improvements to this code and advice me about what am I missing here.


问:
given position and orientation of two coordinate frames, how to transfer a vector from one to another
given position and orientation of two coordinate frames, how can transfer a vector from one frame to another.

The position is given in Cartesian space and the orientations in quaternion or Euler angels.

答:
A homogeneous transform $T$ is 4x4 matrix that looks like:

$$T=\begin{bmatrix}
R & t \\
0_3 & 1
\end{bmatrix}$$

where $R$ is a 3x3 rotation matrix and $t$ is the translation. The rotation matrix can be calculated by converting either the quaternion or the euler angles.

This matrix is used to transform a point from one coordinate system to another. The equation would look like this:

$$p_A=T_{AB}*p_B$$

with $p$ representing a point in the homogeneous form which is just appending a 1 to the end of it.

$$p=\begin{bmatrix}
p_x \\ p_y \\ p_z \\ 1
\end{bmatrix}$$

If you don't want to use the homogenous form then the equations just break down to

$$p_A=R_{AB}*p_B+t_{AB}$$.

here $p$ is just a 3x1 vector without the 1.

You can directly use the quaternion if you change the rotation matrix multiplication to quaternion multiplication.

$$p_A=q_{AB}\otimes p_B+t_{AB}$$.

This equation converts a point in the $B$ coordinate system into the $A$ coordinate system.

You have or can easily figure out $T_{AB}$. I don't know how you are storing the vector, but it can always be broken into 2 endpoints. Now just use the equation above to transform both points, and then convert back into your vector format.


问:
How to represent the Xf axis of the final frame (end-effector) in function of euler angles?
I am working on a robot arm with 7 rotational joints.
I can generate the T of any position, what I want to do ist to represent the vector Xf of the end effector frame in the base frame.
How can I do it?</p>

答:
Assuming:
* the T you mention is a 4x4 transformation matrix
* "by any point you want" you meant any given pose (pose is position and orientation)

So the T transformation matrix links the end effector frame to the base frame. Let's assume that you have a given position (X=10, y = 20, z = 30) and orientation (A = 0, B = 0, C = 0). for this pose, the T matrix is trivial to make:

$$T=\begin{bmatrix}
1 & 0& 0 & 10 \\
0 & 1 & 0 & 20 \\
0 & 0 & 1 & 30 \\
0 & 0 & 0 & 1\\
\end{bmatrix}$$

This matrix can be used to calculate the coordinates of a point given in the end effector frame in the base frame.

E.g. the point (X = 1, Y = 1, Z = 1) in defined in the End Effector Frame will have the following coordinates in the base frame:

$$
\begin{bmatrix}
1 & 0& 0 & 10 \\
0 & 1 & 0 & 20 \\
0 & 0 & 1 & 30 \\
0 & 0 & 0 & 1\\
\end{bmatrix}
 \times
\begin{bmatrix}
1 \\
2  \\
3  \\
1 \\
\end{bmatrix}
=
\begin{bmatrix}
11 \\
22 \\
33  \\
1 \\
\end{bmatrix}
$$

If you want to represent the coordinate fram visually, you will need to take one point on each axis and the origin and see what coordinates corespond to these points in the base frame, so:

$$P_X=
\begin{bmatrix}
1 & 0& 0 & 10 \\
0 & 1 & 0 & 20 \\
0 & 0 & 1 & 30 \\
0 & 0 & 0 & 1\\
\end{bmatrix}
 \times
\begin{bmatrix}
1 \\
0  \\
0  \\
1 \\
\end{bmatrix}
=
\begin{bmatrix}
11 \\
20 \\
30  \\
1 \\
\end{bmatrix}
$$

$$P_Y=
\begin{bmatrix}
1 & 0& 0 & 10 \\
0 & 1 & 0 & 20 \\
0 & 0 & 1 & 30 \\
0 & 0 & 0 & 1\\
\end{bmatrix}
 \times
\begin{bmatrix}
0 \\
1  \\
0  \\
1 \\
\end{bmatrix}
=
\begin{bmatrix}
10 \\
21 \\
30  \\
1 \\
\end{bmatrix}
$$

$$P_Z=
\begin{bmatrix}
1 & 0& 0 & 10 \\
0 & 1 & 0 & 20 \\
0 & 0 & 1 & 30 \\
0 & 0 & 0 & 1\\
\end{bmatrix}
 \times
\begin{bmatrix}
0 \\
0  \\
1  \\
1 \\
\end{bmatrix}
=
\begin{bmatrix}
10 \\
20 \\
31  \\
1 \\
\end{bmatrix}
$$

$$P_O=
\begin{bmatrix}
1 & 0& 0 & 10 \\
0 & 1 & 0 & 20 \\
0 & 0 & 1 & 30 \\
0 & 0 & 0 & 1\\
\end{bmatrix}
 \times
\begin{bmatrix}
0 \\
0  \\
0  \\
1 \\
\end{bmatrix}
=
\begin{bmatrix}
10 \\
20 \\
30 \\
1 \\
\end{bmatrix}
$$

In order to visualize the coordinate system, you need to draw lines between $P_O P_X$, $P_O P_Y$ and $P_O P_Z$. If the coordinate system drawn is too small, you can use a point on each axis which is further away from the origin (instead of alway useing 1 you can use 10, 100 or even larger numbers)

The above method works also if the rotation part of the transformation matrix is different from the identity matrix, this example was chosen for the easy calculations.


问:
How do quadcopters turn left and right?
In this picture, a sketch of a quadcopter is displayed with rotor's direction of motion.  The magnitude of the rotational velocity is depicted by the thickness of the lines (thicker lines are higher velocity, thinner lines are lower velocity).

I'm told this is the correct way to produce turning motion, but my intuition (which is usually wrong) tells me that the two pictures should be reversed.  My argument is  as follows:  For the picture on the left, the two rotors of higher velocity are spinning clockwise.  If the motion of the rotors of greater velocity are clockwise, shouldn't the quadcopter also rotate clockwise?  What am I missing here?

答:
There are actually two sources of yaw torque in a quadrotor and a multirotor in general.

The first is the torque from an imbalance between the torque generated by the CW spinning rotors and that generated by the CCW spinning rotors. This is entirely a function of friction in the motor bearings and aerodynamic drag.

The second is torque arising from the conservation of angular momentum when the rotor speeds are changed, similar to how a reaction wheel works (see the Cubli). This effect is present in a vacuum, so it does not rely on aerodynamic forces.

The first effect causes angular acceleration of the vehicle proportional to the difference in rotor speeds between the CW and CCW rotors. The second effect causes angular acceleration of the vehicle proportional to the difference in the derivative of the rotor speeds between the CW and CCW rotors.

See https://aapt.scitation.org/doi/pdf/10.1119/1.5033880 for a short experimental study of these two effects.

Title: How do quadcopters turn left and right?


问:
Feed both position and speed errors to one single PID
I have seen in the documentation of an old subcontractor's product that the controller of a robotic arm joint was basically feeding error position + error rate directly into one single PID.

It feels wrong to have two different variations mixed before acting on the mix, but I may be wrong given that, after all, speed is directly linked to position.

Is it correct? What are the drawbacks to that, for systems where both positions and speeds need to be tracked? Should state-space MISO controllers be used instead?

答:
This is very standard, when the position error is multiplied by the P gain and the velocity error is multiplied by the D gain. Typically, a measurement of the velocity error is preferred as it will be smoother than that computed by numerically differentiating the position.

When you want to track both a position and a speed, it is important that the reference speed and reference position are consistent, i.e. that the derivative of the position is the velocity. In that case, yes you want to feed both into the PID or PD loop.

It may depend on the application, but typically, there is a feedforward term that strives to keep both errors as low as possible (if pos error is always zero then so will vel error) so that the feedback controller has to do minimal work.


问:
How to calculate rotation angle from 3D points
I have 4 3d points(x, y, z). Using these points how can I calculate rotation angle? Consequently, I want to have a 4x4 transformation matrix including rotation and translation information.

4 points are in a plane and they are rectangle and I set the top-left point as the origin of the rectangle coordinate. Look at the bellow pictures.

Here I only have four 3D points(Xw,Tw,Zw) based on the world coordinate. According to the 4 points, I can calculate 4 points based on the rectangle coordinate. For instance, value [Xr] in P2 is the distance between P1 and P2. And [-Yr] in P3 is the distance between P1 and P3.

Then how could I get rotation angle of the rectangle based on the world coordinate using those information?

```<code>Additional information (3d points in the world coordinate):
P1(-401.428, 485.929, 772.921)
P2(-21.4433, 475.611, 772.511)
P3(-400.982, 483.56, 403.703)
P4(-21.589, 473.028, 403.242)
</code>```

答:
You want to find a translation vector $t \in \mathbb{R}^3$ and rotation matrix $R \in \mathbb{R}^{3 \times 3}$ such that vectors $p_k \in \mathbb{R}^3$ map to $p_k' \in \mathbb{R}^3$ using

$$
\begin{bmatrix}
p_k' \\ 1
\end{bmatrix} =
\begin{bmatrix}
R & t \\ 0 & 1
\end{bmatrix}
\begin{bmatrix}
p_k \\ 1
\end{bmatrix},
$$

or equivalently $p_k' = R\,p_k + t$. Since the zero vector in rectangle coordinates should map to P1 in world coordinates it follows that $t$ should be equal to P1 in world coordinates. The rotation matrix can be found by using a solution to Wahba's problem, which in your case can be defined as

$$
\min_{R \in SO(3)} \sum_{k=1}^n a_k\, \|w_k - R\,v_k\|^2,
$$

with $w_k = p_k' - t$ and $v_k = p_k$. The weights $a_k$ can be any positive number, often used to indicate how "trustworthy" each vector is and/or normalize with respect to vector length. However, one can always just set all $a_k$ equal to one.

If the world coordinates are subjected to any disturbance it might be that using P1 in world coordinates as $t$ gives a slightly sub-optimal solution. However, the solution proposed in this answer should still give a good approximated optimal solution (with respect to $\sum \|R\,p_k + t - p_k'\|^2$).


问:
How to avoid false encoder counts due to vibration in robot?
I have a couple of motors on my mobile base other than the drive motors, and they tend to create vibration in the robot. When these motors are turned on, the wheel's encoder ticks increase without any actual movement. This is destroying my odometry. As I need the motors to be on all the time, I need some help to figure this out.

How to avoid false encoder counts like these due to vibration?

答:
The one easy solution is to put in logic in your odometry that lets it know if your robot is moving. Just have a flag that tells your odometry to stop reading encoder inputs. when you know the robot is standing still.

If this doesn't work for you then it becomes quite a hard problem to solve. You can either try and model the vibration error(that seems to be the approach in this paper), or you can try and detect false positives using an additional sensor like an IMU. This approach is often used to help detect false encoder values during conditions like wheel slip. There are a ton of papers on this approach. E.g. this one. You just have to understand and adapt it to your specific problem.


答:
If you're talking about vibration causing the effect, then the encoders are giving you readings because the system is actually moving. It shouldn't affect odometry because a vibration is a back and forth thing; it should net zero counts over time.

If it is causing you problems then it could be noise, or you're not sampling fast enough, or your arm is too rigidly constrained. Rigid systems are rigid because any effective spring constant is very high, which would cause very fast oscillations that may happen faster than you can count.

Also your encoder may be too sensitive.

Title: How to avoid false encoder counts due to vibration in robot?

Body:
I have a couple of motors on my mobile base other than the drive motors, and they tend to create vibration in the robot. When these motors are turned on, the wheel's encoder ticks increase without any actual movement. This is destroying my odometry. As I need the motors to be on all the time, I need some help to figure this out.

How to avoid false encoder counts like these due to vibration?


问:
what are shoulder ball and socket joints called? (i am trying to design and build a hydraulic assistance mech suit)
I'm having problems finding the right sort of joint for my shoulder connection. It is a flat surface on the side of the mech body and I need to create a joint with maximum movement rotating: up, down and to have a possible butterfly style movement.the joint will be above and behind my arm like an exoskeleton.</p>

答:
A 2 degree of freedom shoulder joint can be called a gimbal or pan-tilt.  It can be modeled with a universal joint (aka U-joint).

Usually, the third degree of freedom to twist the arm can be added a little further down the arm with no loss of generality.  Because the axis of rotation still intersects the other two.

But the human shoulder joint is actually much more complicated than just 3 DOF.  It is what allows you to shrug.

You might be interested in the "Anthrobot" designs by Mark Roshiem.  Specifically the Omni-Wrist III "wrist".  (A wrist can also be used as a shoulder).  He has many different versions of this joint mechanism.  This one is driven by linear actuators which might suit your needs.  These joint designs are nice because there is free space down the center of the joint where the axes intersect.  This allows for cable routing or maybe a human arm.

He also has a few books on robot joint design which are very interesting.  I highly recommend "Robot Evolution: The Development of Anthrobotics".


问:
SLERP velocity and acceleration (3d Orientation interpolation)
Hey guys I am quite new to quaternions and I am a bit unsure as to how to use them. How are the velocities and accelerations trajectories determined using quaternions?

I have been using slerp to interpolate the orientations.

$$
\begin{align}
Slerp(Q_0, Q_1,t) = Q_0(Q_0^{-1}Q_1)^t
\end{align}
$$

Where $t$ is the interpolation parameter from $[0;1]$. I read on wikipedia that:

*The derivative of $Slerp(q_0, q_1; t)$ with respect to t, assuming the ends are fixed, is $log(q_1 q_0^{−1})$ times the function value, where the quaternion natural logarithm in this case yields half the 3D angular velocity vector.*

This is further supported by a document I found on the topic of quaternion interpolation in animation (Dam, E. B., Koch, M., & Lillholm, M., (1998)), where in equation: (6.12), they show the first and second derivative (using their notation):

$$
\begin{align}
\frac{d}{dt}Slerp(p,q,h) &= \frac{d}{dt} p (p^* q)^h \\
&= p(p^* q)^h log(p^* q)\\
&= Slerp(p,q,h) log(p^*q)\\\\
\frac{d^2}{dh^2}Slerp(p,q,h) &= p(p^* q)^h log(p^*q)^2\\
&= Slerp(p,q,h) log(p^* q)^2
\end{align}
$$

In other words, to get the second derivative of $Slerp$ you need to times the function value with $log(Q_1, Q_0^{-1})^2$.

Is this legitimate? And can I use these resulting quaternions for:

$$
\begin{align}
X_d = \begin{bmatrix}p_d\\ Q_d \end{bmatrix}, \quad \dot{X}_d = \begin{bmatrix}\dot{p}_d\\ \dot{Q}_d \end{bmatrix} \quad \ddot{X}_d = \begin{bmatrix}\ddot{p}_d\\ \ddot{Q}_d \end{bmatrix}
\end{align}
$$

where $p = [x,y,z]^\intercal$ and $Q = \{\eta, \epsilon \}$, and the dots $\dot{X}$ and $\ddot{X}$ are the respective velocities and accelerations.

Because I am not sure what to make of the results. Maybe because I don't really understand quaternions. For context here's an example:

Python:

```<code>import numpy as np
import quaternion
import matplotlib.pyplot as plt
import matplotlib as mpl
mpl.style.use('seaborn')
plt.rcParams['figure.figsize'] = [10, 15]

t = np.linspace(0,1,100)
Q = np.zeros((4,100))
Qd = np.zeros((4,100))
Qdd = np.zeros((4,100))

rot0 = np.array([[ 1.0,  0.0,  0.0],
                 [ 0.0, -1.0,  0.0],
                 [ 0.0,  0.0, -1.0]])

rotf = np.array([[ 0.0, -1.0,  0.0],
                 [-1.0,  0.0,  0.0],
                 [ 0.0,  0.0, -1.0]])

q0 = quaternion.from_rotation_matrix(rot0)
qf = quaternion.from_rotation_matrix(rotf)

for i in range(100):
    slerp = q0 * (q0.inverse() * qf)**t[i]
    slerp_d = slerp * np.log(qf * q0.inverse())
    slerp_dd = slerp * np.log(qf * q0.inverse())**2

    Q[:,i] = quaternion.as_float_array(slerp)
    Qd[:,i] = quaternion.as_float_array(slerp_d)
    Qdd[:,i] = quaternion.as_float_array(slerp_dd)

fig, (ax1, ax2, ax3) = plt.subplots(3, 1)
ax1.plot(Q.T)
ax1.set_title("Slerp")
ax1.legend(['w','i','j','k'])
ax2.plot(Qd.T)
ax2.set_title("Slerp_d")
ax2.legend(['w','i','j','k'])
ax3.plot(Qdd.T)
ax3.set_title("Slerp_dd")
ax3.legend(['w','i','j','k'])
plt.show()
</code>```

答:
Derivative of $\sin$ is $\cos$, and the derivative of $\cos$ is $-sin$.

Given a quaternion definition of:

$q = \cos{a} + \mathbf{r}\sin{a}$

$\mathbf{r}^2 = -1$

I would expect to see what is effectively a phase shift at every derivative level, and that's what I'm seeing in your curves. I noticed your magnitude seems to be growing, but that may be because you need to normalize at every step.


答:
As @jsotola guessed in the comments, it is possible you have magnetic interference between your motors and hall effect encoders.  This exact thing happened to me on a very compact electo-mechanical design for a robot hand seen here.  If I remember correctly, we wanted to keep our magnetic encoders so we moved the motors farther away.  But you might be able to switch to an optical encoder.

The other possibility is to switch to a full quadrature encoder.  Most encoders already do this, but some very cheap encoders (like on the Roomba Create) are only half-quadrature.  With full quadrature, you should be able to determine the direction of travel from the signals, so any vibration will only make it oscillate between 2 values, not increase indefinitely.  With half-quadrature, it is only a square-wave and you have to assume a direction of travel, so noise might make the values increase as you said.


问:
Measuring a Motor's Internal Temperature
As a final safety measure regarding some high wattage motors, I want to measure their temperature (either internal or surface) in live time so that I can automatically kill the power if it crosses a threshold. Temperature modules I am finding appear to be mainly for air or liquids; I don't know if there's anything out there to measure the surface of metals (surrounded by air). A couple examples I don't think would work are the DS18B20 and DHT22 modules.</p>

答:
First, you would need to know:

which are the parts of the motor that heat most;

which are the parts of the motor (or surrounding components) that can get damaged by the heat;

which are the maximum temperatures involved;

the cause of the heating: friction, EM effects, surrounding components...

This might require some serious skills and work. Ideally, the manufacturer of the motor provides you with some information.

Once you have that information, you can choose the strategy and components to alleviate the issue.

Otherwise, you risk cooling the already "cool" areas, while the heat still does damage to your project.

Temperature modules I am finding appear to be mainly for air or liquids

Well, that is for a good reason. To measure the temperature inside a solid, you need to drill a hole in that solid, to reach the desired "point". Or use some technologies (similar to X-rays, MRI, CAT scanners) which can see inside the solid without material damage.


问:
Understanding Twists as 3D Velocities
I am just learning about twists to represent 3D velocities (e.g. of a robot's end-effector), and I have two questions:

1) Wikipedia defines a twist as "an angular velocity around an axis and a linear velocity along this axis". To represent a twist mathematically, this requires defining a point in 3D space (3 coordinates), the direction of the axis passing through this point (2 coordinates), and the ratio of the linear and angular velocities. This is 6 numbers in total. However, it seems that you could also represent this same 3D velocity just by defining linear velocity about the axes (3 coordinates), and a rotational velocity about the three axes (3 coordinates), so that the linear and rotational velocities are defined independently. This is also 6 numbers in total. So why do we define 3D velocities as twists, rather than using my version? Is it just that there are some nice mathematical properties of twists that my version does not have? Or is it that my version is actually fundamentally wrong in some way?

2) Let's say I have an object with positive x- and y- linear velocity, but zero z- linear velocity. The object is also spinning around its z-axis, but there is no rotation about the x- and y- axes. You can think of this as a ball sliding across the floor, whilst spinning about its vertical axis. I am struggling to understand how it is possible to represent this 3D velocity using "an angular velocity around an axis and a linear velocity along this axis". In this example, it seems clear to me that the linear velocity is acting orthogonally to the axis of rotation of the angular velocity. The ball is rotating about its z-axis, and so it doesn't seem possible to also define the linear velocity in terms of this z-axis, because the linear velocity only has components in the x- and y- axes. What am I missing?

Thank you!

答:
1)
There are many ways to express velocities. All of them are mathematical constructs to describe the same motion. They can have some minor advantages/disadvantages depending on the applications. The one main disadvantage of an Euler angle based approach is the gimbal lock problem. An advantage of Euler angle based approaches is, that they can be more intuitive to comprehend.

The wikipedia site you quote addresses the advantage of screw-theory based approaches explicitly:

An important result of screw theory is that geometric calculations for points using vectors have parallel geometric calculations for lines obtained by replacing vectors with screws. This is termed the transfer principle.[4]

Screw theory has become an important tool in robot mechanics, mechanical design, computational geometry and multibody dynamics. This is in part because of the relationship between screws and dual quaternions which have been used to interpolate rigid-body motions.Based on screw theory, an efficient approach has also been developed for the type synthesis of parallel mechanisms (parallel manipulators or parallel robots)

2) I am not sure if I interpret you're question right. I think the point you might be missing, is that you assume a constant, fixed set of reference frame and a fixed axis used to describe the motion. If you assume that the axis is fixed you do not have enough parameters left to describe a complex motion. If you consider that the axis is instantaneous, then you have all 6 parameters available and complex motions can be descirbed. Note that the reference frame in which the axis is expressed is fixed, but not the axis itself.

Title: Understanding Twists as 3D Velocities


答:
Measuring motor temperature and either forcing a stop or limiting performance when it gets too hot is common practice.  I have always used a simple thermistor.  You should make it have intimate contact with the motor's metal either by using thermal paste or epoxy.  Sometimes, you can put it in a small hole drilled in the motor mount.

The danger with motors getting too hot is the insulation melting off the very thin wires.  So putting the thermistor as close as possible to the winding is important.  With brushless motors you can sometimes get the thermistor inside and mounted right on the coils which is ideal.  Although I think this must usually be done in the factory.

The thermistor datasheet should give you the resistance vs temperature curve.  Being just a resistor, some external support circuitry will be needed.

Title: Measuring a Motor's Internal Temperature

Body: As a final safety measure regarding some high wattage motors, I want to measure their temperature (either internal or surface) in live time so that I can automatically kill the power if it crosses a threshold. Temperature modules I am finding appear to be mainly for air or liquids; I don't know if there's anything out there to measure the surface of metals (surrounded by air). A couple examples I don't think would work are the DS18B20 and DHT22 modules.</p>


答:
For your second question: Moving in the xy plane, rotating around the z axis can be treated as instantaneously rotating around a "virtual center", located along the line perpendicular to the xy velocity, with a distance that is the ratio between the speed and the spin rate.

This interpretation highlights the weakness of the "slide along and rotate around an axis" interpretation of twists -- if you were to reduce the spin rate, the axis would go further and further away. As the spin rate goes to zero, the axis distance goes to infinity, at spin=zero the axis is just the axis in the direction of the pure translation, and as the spin becomes negative, the axis jumps to negative infinity and then walks its way back in.

Title: Understanding Twists as 3D Velocities

Body:
I am just learning about twists to represent 3D velocities (e.g. of a robot's end-effector), and I have two questions:

1) Wikipedia defines a twist as "an angular velocity around an axis and a linear velocity along this axis". To represent a twist mathematically, this requires defining a point in 3D space (3 coordinates), the direction of the axis passing through this point (2 coordinates), and the ratio of the linear and angular velocities. This is 6 numbers in total. However, it seems that you could also represent this same 3D velocity just by defining linear velocity about the axes (3 coordinates), and a rotational velocity about the three axes (3 coordinates), so that the linear and rotational velocities are defined independently. This is also 6 numbers in total. So why do we define 3D velocities as twists, rather than using my version? Is it just that there are some nice mathematical properties of twists that my version does not have? Or is it that my version is actually fundamentally wrong in some way?

2) Let's say I have an object with positive x- and y- linear velocity, but zero z- linear velocity. The object is also spinning around its z-axis, but there is no rotation about the x- and y- axes. You can think of this as a ball sliding across the floor, whilst spinning about its vertical axis. I am struggling to understand how it is possible to represent this 3D velocity using "an angular velocity around an axis and a linear velocity along this axis". In this example, it seems clear to me that the linear velocity is acting orthogonally to the axis of rotation of the angular velocity. The ball is rotating about its z-axis, and so it doesn't seem possible to also define the linear velocity in terms of this z-axis, because the linear velocity only has components in the x- and y- axes. What am I missing?

Thank you!


问:
Omron G5V-2 relay NO pins not working
I could swear that it was working for a while. I got back to my desk, tried it again, and it's no longer working. Could I have fried the NO pins on both sides? This is a DPDT relay. Everything works normally on the NC pins. I have never applied more than 5V. I do hear the relay click when I apply 5V to the coil. But when I measure voltage on the NO pins, I get 0V. Has anyone else seen this? I have two of these relays and I can't seem to get voltage on the NO pins with either relay. I should clarify that I'm expecting the same 5V power source to power both the coil and the common pins. If the NC pins work then I don't see why the NO pins shouldn't. In both cases the 5V is shared between the coil and any load attached to the NC/NO pins. I did try driving the entire circuit off a 9V power supply, but that did not change the results (and that does contradict my earlier statement that I've never applied more than 5V to this relay). My circuit is based on Charles Platt's "Make: Electronics", p. 59.

Here's a pic of the schematic I am following, except that I am using a 5V relay and a 5V power supply (USB port) and I am using piezo buzzers without resistors instead of LEDs.

答:
Problem solved. It turns out that the pin assignment on my relay is different from that in the book.

Here's the schematic for my actual relay (notice that the pin assignment is CO,NC,NO instead of NC,CO,NO as in the book)

Schematic of my actual relay:

Title: Omron G5V-2 relay NO pins not working