答:
I found the solution.

Turns out

```<code>This robot has a joint named "base_plane__ArmBase" which is not in the gazebo model.
</code>```

This message was the key. I have set <transmission> tag for all the joints in my URDF. But gazebo treats every links that have fixed joint with each other as a single rigid body. Thus base_plane__base_bar & base_bar__ArmBase joints were not present in the robot model. So gazebo couldn't figure out where to set the hardware interface. This is why the simulation interface was not running.

So I removed the <transmission> tag for both of this joints and it runs expectedly.

Body:
I'm relatively new to ROS. I started with raw URDF exported from solidworks. Then created moveit_config package by moveit_setup_assistant. Here are the related files:
URDF file:

```<code><robot name="prochesta_arm">
    <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
    </material>
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1.0"/>
    </material>
    <material name="grey_2">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
    </material>
    <material name="pink">
        <color rgba="1.0 0.5 0.5 1.0" />
    </material>
    <material name="blue">
        <color rgba="0.0 0.4 1.0 1.0" />
    </material>
    <material name="green">
        <color rgba="0.0 1.0 0.5 1.0" />
    </material>
    <link name="base_bar">
        <!--inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="0.392347184025621" />
            <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial-->
        <visual>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.01 0.01 0.1"/>
            </geometry>
            <material name="grey"/>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.01 0.01 0.1"/>
            </geometry>
        </collision>
    </link>
    <joint name="base_plane__ArmBase" type="fixed">
        <origin rpy="0.0 0.0 0.0" xyz="0.0546412131471822 0.0 0.11" />
        <parent link="base_bar" />
        <child link="ArmBase" />
        <axis xyz="0.0 0.0 0.0" />
        <limit lower="0.0" upper="0.0" effort="1000.0" velocity="50.0"/>
    </joint>
    <joint name="base_bar__base_plane" type="fixed">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <parent link="base_plane"/>
        <child link="base_bar"/>
        <axis xyz="0.0 0.0 0.0"/>
        <limit lower="0.0" upper="0.0" effort="0.0" velocity="0.0"/>
    </joint>
    <link name="base_plane">
        <!--inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="0.392347184025621" />
            <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial-->
        <visual name="">
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="1.5 1.5 0.02"/>
            </geometry>
            <material name="grey"/>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="1.5 1.5 0.02"/>
            </geometry>
        </collision>
    </link>
    <link name="ArmBase">
        <inertial>
            <origin rpy="0 0 0" xyz="-0.0546412131471822 0.0142569733044021 0.0105265889786102" />
            <mass value="0.392347184025621" />
            <inertia ixx="0.00037313359562078" ixy="-3.88508187746182E-07" ixz="-9.7227823468437E-06" iyy="0.000612254064917396" iyz="6.685567634577E-07" izz="0.000292429195752973" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/ArmBase.STL" />
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/ArmBase.STL" />
            </geometry>
        </collision>
    </link>
    <link name="BaseShaft">
        <inertial>
            <origin rpy="0 0 0" xyz="-0.00124611489784311 -0.108184816547343 -1.18094289902615E-09" />
            <mass value="0.172603193441375" />
            <inertia ixx="0.000449963539893855" ixy="2.29673976828531E-06" ixz="4.03285265163475E-09" iyy="0.000436230277248471" iyz="-5.17320253939005E-12" izz="0.000511313152396509" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/BaseShaft.STL" />
            </geometry>
            <material name="pink"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/BaseShaft.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="ArmBase__BaseShaft" type="revolute">
        <origin rpy="-1.5708 0 -1.5708" xyz="-0.075617 0.021609 0" />
        <parent link="ArmBase" />
        <child link="BaseShaft" />
        <axis xyz="0 1 0" />
        <limit effort="1000" lower="-3.1416" upper="3.1416" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Shoulder">
        <inertial>
            <origin rpy="0 0 0" xyz="0.00251674474561527 0.170121527752926 0.00159696012847385" />
            <mass value="0.100791466963889" />
            <inertia ixx="0.000469966041530564" ixy="-5.89764046954074E-06" ixz="5.65207828543954E-09" iyy="7.13384967018999E-05" iyz="-3.44218965703515E-07" izz="0.000512104689605307" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Shoulder.STL" />
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Shoulder.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="BaseShaft__Shoulder" type="revolute">
        <origin rpy="1.5535 1.556 -1.5873" xyz="-0.02 -0.1456 0" />
        <parent link="BaseShaft" />
        <child link="Shoulder" />
        <axis xyz="-0.99989 0.01479 0.00025546" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-1" upper=".5" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Elbow">
        <inertial>
            <origin rpy="0 0 0" xyz="-4.98394797654456E-11 -3.62130138409911E-05 -0.17234820422493" />
            <mass value="0.155859341630008" />
            <inertia ixx="0.000268369583946352" ixy="2.061494260678E-10" ixz="-6.45772822678737E-11" iyy="0.000298530192977532" iyz="-4.44744351270454E-05" izz="0.000181542359930575" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Elbow.STL" />
            </geometry>
            <material name="pink"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Elbow.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Shoulder__Elbow" type="revolute">
        <origin rpy="-2.8163 -0.00025546 3.1268" xyz="0.0051765 0.34996 0.00027318" />
        <parent link="Shoulder" />
        <child link="Elbow" />
        <axis xyz="1 0 0" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-0.5" upper=".7837" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Yaw">
        <inertial>
            <origin rpy="0 0 0" xyz="-0.00438644516368353 0.0559409791205142 -0.0570850114857289" />
            <mass value="0.296477336396768" />
            <inertia ixx="0.000492376543215998" ixy="-1.09052812794105E-05" ixz="-2.66243025248788E-05" iyy="0.000302345188534722" iyz="-2.34599297023637E-05" izz="0.000282182542169169" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Yaw.STL" />
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Yaw.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Elbow__Yaw" type="revolute">
        <origin rpy="-0.0016492 -0.098555 0.033437" xyz="0 -0.098331 -0.29083" />
        <parent link="Elbow" />
        <child link="Yaw" />
        <axis xyz="0 0.94732 -0.3203" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-1.5708" upper="1.5708" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Pitch">
        <inertial>
            <origin rpy="0 0 0" xyz="0.00193637684331768 -0.00113094288383903 -0.00473550556070457" />
            <mass value="0.245672351084059" />
            <inertia ixx="0.000112445414870033" ixy="3.7801113676169E-06" ixz="1.13310562352918E-05" iyy="0.000222075669730552" iyz="8.19497975087401E-07" izz="0.000225909542391733" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Pitch.STL" />
            </geometry>
            <material name="pink"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Pitch.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Yaw__Pitch" type="revolute">
        <origin rpy="-0.095533 0.0036424 0.0092858" xyz="-0.013295 0.024778 -0.14277" />
        <parent link="Yaw" />
        <child link="Pitch" />
        <axis xyz="0.99459 -0.033268 -0.098395" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-1.5708" upper="1.5708" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Roll">
        <inertial>
            <origin rpy="0 0 0" xyz="0.013346395763443 0.026611833443925 0.0965457711475227" />
            <mass value="0.219051002803153" />
            <inertia ixx="0.000155851657200548" ixy="-1.61881929035119E-05" ixz="1.50471506253359E-05" iyy="0.000101493190927247" iyz="4.64669877102981E-05" izz="0.000174715266607372" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Roll.STL" />
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Roll.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Pitch__Roll" type="revolute">
        <origin rpy="-2.9699 0.21413 -0.25831" xyz="0 0 0" />
        <parent link="Pitch" />
        <child link="Roll" />
        <axis xyz="0.16462 0.40474 0.89949" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-3.1416" upper="3.1416" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="LeftClaw">
        <inertial>
            <origin rpy="0 0 0" xyz="0.0160548042850929 0.363052679901909 -0.277299510990579" />
            <mass value="0.0710508103277403" />
            <inertia ixx="6.33292286188025E-05" ixy="-7.37244945176046E-06" ixz="-1.6248611953797E-05" iyy="5.934165926219E-05" iyz="-1.6824179025353E-05" izz="2.45890867032075E-05" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/LeftClaw.STL" />
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/LeftClaw.STL" />
            </geometry>
        </collision>
    </link>
    <link name="RightClaw">
        <inertial>
            <origin rpy="0 0 0" xyz="0.0160548042897368 0.363052680008611 -0.277299511621476" />
            <mass value="0.0710508080215541" />
            <inertia ixx="6.33292264052498E-05" ixy="-7.37244969866602E-06" ixz="-1.62486114309369E-05" iyy="5.9341657110105E-05" iyz="-1.68241784848982E-05" izz="2.45890872002616E-05" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/RightClaw.STL" />
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/RightClaw.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Joint_LeftClaw" type="prismatic">
        <origin rpy="0 0 0" xyz="-0.0088606 -0.28846 0.43314" />
        <parent link="Roll" />
        <child link="LeftClaw" />
        <axis xyz="-0.96885 0.23743 0.070474" />
        <limit effort="1000" lower="0" upper="0.021" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <joint name="Joint_RightClaw" type="prismatic">
        <origin rpy="0.86688 -0.30065 3.0016" xyz="0.099182 0.50791 0.05503" />
        <parent link="Roll" />
        <child link="RightClaw" />
        <axis xyz="0.96885 -0.23743 -0.070474" />
        <limit effort="1000" lower="-0.021" upper="0" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>

    <transmission name="trans_ArmBase__BaseShaft">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="ArmBase__BaseShaft">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="ArmBase__BaseShaft_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_BaseShaft__Shoulder">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="BaseShaft__Shoulder">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="BaseShaft__Shoulder_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Shoulder__Elbow">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Shoulder__Elbow">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Shoulder__Elbow_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Elbow__Yaw">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Elbow__Yaw">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Elbow__Yaw_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Yaw__Pitch">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Yaw__Pitch">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Yaw__Pitch_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Pitch__Roll">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Pitch__Roll">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Pitch__Roll_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Joint_LeftClaw">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint_LeftClaw">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Joint_LeftClaw_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Joint_RightClaw">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint_RightClaw">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Joint_RightClaw_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_base_plane__ArmBase">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="base_plane__ArmBase">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="base_plane__ArmBase_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_base_bar__base_plane">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="base_bar__base_plane">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="base_bar__base_plane_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
            <robotNamespace>/</robotNamespace>
            <legacyModeNS>true</legacyModeNS>
        </plugin>
        <!--plugin filename="libgazebo_ros_joint_state_publisher.so" name="joint_state_publisher">
            <robotNamespace>/</robotNamespace>
            <jointName>
                ArmBase__BaseShaft,
                BaseShaft__Shoulder,
                Elbow__Yaw,
                Pitch__Roll,
                Shoulder__Elbow,
                Yaw__Pitch,
                Joint_LeftClaw,
                Joint_RightClaw
            </jointName>
        </plugin-->
    </gazebo>
</robot>
</code>```

gazebo.launch file:

```<code><!-- 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="true"/>
  <arg name="gui" value="$</span>(arg gazebo_gui)"/>
</include>

<!-- send robot urdf to param server -->
<param name="robot_description" command="xacro  '$(find mybot)/urdf/mybot.urdf.xacro'" />

<!-- unpause only after loading robot model -->
<arg name="unpause" value="<span class="math-container">$(eval '' if arg('paused') else '-unpause')" />
<!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
<arg name="world_pose" value="-x 0 -y 0 -z 0" />
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $</span>(arg unpause) <span class="math-container">$(arg world_pose) $</span>(arg initial_joint_positions)"
  respawn="false" output="screen" />

<!-- Load joint controller parameters for Gazebo -->
<rosparam file="<span class="math-container">$(find mybot_moveit_config)/config/gazebo_controllers.yaml" />
<!-- Spawn Gazebo ROS controllers -->
<node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
<!-- Load ROS controllers -->
<include file="$</span>(dirname)/ros_controllers.launch"/>
</code>```

After launch the error I get:

```<code>[ERROR] [1681988267.705543000]: This robot has a joint named "base_plane__ArmBase" which is not in the gazebo model.
[FATAL] [1681988267.705556000]: Could not initialize robot simulation interface
</code>```

...

```<code>[WARN] [1681988296.560628, 28.000000]: Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [1681988296.561593, 28.001000]: Controller Spawner couldn't find the expected controller_manager ROS interface.
</code>```

The gazebo_ros_controller plugin doesn't load. I have a similar arm which I have set up exactly the same way and that works expectedly whereas this one just falls down in the gazebo.

I have found similar problems in this forum and over the internet, but none of them works.

[Edit]
Here is the github repository for both the robots. mybot is the one that runs successfully.
GitHub

Body:
I found the solution.

Turns out

This robot has a joint named "base_plane__ArmBase" which is not in the gazebo model.

This message was the key. I have set <transmission> tag for all the joints in my URDF. But gazebo treats every links that have fixed joint with each other as a single rigid body. Thus base_plane__base_bar & base_bar__ArmBase joints were not present in the robot model. So gazebo couldn't figure out where to set the hardware interface. This is why the simulation interface was not running.

So I removed the <transmission> tag for both of this joints and it runs expectedly.


问:
Difficulty with tuning motor PID controller
I am struggling for a while now with tuning PID control on my motors. I have bought different motor driver boards better motors etc but still I am unable to get a good control on the motors.

What I am trying to do is give a setpoint in RPM and the PID controller should reach this setpoint (pretty straight forward I guess). I use at the moment two Pololu 4755 motors which have encoders built on them. I have connected them to a Pololu TB9051FTG motor driver board. As µController I use a Nano IoT it counts with interrupts the amount of pulses in a time interval of 100 ms and then computes the rpm value.

This value I parse to the PID controller, found at the Arduino Library (PID_v1.h) , as input. The output is a PWM signal that should control the motor.

I have started with Kp and increases it up to oscillation occurs and that used the half of that value as a starting point. If I do so (Ki and Kd are set to 0) I have already a huge error i.e. 30%. Then I tried to adjust the Kd and Ki but the system will never gets stable. I'm completely lost I don't know where to start fixing this issue. Is it my code is it hardware or is it the way I tune the controller.

So I hope someone can help me fixing this issue I would be very happy to understand this problem. Below I have posted the code I currently use.

```<code>#include <Arduino.h>
#include <PID_v1.h>

#define ENC_COUNT_REV 16                          // Motor encoder output pulses per
rotation, property of the encoder see comments above.
#define ENC_IN_LEFT 2                             // Interrupt pin for Left encoder
input pulses per rotation
#define ENC_IN_RIGHT 3                            // Interrupt pin for Right encoder
input pulses per rotation
#define PWM_Left_Motor 10                         // Left Motor PWM output to Motor
driver board
#define DIR_Left_Motor 5                          // Direction output for left motor,
High is one direction and Low other direction
#define Enable_Left_Motor 4                       // Enable pin for left motor
#define PWM_Right_Motor 11                        // Right Motor PWM output to Motor
drive board
#define DIR_Right_Motor 7                         // Direction output for right motor,
High is one direction and Low other direction
#define Enable_Right_Motor 6                      // Enable pin for right motor

volatile long encoderValueLeft = 0;               // Pulse values from encoder outputs
volatile long encoderValueRight = 0;

long previousMillisLeft = 0;                      // Counters for duration measurement
of intervals for left and right motors
long previousMillisRight = 0;
long currentMillisLeft = 0;
long currentMillisRight = 0;

int rpmLeft = 0;                                  // Variables for RPM measurements
int rpmRight = 0;

int interval = 100;                               // Constant for measurement interval,
during this interval the amount of pulses is counted

int PWM_Left_Motor_Output = 0;
int PWM_Right_Motor_Output = 0;

// PID variables
double Setpoint;
double LeftInput;
double RightInput;
double LeftOutput;
double RightOutput;

PID PID_Control_Left_Motor(&LeftInput, &LeftOutput, &Setpoint, 1, 0, 0, DIRECT);
PID PID_Control_Right_Motor(&RightInput, &RightOutput, &Setpoint, 0.035, 0.5, 2,
DIRECT);

void updateEncoderLeft()
{
  encoderValueLeft++;                           // Increment the value with 1 each time
 the interrput is called
}

void updateEncoderRight()
{
  encoderValueRight++;                            // Increment the value with 1 each
 time the interrput is called
}

void setup()
{
 Serial.begin(9600);

  pinMode(ENC_IN_LEFT, INPUT_PULLUP);               // Set the pin as input with
  internal pullup resistors
  pinMode(ENC_IN_RIGHT, INPUT_PULLUP);
  pinMode(PWM_Left_Motor_Output, OUTPUT);           // Set output PWM channels
  pinMode(PWM_Right_Motor_Output, OUTPUT);
  pinMode(DIR_Left_Motor, OUTPUT);                  // Set direction control pins
  pinMode(DIR_Right_Motor, OUTPUT);
  pinMode(Enable_Left_Motor, OUTPUT);               // Set enable motor control pins
  pinMode(Enable_Right_Motor, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(ENC_IN_LEFT), updateEncoderLeft, RISING);
  //Attach an interrupt
  attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT), updateEncoderRight, RISING);
  //Attach an interrupt

  previousMillisLeft = millis();
  previousMillisRight = millis();

 // Set setpoint to 5000rpm
 Setpoint = 5000;

 PID_Control_Left_Motor.SetSampleTime(50);
 PID_Control_Right_Motor.SetSampleTime(100);

 PID_Control_Left_Motor.SetMode(AUTOMATIC);
 PID_Control_Right_Motor.SetMode(AUTOMATIC);
 }

 void loop()
 {
  // Configure motor direction and enable motors
  digitalWrite(DIR_Left_Motor, LOW);
  digitalWrite(DIR_Right_Motor, LOW);
  digitalWrite(Enable_Left_Motor, HIGH);
  digitalWrite(Enable_Right_Motor, HIGH);

  // Update left RPM value
  currentMillisLeft = millis();
  if (currentMillisLeft - previousMillisLeft > interval)  // Call this loop if the interval time has passed
  {
      previousMillisLeft = currentMillisLeft; // Reset previousTime for next computation
      rpmLeft = (double) (encoderValueLeft * 600 / ENC_COUNT_REV); // Compute the speed in rpm with the total amount of pulses counted by the interrupt
      Serial.print("Left speed = ");  // Serial print only for debugging
      Serial.println(rpmLeft);
      LeftInput = rpmLeft;
      encoderValueLeft = 0; // Reset the encoderValue to 0
   }

  // Update right RPM value
  currentMillisRight = millis();
  if (currentMillisRight - previousMillisRight > interval) // Call this loop if the interval time has passed
   {
    previousMillisRight = currentMillisRight; // Reset previousTime for next computation
    rpmRight = (double) (encoderValueRight * 600 / ENC_COUNT_REV);// Compute the speed in rpm with the total amount of pulses counted by the interrupt
    Serial.print("Right speed = "); // Serial print only for debugging
    Serial.println(rpmRight);
    RightInput = rpmRight;
    encoderValueRight = 0; // Reset the encoderValue to 0
    }

   PID_Control_Left_Motor.Compute();
   PID_Control_Right_Motor.Compute();

   // Set a PWM output to the motors
   analogWrite(PWM_Left_Motor, LeftOutput);
   analogWrite(PWM_Right_Motor, RightOutput);
 }
</code>```

答: I believe your problem is the rate that you are sampling and controlling the motor.  A 10Hz sample rate for PID control is extremely slow.  In my experience, 1000Hz to 2000Hz sampling provides reliable control.</p>

ContentLicense: CC BY-SA 4.0


问:
Start gazebo headless mode based on launch config argument
I was trying to start a simulation in ingition gazebo based on a condition.

condition

launch argument : headless_mode (bool) -> default : False

if headless_mode :
     start simulation in headless mode
else :
     start simulation normally
```
<p>The launch file that I have written is given below, The problem I am having is that the condition is not working by externally setting the <strong>headless_mode</strong> to <strong>true</strong></p>
<pre class="lang-py prettyprint-override"><code>import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    sim_dir = get_package_share_directory('simulation')
    world_launch_file = os.path.join(sim_dir, 'sdf', 'world.sdf')

    # Choose ignition headless rendering mode
    headless_mode = LaunchConfiguration('headless_mode')
    declare_headless_mode_argument = DeclareLaunchArgument(
        'headless_mode',
        default_value='False',
        description='Whether to render ignition headless or not'
    )

    # World launch process
    world_launch = ExecuteProcess(
        cmd=["ign", "gazebo", "-r"]
        + (["-s"] if (headless_mode == "True") else [])
        + [world_launch_file],
        output="screen",
        condition=IfCondition(use_ign_sim),
    )

    return LaunchDescription([
        declare_headless_mode_argument,
        world_launch,

    ])
</code>```

答:
Track down headless in Nav2 for an example of how it works.

It's worth reading the Launch architecture readme file to get some context about how to work with substitutions and actions in this environment.


问:
Design of the baseplate of mobile robot
I need to construct a Mobile robots preferably circular type of diameter 30 cm, it will be carrying a payload of 5kg. What will be optimal thickness the baseplate considering the material as aluminium? How to calculate these kind of these kind of problems</p>

答:
Unless the robot is going to have an unusually heavy payload or be subject to excessive forces, you will probably be OK with any reasonable thickness aluminum plate.

It does not provide any real benefit to calculate that you need a plate with a thickness of 1.45657 mm to provide just enough strength for your payload since you cannot purchase aluminum plate with that precise thickness and it would be prohibitively expensive to machine down a plate that precisely.

You would be better off simply checking with your supply house and get what they have already in stock such as 2.6 mm (about 0.1 inch) plate of 6061.

Title: Design of the baseplate of mobile robot


问:
Parallel spatial mechanism - DOF

The parallel mechanism is composed of a moving platform
and a base (or frame) connected by four legs. Each leg is composed of five
revolute joints numbered 1, 2, … 5 from the base to the moving platform. The
axes of the revolute joints 3, 4 and 5 in leg 1, the revolute joints 1 and 5 in leg 2,
as well as the revolute joints 1, 2 and 3 in leg 3 are all parallel to the Z-axis. The
axes of the remaining revolute joints in each leg are parallel

I want to know the DOF. Can someone help?

答:
Usually, the motion capability of the platform with this kind of constraint can be obtained with the basic DoF formula such as Tsai's Formula, i.e.,

$F =\lambda(n-j-1)+ \sum f_i $ where $\lambda$ is motion parameter, $n$ is number of link, $j$ is number of joints and $f_i$ is degree of relative motion permitted by joint $i$.

For the particular manipulator you provided,
$\lambda=6, n=14, j=15$ and $f_i=15$. Hence, we get

$$3 = 6(14-15-1)+ 15$$

Title: Parallel spatial mechanism - DOF


问:
Testing and Lifespan of Industrial Robots?
How does one go about testing the robot once it is built? How does one predict the number of hours it can operate? I see most of the industrial robots for instance robotic arms have warranty of 12-18 months, how did they arrive at such an estimate, clearly testing for 12-18 months is not on option, so what is the procedure for determining the lifespan? </p>

答:
In order to analyze and predict the lifespan of an industrial robot arm correctly, two things need to be done:

Calculate the lifespans of the individual components that can fail.

Calculate the lifespan of the composite system of the robot arm.

The 2nd step is more straightforward.

If the mean time to failure (MTTF) of the $i$th component is $t_i$, the probability that the component will not fail before time $t$ is usually modeled with the exponential distribution:

$$R_i(t) = e^{-{t/t_i}}$$

The probability that a system with $n$ such components with MTTF $t_1$, $t_2$, ... $t_n$ will not fail is:

$$R(t) = e^{-\lambda t} = R_1(t)\cdot R_2(t) \cdots R_n(t) =
e^{-t\left[
(1/t_1)+(1/t_2)+\cdots (1/t_n)
\right]}
$$
so that the MTTF of the overall system is given by:
$$\frac{1}{\lambda} = \frac{1}{\frac{1}{t_1}+\frac{1}{t_2}+...\frac{1}{t_n}}$$

The first step is more difficult.

The essential components of the robot arm are the different revolute joints.  In general, each joint includes an actuator with a motor and a gearbox, so the set of failure times that must be considered are:
$$\left\{
t_{m1},t_{g1},t_{m2},t_{g2},...,t_{mD},t_{gD}
\right\}$$
where $t_{mk}$ is the motor MTTF of the $k$th actuator, $t_{gk}$ is the gearbox MTTF of the $k$th actuator, and $D$ is the number of degrees of freedom in the arm.

But each of these will depend on the load - i.e. torque - that the motor or gearbox must sustain over time, and this can be difficult to calculate because it varies over time.

For example, a gearbox may be specified in terms of the number of cycles for which less than 10% of gearboxes tested under the same conditions might fail ($L_{10}$) under the application of some torque (e.g. rated torque).  If one operates the gearbox at less than this particular torque, then $L_{10}$ will be longer, empirically according to some power law that depends on the gearbox design.  For harmonic drives, for example, $L_{10}$ is roughly proportional to the cube of the torque:

$$L_{10}(T) \approx L_{10}(T_r)\left(
\frac{T_r}{T}
\right)^3
$$

Assuming failure times are exponentially distributed, $L_{10}$ and MTTF $1/\lambda$ are related:
$$
L_{10} = -\frac{\ln{0.9}}{\lambda}
$$

So to be rigorous, one needs to take into consideration the robot arms trajectory over time in order to understand the output torque at each joint over time and somehow compute how the lifetime is impacted.  It is very complex.  There is a paper here that discusses some simulations in this context.

ContentLicense: CC BY-SA 3.0


问:
Why are bird-like bipedal robots more efficient than humanoid robots?
Well, some websites wrote that "Robotic legs with an unconventional design inspired by flightless birds can run 300 per cent more efficiently than the same device would if designed traditionally.".

I couldn't find an online version to read the article, but the article itself says that it is "energy-efficient".

Assuming it is not just an unbased affirmation, why is that bird-like robots are more energy efficient while walking?

答:
You might try searching IEEE Explore for papers on the subject.  The IEEE sponsors a humanoid robotics conference each fall that publishes a journal.  I attended one year and I think I recall a couple of papers that might have touched on this.  The papers are not free, but sometimes if you search the web you can find free preprints.

Some reasons for increased efficiency in bird-like bipedal robots include:

Stability: Bird-like robots often have a lower center of gravity and
a wider base due to their legs being angled outward, which can
provide better stability than humanoid robots with vertically aligned
legs. This stability can make it easier for bird-like robots to
maintain balance, especially on uneven terrain.  This would simplify the control system and/or control algorithms required, possibly requiring fewer and/or less complex sensors that add additional weight.

Passive dynamics: Bird-like robots can take advantage of passive
dynamics, which rely on the robot's mechanical design and natural
forces like gravity to conserve energy while walking or running. For
example, the inverted pendulum model used in bird-like robots can
result in efficient gait patterns that minimize energy consumption.  This would lead to much longer battery life, assuming the robot is untethered.

Simplicity: Bird-like robots typically have fewer joints and degrees
of freedom compared to humanoid robots, which can simplify the design
and control algorithms needed for locomotion. This reduced complexity
can make bird-like robots less prone to mechanical failures and
easier to maintain.  This would greatly simplify the mechanical design, requiring fewer and lighter actuators.  The weight of a robot arm increases linearly with both required torque and number of degrees of freedom, requiring more power.

Adaptability: Bird-like robots can often traverse a wider range of
environments, such as climbing steep inclines or navigating rough
terrain, more easily than humanoid robots. Their unique leg designs
can allow for greater flexibility and adaptability to various
conditions.


问:
Conversion of roll, pitch and yaw from ENU to NED
I have roll, pitch and yaw values in an ENU frame.

How can I convert it into a NED frame?

答:
ENU = East, North, Up

NED = North, East, Down

So, if you have x_ENU, y_ENU, and z_ENU, then:

```<code>x_NED = y_ENU;
y_NED = x_ENU;
z_NED = (-z_ENU);
</code>```

This is like saying North = North, East = East, and Down = (-Up)

Body:
ENU = East, North, Up

NED = North, East, Down

So, if you have x_ENU, y_ENU, and z_ENU, then:

x_NED = y_ENU;
y_NED = x_ENU;
z_NED = (-z_ENU);

This is like saying North = North, East = East, and Down = (-Up)


问:
How to control the position of a load shared between two (servo)motors?
I want to position-control an axis combining the action of two DC servomotors - one of them alone is not powerful enough; I had those available and preferred not to buy a new one.

I have not the possibility to measure the output axis position directly, so I must rely on the integrated position, speed or torque feedback control logic built in my actuators.

Long story short, the two motors will transmit power to the output axis through gears (a separate gearbox for each one).

Now I am approaching the design of the control system and I realized that using the built-in position-control logic for both actuators might not be a recommendable choice, the reason being that it will be practically impossible to calibrate them with the exact same zero reference, meaning that a small misalignment in the motors' output positions will arise, leaving during operation just a single one of them in actual contact with the respective geartrain, sustaining all the load, and the other unloaded.

The only alternative approach I can think about is the following one: to control one of the two motors (let's call it 1) in position, while controlling the other one (call it 2) in torque by feeding it, as setpoint, the torque corresponding to the current absorption measured on motor 1.

This way motor 2 should provide power until it reaches the same torque motor 1 is providing, thus sharing the load without introducing uncertainty on what position setpoint is being followed.

I would like to know if this approach could be likely to succeed, if there are some precedents and/or if there exists any better solution.

答:
I've done several applications similar to this one, which we call here as "Torque sharing", where two motors move the same load.

Your approach is right: Control a motor in Position Mode and the second only cares about applying a torque vector oriented in the same direction and amplitude as the master, with time the load will be shared and the torque on the master will reduce and it will be possible to observe both dividing the work.

As the motors are different and the reduction too (?) you may need to work with some gain/scale before applying to the slave.

In your case, it seems that the coupling between the motors and the load is rigid, that is, there is no risk of slipping, eventually this causes some problems in this type of application that I don't even like to remember lol...

In my applications the "Torque Mode" of the servos was nothing more than a "Speed Control Mode" with limited torque.

Some tips that may help you:

The slave motor (in Torque Mode) must have the fastest possible acceleration/deceleration ramp and with gains for high dynamics (if possible), because you may not want it to take too long to respond in helping the master.

Adjusting the slave speed limit to the motor limit or a percentage above the current master speed (Eg: MasterSpeed x 1.3 >>>> 130%), this helped to improve the response for the axis never reaching the sepoint speed while maintaining the set torque.

The torque update to the slave must be as fast as possible, or conflict between the motors may occur, leading to excessive vibration.


问:
Python's all tf frames function in ROS2 only returns some frames
Spining off of 347857#comment that seems ROS1 code, I tried to get all tf frames in Python but the following quick dirty code goes into "No frames" else block, while I verified that tf frames exists by using ROS2 Service in the answer I posted in the same link (See UPDATE section the concrete command FYI). What's missing/wrong?

```<code>#!/usr/bin/python3

import time
import yaml

import rclpy
import tf2_ros

class TfFramesFinder(rclpy.node.Node):
    def __init__(self):
        super().__init__("tf2_frames_finder")
        self.get_logger().info("In class")

        self._tf_buffer = tf2_ros.buffer.Buffer()
        self._tf_listener = tf2_ros.transform_listener.TransformListener(self._tf_buffer, self)
        time.sleep(20.0)
        _frames_dict = yaml.safe_load(self._tf_buffer.all_frames_as_yaml())
        if _frames_dict:
            _frames_list = list(_frames_dict)
            if _frames_list:
                self.get_logger().info(_frames_list)
        else:
            self.get_logger().warn("No frames collected")

rclpy.init()
node = TfFramesFinder()
try:
    rclpy.spin(node)
except KeyboardInterrupt as e:
    print("k/b interrupted")

rclpy.shutdown()
</code>```

UPDATE I noticed I needed to have spin cycle completed at least once (answers.ros.org#386260). With that applied to my code, I do get only fraction of the existing frames, not the whole.

```<code>#!/usr/bin/python3

import time
import yaml

import rclpy
import tf2_ros

class TfFramesFinder(rclpy.node.Node):
    def __init__(self):
        super().__init__("tf2_frames_finder")
        self.get_logger().info("In class")

        self._tf_buffer = tf2_ros.buffer.Buffer()
        self._tf_listener = tf2_ros.transform_listener.TransformListener(self._tf_buffer, self)
        time.sleep(5.0)

    def get_all_frames(self):
        _frames_dict = yaml.safe_load(self._tf_buffer.all_frames_as_yaml())
        if _frames_dict:
            self.get_logger().info(yaml.dump(_frames_dict))
        else:
            self.get_logger().warn("No frames collected")

rclpy.init()
node = TfFramesFinder()
try:
    rclpy.spin_once(node)
    node.get_all_frames()
except KeyboardInterrupt as e:
    print("k/b interrupted")
rclpy.shutdown()
</code>```

With this I got only turtle2 frame.

```<code>[INFO] [1683237201.154050606] [tf2_frames_finder]: turtle2:
  broadcaster: default_authority
  buffer_length: 0.0
  most_recent_transform: 1683237200.355238
  oldest_transform: 1683237200.355238
  parent: world
  rate: 10000.0
</code>```

While there's also turtle1 frame.

```<code># ros2 run tf2_tools view_frames
[INFO] [1683237263.811576147] [view_frames]: Listening to tf data for 5.0 seconds...
[INFO] [1683237268.821867546] [view_frames]: Generating graph in frames.pdf file...
[INFO] [1683237268.825114244] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml="turtle1: \n  parent: 'world'\n  broadcaster: 'default_authority'\n  rate: 62.685\n  most_recent_transform: 1683237268.819402\n  oldest_transform: 1683237263.810233\n  buffer_length: 5.009\nturtle2: \n  parent: 'world'\n  broadcaster: 'default_authority'\n  rate: 62.686\n  most_recent_transform: 1683237268.819385\n  oldest_transform: 1683237263.810261\n  buffer_length: 5.009\n")
</code>```

答:
The tf information is published from one of more nodes across the system. Because you are creating a fresh buffer with a new buffer you need to let it listen long enough that it will have heard from the lowest rate publisher with the highest latency on your system before you can expect to have a full spanning set of the tree. And you need to leave enough time for connections to all those potential publishers to be established. You might get lucky in a single spin but its usually a matter of several seconds for everything to be established.

If you need it want a faster query you can configure and query an already running buffer in another node with established connections and ask it for what it has already seen via the buffer server client interface.

You'll want to add a Buffer Server C++ only to an existing node. And then query it from the Buffer Client c++ or Python in another node.


问:
How to detect the position in a linear actuator?
I was planning on 3D printing my own 30 cm stroke length 3D printed linear actuator and I don't know how I could read its position, or tell it to go say to 16.5 cm. I used this video, 3D Printed Linear Actuator??? Surprisingly Strong!, as reference

Issue

The problem is, the guy in the video is using only 2 mechanical endstops, which won't work for accurate positioning or anything between 0 and 30cm. Not only that, but since my actuator needs to be strong I will have to print a planetary Gearbox of 10:1, which means the input and output shaft will have completely different rotations, and thus positions!

I looked into Absolute Magnetic encoders and found this video of the AS5600, Measuring speed and position using the AS5600 magnetic encoder. And I had two problems:

What happens if the rotation goes beyond 360 degrees? Does it go back to 0?

And it turns out it keeps incrementing the value as shown in the video, so it won't go back to 0.

BUT what if we cut the system power? Will its position be remembered? Say it was at rotation position 1053.2, will it go back to 0 when the power comes back? This will lead to losing the position data!

The solution I thought of

So if the position is not lost at power off, then calculating the position of the gearbox output shaft would be very simple:

Output Angle = Input Angle / Reduction Ration % 360

(If I got it right)

And it would be as easy as mounting the encoder on the back of the driver motor!

Is this right? Does the encoder remember the position even after the power off? Do you guys have experience with any absolute encoder that can do that, and NOT return to 0 once it reached 360 degrees?

答:
In a linear actuator usually there is a stepper motor or a DC brush(-less) motor. Together with an appropriate encoder you can measure its position deviation from the initial position on start-up.

Issue Comment: Even if you have multiple different gears, the input/output ratio of the gear combination will always be predictable and can be described as a (mostly linear) function. Hence to compute the actual position of the piston you just have to transform the deviation measured on the rotational motor based on this function. It takes a little paperwork if your gearbox is complicated but it should definitely be possible.

Comment 1.:
In most cases this is done after reading the registers of encoder and handled in low-level software. For example if you are using an Arduino to read your encoder, you could handle this in your main.c code to account for overflow.

Comment 2.: The simplest and I assume the method used the most is to do a calibration on start-up. This means that you move your actuator to the max or min limit and set the origin point there.

In some cases you need to know which was the last position. This position can be stored in a NVRAM, as mentioned in one comment. This memory is kept even if there is no power. Just be aware that if you move your linear actuator when power is off, this will not update the NVRAM!

An alternative which knows the exact absolute position at all times is to use a closed loop encoder. This means that there is some feedback loop from a measurement method in the encoder or the motor. If you really need this feature there are for sure systems which can offer you a closed loop encoding. However, since this requires more technical affinity, these products will usually be more expensive.

I hope this answers some of your doubts and gives you some ideas to approach the problem.


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

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

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

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

答:
You can use all versions of the Kinect for Windows SDK, even with an Xbox-version. Some parts are limited and you require a Kinect for Windows in commercial scenarios (more information on my blog).

In your scenario you should be able to use the official Kinect for Windows SDK v1.8 to use the speech/sound scenario. If you are going to do speech/voice recognition, in this post I set up a basic version to controll my drone which should be the same.

You don't need to use VIsual Studio but C#/C++/VB.net require a compiler to run the code.

CONCLUSION - The official SDK will fit your needs, incl. support for speech/voice, but if you want to deploy the application in public you'll need the Kinect for Windows sensor for the public license.

More questions? --> hello[at]kinectingforwindows[dot]com

Title: Kinect for Xbox: SDK selection


问:
Multisession odometry
I have two bag files with significant overlap and multiple loop closure opportunities. However, these bag files were recorded at different times. I want to use these bag files to reconstruct a scene and chain the reconstructions together. Currently, I am using fastlio with scancontext.

However, when I run the second bag file in the same fastlio session, the reconstruction breaks, possibly due to imu initialization issues. If I keep the scancontext session and restart the fastlio session, the reconstruction gets placed at 0,0,0, and scancontext cannot find a proper match.

Can anyone suggest a workflow for chaining the reconstructions together using multi-session data and bag files with significant overlap? How can I avoid these initialization issues in fastlio and maintain the proper placement of the reconstruction using scancontext?

I would also like to ask if there are alternative solutions for using Livox LiDARs (Mid-360) and offline SLAM to chain reconstructions together with multi-session bag file data.

答:
Have you tried the graph optimization approach?

Cut the fastlio local map into segments

Generate a pose graph from the segments

Repeat 1,2 for the other dataset

Run a place recognition algorithm like BoW3D or scancontext to find the correspondence between the segments

Find the 6 dof transformations between the matchings

Add them to the graph

Run the optimization

Title: Multisession odometry


问:
How to exchange commands and states between hardware interfaces (to generate a CAN message)
I have a robot with ackermann steering and front 2 wheel drive. The vehicle already has all the mobility controls executed by the ground mobility module. The autonomy layer (the computing unit) will communicate and send drive and steering commands to the existing vehicle mobility module via CAN.

I have 2 hardware interfaces, one for steering and one for wheel speed, BUT both signals are part of the same CAN message.
For example:

```<code><ros2_control name="SteeringHarware" type="system">
   <hardware>
       <plugin>steering_hardware/SteeringHardware</plugin>
   </hardware>
   <joint name="steering_joint">
       <command_interface name="position"/>
       <state_interface name="velocity"/>
       <state_interface name="position"/>
   </joint>
</ros2_control>

<ros2_control name="WheelSpeedHardware" type="system">
   <hardware>
       <plugin>wheel_speed_hardware/WheelSpeedHardware</plugin>
   </hardware>
   <joint name="wheel_joint">
       <command_interface name="velocity"/>
       <state_interface name="velocity"/>
       <state_interface name="position"/>
   </joint>
</ros2_control>
</code>```

I need to send a CAN message to the vehicle module containing both desired Steering and Wheel Speed.
I can't do it in the hardware interfaces since I wouldn't know what to put for the other one.
How should I approach this?
Initially I thought to have the hardware interfaces publish this information to a topic and then another node would take both info and send the CAN message. However, this breaks the separation of concerns.

I have looked into Actions and Transmissions and the latter seem the more appropriate.

Any help will be appreciated.
Thank you

答:
Moving these comments to an answer:

Why are speed and steering both on the same message? Just split the messages. It's also hard to follow your description, though - I can't tell if you have one device making steering messages and one making throttle messages, or one making both and it going to one device consuming steering and one consuming throttle. But regardless, split the message. A throttle doesn't (shouldn't) know anything about a steering, and vice-versa. When you get a steering message, actuate steering. When you get a throttle message, actuate throttle.

What if I cannot change the way the vehicle CAN is setup? And, assuming I could split the messages, what if I need to pass other information in the message? I think, regardless of my specific use case, there are many cases where passing additional information to the hardware interface can be very useful. Also, there is only 1 device on the vehicle that needs a CAN message with both steering and throttle. The device consumes that message and does the rest, commanding the two actuators.

The questions you're asking are kind of the heart of engineering.

what if I need to pass other information in the message?

You need to think through everything you're trying to do up front. If you think your messages should be throttle and steering, and then later you realize you need to also include a brake torque value, or a parking brake signal, or turn signal/headlight status, transmission gear selection, etc. - if you don't include the data you need from the start then you need to change the message later.

What if I cannot change the way the vehicle CAN is setup?

This question is ambiguous. If you mean you can't change the CAN configuration in that you can't add or change messages then you're out of luck because you can't extend the messages or create other messages to cover all of your use cases. If you mean you can't change it in that you've got a steering controller generating steering signals and a throttle controller generating throttle signals then that's not an issue; have the steering controller send steering-only messages and the throttle controller send throttle-only messages.

You're missing a block diagram that explains how your network is setup, what the flow of messages are, etc.

The line I keep coming back to in your problem statement is this:

I need to send a CAN message to the vehicle module containing both desired Steering and Wheel Speed. I can't do it in the hardware interfaces since I wouldn't know what to put for the other one.

This makes it sound like you have two different publishers, and each only knows how to fill out half of the CAN message. This is the basis for my suggestion to split the message. If you're not able to change the message then you could use a magic number, so a steering unit would publish 9999 or something for throttle, because it doesn't have a throttle, and then your receiver could look for the 9999 value and ignore that portion of the message.

But ultimately a steering device makes a steering message, and the receiver acts on that steering message. A throttle device makes a throttle message, and the receiver acts on that throttle message.


问:
Why visual SLAM is seldomly used in autonomous driving?
In research called Occupancy Network by Tesla, the occupancy map is constructed by the deep learning method.

We know that visual SLAM can compose maps like Octomap, either. And Octomap can be used for quadrotor's fast flight (can refer to this RAPTOR article, a TRO).

However, we can seldom see visual SLAM projects that have been used in the autonomous driving area (or maybe because I don't know, if you know any, please tell me, big thanks!!! ).

What's the reason for it?  Because visual SLAM can only construct sparse point clouds? And the time cost for constructing dense point clouds is heavy? Or are there any other reasons?

答:
There are a couple of reasons.

Autonomous driving is all about perception and localization. Not the mapping. For localization, a simple map and GPS are enough. In terms of perception, the conventional SLAM is not intended for it.

Road is changing every day. The conventional SLAM usually can't localize itself when the scene has significantly changed. Also, companies don't want to update their point cloud map every day.

There are another area where visual SLAM is used for autonomy such as logistics robot, robotic lawn mower, cleaning robot where small scale indoor localization using camera is just enough for their tasks.


问:
Does a controlling system need to be more complex than the system being controlled?
Is there any theoretical principle, or postulate, that states that the controlling system has to be more complex than the system being controlled, in any formal sense of the notion "complex"?</p>

答:
Approaching your question in a broader perspective... I believe that a good answer to it could be provided by Ashby's Law of Requisite Variety - postulated as follows:

When the variety or complexity of the environment exceeds the capacity of a system (natural or artificial) the environment will dominate and ultimately destroy that system.

Ashby's law is well-known as the First Law of Cybernetics and it could also be put as the following:

In order to deal properly with the diversity of problems the world throws at you, you need to have a repertoire of responses which are (at least) as nuanced as the problems you face.


问:
DH Parameter for OpenManipulator X from Robotis
I try to solve forward kinematics problem for OpenManipulator X from Robotis by using standard DH convention. The figure and dimension of the robot like this pic

I try to find DH parameter and got result like this

To match frame assignment requirement, I move frame 2 to the left 0.024m. But when I verify the forward kinematics result, the position of end effector is wrong when I move joint 3. For example if I move joint 3 to 90 degree, the position of end effector should be X = 0.024m, Y = 0, and Z = 0.455M. But I got from the software X = 0, Y = 0, and Z = 0.455. It is because of I move the centre of frame 3 to the left. But if I dont move it, the DH frame assignment rule doesn't match.

How to figure out this problem?

Here is my code to find forward kinematics

def std_DH(theta, alpha, a, d):
    DH = np.array([[np.cos(theta),  -np.sin(theta)*np.cos(alpha),   np.sin(theta)*np.sin(alpha),    a*np.cos(theta)],
                   [np.sin(theta),  np.cos(theta)*np.cos(alpha),    -np.cos(theta)*np.sin(alpha),   a*np.sin(theta)],
                   [0,              np.sin(alpha),                  np.cos(alpha),                  d],
                   [0,              0,                              0,                              1]])
    return DH
```

<pre class="lang-py prettyprint-override"><code>def forward_kinematics(q1, q2, q3, q4):
    _0T1 = std_DH(q1, np.pi/2, 0, 0.077)
    _1T2 = std_DH(q2+np.pi/2, 0, 0.128, 0)
    _2T3 = std_DH(q3-np.pi/2, 0, 0.124, 0)
    _3T4 = std_DH(q4, 0, 0.126, 0)
    _0T4 = _0T1.dot(_1T2).dot(_2T3).dot(_3T4)
    return _0T4
</code>```

Is there any method to find forward kinematics without using DH convention?

答: I struggled with the same thing. I solved it rotating the second frame to a fixed angle, and using the 0.130 m distance. I think this satisfies the DH convention.


问:
How to deal with .zst compresion during installation of ROS 2 Foxy on Debian Bullseye from source?
I want to install ROS2 from source on Debian Bullseye, since installation of a .deb package does not seem to be possible for that Debian version. For that reason, I added Ubuntu Focal repository of ROS 2, which broke the .zst compression support of dpkg and prevented me from installing python3-colcon-notification, which is also necessary for the installation of ROS 2. I can comment out the focal repository in sources.list.d/ros2.list and then run sudo apt update and sudo apt upgrade, which will restore .zst compression support for dpkg, but I need this repository, according to official installation from source ROS2 Foxy instructions.

How do I go? Is it somehow possible to build ROS 2 with "traditional" CMake, without Colcon

答:
Using a package repository from Ubuntu Focal on Debian Bullseye not a supported configuration. There's no testing or expectation that anything will work.

You don't link to which instructions you're following. However, since you are going into unsupported configuration the documentation will not necessarily be right. For example you assert "I need this repository, according to official installation from source ROS2 Foxy instructions." Whatever it says in the instructions you are going to have to interpret and adapt for attempting to build on you alternative platform taking into account the difference between your platform and the one that you're reading documentation for.

In this case since you're working on a platform for which you aren't finding debian packages the better approach to running into repositories that don't exist is to understand what the documentation needed from that repository and attempt to find ways to install it from other mechanisms. Often that will require installation from source. The primary user documentation generally doesn't go into that for things like underlying dependencies, and you're likely going to need to dig into the developer documentation of those types for those things.

For example you can find more about installing colcon on your system by searching "colcon install from source" for which will get your to https://colcon.readthedocs.io/en/released/user/installation.html which is a much better path then trying to shoehorn binaries from one platform into another.

It's also harder to necessary understand as your coming to a new project, but depending on you use case some components are required and some are recommended. It could help reduce your effort to try to understand that distinction and not spend extra time and energy adapting the unnecessary components. I say this to point out that it's unlikely that colcon-notifications specifically is necessary but more core colcon packages will be necessary. In the basic documentation we seek to provide the simplest experience for new users on the supported platforms. But as you're diverging it might be worth understanding more about what you're attempting to do before you try to do the adaptation.

I want to make it clear that this is going to potentially be a lot to adapt. And if you're just learning how to work with ROS I strongly suggest that you learn how to interact with it and use it successfully on one of the supported platforms before you try to do the adaptation to an unsupported platform. Trying to do both at once will likely take much longer as you won't be able to diagnose if you're off track as easily and likely end up spending a lot of time trying to solve problems that don't need to be solved. These often manifest themselves as XY problems. Which is why I'm ending up writing such a long post that doesn't appear to directly answer your problem directly.


问:
ROS force vector transformation
In my work, I am using force sensors that provide measurements based on their frame pose in Gazebo. Due to the fact that frames are not constructed based on DH parameters, I need to track the tip's pose in order to be able to apply force measurements to a haptic device {$H$} (i.e. not shown there). I'm struggling now with the right transformation matrix between the tip's frame and the frame of the haptic device. To create a minimal working example, take a look at the following picture.

The tip's frame {$T$} is shown at the top of the second arm. In the left side menu, there are the pose and the relative pose. The former seems to me the pose of {$T$} with respect to the base's frame. When the tip interacts with the enviroment, the force sensor provides measurements with respect to {$T$}, my question is what is the proper way to transform ${}^Tf$ to ${}^Hf$ using ROS capabilities?

答:
If you have robot state publisher that publishes transformations between links/joints. You can use tf package to fetch those transformations.

If you want to transform tip measurements into base frame, you should multiply T^-1 * tip_measurements.

To find out T you can use tf package and lookUpTransform to find out what's the transform between tip frame and the base frame.

You can check Python API for tf here and C++ API for tf here.

C++ API has helper classes TransformBroadcaster and TransformListener which can help you.

Title: ROS force vector transformation


问:
Benefits of the Number of Propellers
I am planning on creating a quad-copter with my Arduino that I have. I have created a few land robots before but no aerial vehicles, so this is all new to me. I was looking on the Internet for different models, and I see that most robots have 4 propellers. I have also seen a few hexacopters (?) and octocopters but that many propellers can't get a but out of hand. Does having 4 propellers the best and most efficient thrust to weight ratio, or will 3 propellers/arms work better?</p>

答:
Four fixed pitch propellers is one of the simplest configurations.  The motors are mounted rigidly and the control of the vehicle is 100% electronic through controlling the speed of the propellers ( http://en.wikipedia.org/wiki/Quadcopter#Flight_control ).

With the classical two prop helicopter configuration you need pitch control for the main rotor.  Let's say you have two blades on the main rotor you would need two additional actuators and you end up with a more complex and expensive design.  I think you could get by without pitch control of the tail rotor but I'm not sure. ( http://en.wikipedia.org/wiki/Helicopter_flight_controls#Flight_conditions )

A tri-copter would typically need tilt control of the rear motor. ( http://en.wikipedia.org/wiki/Multirotor )

The basic physics of the situation is that in order to move freely around in 3d space you require certain degrees of freedom.  A quadcopter is arguably the simplest mechanical design that provides those degrees of freedom.  It's a trade-off.

In terms of additional propellers, the trade-off is often the inertia of a single propeller.  Higher inertia means it's harder to change the speed of the propeller and so the vehicle is less agile and less controllable.  Therefore to generate more lift more propellers are often a better choice than a larger propellers (if you stick with the premise of fixed pitch, fixed mount).  Since adding one prop doesn't really make much sense you mostly see an even number of propellers.

Title: Benefits of the Number of Propellers


问:
Is there a better command line tool than check_urdf?
When I am working on my robot's URDF (for ROS2 GoPiGo3 robot), I edit the URDF (in a command shell/nano to the robot), then run check_urdf.  check_urdf will often tell me the line in the check_urdf source code where an error occurs but not the line in my URDF file.

I do not have remote VSCode or a graphical environment set up on my robot, so am looking for a command-line tool for checking URDF files that is better or will augment the ROS check_urdf tool.

I run the robot state and joint state publishers on my robot, not on the visualization platform.

答: To detect xml errors, you can use the xmllint tool. You can also pass a URDF file to gz sdf --check <file>, which will convert the file to SDFormat and then check it, but that's probably not exactly what you're looking for.


问:
URDF to SDF Frame of Reference Calculation for Base Joint Fixed vs Floating
I am trying to simulate a robot I do not want fixed to the Gazebo world I am spawning it into. However, when I make the joint between my base link and the world floating, I get unexpected behavior in the way children frames of reference are established.

To preface, here is all the relevant XACRO

```<code><?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">

  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/base_wide.xacro" />
  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/polou_4801.xacro" />

  <link name="world"></link>

  <joint name="world_to_base" type="floating">
      <parent link="world" />
  <child link="a_base_wide" />
  <origin  rpy="0 0 0" xyz="0 0 0" />
  </joint>

  <xacro:base_wide prefix="a" x="0.0" y="0.0" z="0.374" />

  <joint name="base_to_motor_body" type="fixed">
      <parent link="a_base_wide" />
      <child link="a_robot_body" />
      <origin  rpy="0 0 0" xyz="0 0 .359" />
  </joint>

  <xacro:polou_4801 prefix="a" x="0.0" y="0.0" z="0.0" phi="0.0" theta="0.0" psi="0.0" />
</robot>
</code>```

base_wide.xacro

```<code><?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="base_wide" params="prefix x y z">

  <link name="<span class="math-container">${prefix}_base_wide">
    <visual>
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="<span class="math-container">${x} $</span>{y} <span class="math-container">${z}" rpy="0 0 0"/>
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0"/>
        <mass value="131.0971" />
        <inertia ixx="0.1811" ixy="0.0" ixz="0.0" iyy="0.1809" iyz="0.0002" izz="0.18" />
    </inertial>
  </link>
  </xacro:macro>
</robot>
</code>```

polou_4801.xacro

```<code><?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="polou_4801" params="prefix x y z phi theta psi">

    <link name="<span class="math-container">${prefix}_robot_body">
    <visual>
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" />
      <geometry>
        <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" />
      <geometry>
          <mesh filename="file://file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} ${psi}" />
        <mass value="94" />
        <inertia ixx="3.4438" ixy="0.0" ixz="0.0" iyy="3.4438" iyz="0.0" izz="0.7338" />
    </inertial>
  </link>

  <joint name="<span class="math-container">${prefix}_body_to_rotor" type="revolute">
      <parent link="$</span>{prefix}_robot_body" />
      <child link="<span class="math-container">${prefix}_robot_rotor" />
      <axis xyz="0 0 1" />
<!--      <origin  xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" /> -->
         <origin  xyz="0 0 0.374" rpy="0 0 0" />
      <limit lower="0" upper="$</span>{4*pi}" effort="10" velocity="3"/>
  </joint>

  <link name="<span class="math-container">${prefix}_robot_rotor">
    <visual>
<!--      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z+0.75}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" /> -->
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
<!--      <origin xyz="<span class="math-container">${x} $</span>{y} <span class="math-container">${z+0.75}" rpy="$</span>{phi} <span class="math-container">${theta} $</span>{psi}" /> -->
      <origin xyz="<span class="math-container">${x} $</span>{y} <span class="math-container">${z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
<!--      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z+0.75}" rpy="<span class="math-container">${phi} $</span>{theta} <span class="math-container">${psi}" /> -->
      <origin xyz="$</span>{x} <span class="math-container">${y} $</span>{z}" rpy="0 0 0" />
        <mass value=".05" />
        <inertia ixx="0.00000" ixy="0.0" ixz="0.0" iyy="0.0000" iyz="0.0" izz="0.0000" />
    </inertial>
   </link>
  </xacro:macro>

</robot>
</code>```

Now in the top level for the joint "world_to_base", when I used floating, which is the setting I want to use, it seems like the transforms are not being done as expected. If I say I want to make a joint at 0.374 +Z, that means I want that joint at +.374 from the parent's frame of reference. If I create a floating joint the move the child +Z .359, that should create the frame of reference from which the joint is created. In that case, the joint would then be at +.359+.374 in the "world" frame. When I do a floating joint, I do not see this behavior.

However, when I used fixed for the connection between my base link and world, I do see this relationship in the link-joint tree.

It makes it difficult to write portable XACRO if I cannot assume that my make first joint in the XACRO (eg the motor body joint) is going to be offset WRT to its parent not and not world. Maybe I just need to refactor and add logic into the polou xacro that takes into account that all joints are being layed out wrt the world frame, but I would prefer not to and just have it calculate like it does for the "fixed" case.

答:
I think there is an issue with the URDF-to-SDFormat converter in libsdformat's parser_urdf.cc file since it currently ignores both floating and planar joint types. I've opened the following feature request to sdformat to properly parse the floating joint origin tags.

https://github.com/gazebosim/sdformat/issues/1282


问:
Arduino-based Drone Project: Struggling with Altitude Control
I've been working on a DIY project which involves creating a drone from scratch. My aim is simple yet challenging; I want the drone to ascend to a specified altitude, hover for a few minutes, then descend, all autonomously without any external controller.

Here are the components I'm using:

Arduino Uno as the primary microcontroller.
Adafruit BNO055 Absolute Orientation Sensor for orientation tracking.
Brushless motors for the propellers.
My approach thus far has been centered around PID (Proportional Integral Derivative) and LQR (Linear Quadratic Regulator) control methods for the stabilization and altitude control. However, despite my best efforts and countless hours poured into this project, I've been unable to achieve a successful, stable flight.

The main issue I'm experiencing is with achieving and maintaining the set altitude. The drone either overshoots the desired height or doesn't lift off properly. I'm sure it's a problem related to tuning the control systems but despite tweaking the PID and LQR parameters, I can't get it right.

答:
There are 2 primary parts to the control system - sensory input and control output. I'd recommend validating your sensory input if that is stable - i.e. provides desired output within reasonable noise limits at specific altitudes. The noise levels are an important factor as those are out of your control, and your controller would need to reject that (via some sort of filtering).
Once the sensor data is validated, plotting your controller output vs sensor input would yield what needs to be tuned in your controller (eg. is your D gain too high in your PID controller?)</p>


问:
Steering wheel angle control
I am working on an autonomous vehicle's control, specifically the steering angle control.

Knowing that I used kinematic bicycle model, my questions are:

How do I choose the actuator for my steering wheel angle?

Why did the article (below) use a stepper motor?

According to what do we choose an actuator to build a control system for steering angle control?

答:
How do I choose the actuator for my steering wheel angle?

Assuming this actuator will be directly driving the steering angle controller which take rotary inputs, you'll need a - Rotary actuator.

Velocity, Acceleration - how fast you want the actuator to turn, and how quickly would you like it to reach your desired velocity

Next is torque - this is the sum of static torque (i.e. torque required to rotate with 0 angular acceleration), dynamic torque (Torque required to accelerate - more on that: https://www.khanacademy.org/science/in-in-class11th-physics/in-in-system-of-particles-and-rotational-motion/in-in-rotational-inertia-and-angular-second-law/a/rotational-inertia-ap1) and some buffer to account for variations in the system.

Voltage - Match your system voltage - since motors draw significant power, sticking with battery voltage is ideal, rather than losing power in stepping up/down. (a 220V motor is no good on a setup where max 12V is available)

Why did the article (below) use a stepper motor?

Likely, they had a stepper motor and controller handy, but only the author can say for sure.

Folks also like to use stepper motors as they are slightly easier to control. Other actuators are suitable as well in a closed loop setup (eg. BLDC, Brushed motors). BLDC motors are almost always better for such applications as they provide a high torque at low speeds and can accelerate rapidly.

According to what do we choose an actuator to build a control system for steering angle control?

The above 2 should take you close to the finish line.

Title: Steering wheel angle control


问:
Visualize contour of camera field-of-view on plane
Is there a ros node that will take in a CameraInfo and target frame parameter to project the field-of-view of the camera onto a flat plane (defined by the target frame)?  Ideally it would output a line list of points in an rviz Marker, or something similar that is easy to visualize.

What the node would do is walk through at least the four corners (or sample multiple times along the edges, especially if the distortion is high) of the image and use opencv to project them into 3D lines using the intrinsics and distortion coefficients, transform those lines into the target frame, then figure out if they intersect Z == 0.0 (or make the plane selectable from X,Y or Z with a configurable offset), throw out a warning if nothing intersects, add intersecting points to the point list.

答:
I've made a ros node for this here: https://github.com/lucasw/vimjay/blob/eliminate_build_warnings/scripts/camera_info_to_plane.py (I'll get it into the main branch later)

https://youtu.be/VOY64AK3-N0

```<code>roslaunch vimjay camera_info_to_plane.launch
</code>```

This is the code, using opencv undistort on an array of points that trace the borders of the image, transforms them into the frame of interest to project onto, then finds the intersection of each point with z == 0.0 on that plane.

It's a little slow, the for-loop for intersection finding is an obvious candidate for vectorization, and there may be a faster way than creating a PointCloud2 to transform a bunch of 3D points from one frame to another (if anyone knows please leave a comment- or submit a PR).  It could also cache results from previous updates if it detects nothing in the relative transform or camera info has changed, or at the least cache the border point strip if the number of points per edge hasn't changed.

```<code>def camera_info_callback(self, msg: CameraInfo):
    try:
        tfs = self.tf_buffer.lookup_transform(self.target_frame, msg.header.frame_id,
                                              msg.header.stamp, rospy.Duration(0.3))
    except (tf2_ros.LookupException, tf2_ros.ExtrapolationException) as ex:
        rospy.logwarn_throttle(4.0, ex)
        return

    camera_matrix, dist_coeff, rvec, tvec = camera_info_to_cv2(msg)

    num_per_edge = 8
    num_points = num_per_edge * 4 + 1

    points2d = np.zeros((num_points, 2))
    ind = 0
    # create points in a loop around the edge of the image
    for i in range(num_per_edge):
        fr = i / num_per_edge
        points2d[ind, 0] = fr * msg.width
        ind += 1

    for i in range(num_per_edge):
        fr = i / num_per_edge
        points2d[ind, 0] = msg.width
        points2d[ind, 1] = fr * msg.height
        ind += 1

    for i in range(num_per_edge):
        fr = 1.0 - i / num_per_edge
        points2d[ind, 0] = fr * msg.width
        points2d[ind, 1] = msg.height
        ind += 1

    # complete the loop with the + 1
    for i in range(num_per_edge + 1):
        fr = 1.0 - i / num_per_edge
        points2d[ind, 1] = fr * msg.height
        ind += 1

    # idealized points all at range 1.0
    ideal_points = cv2.undistortPoints(points2d, camera_matrix, dist_coeff)

    points3d_in_camera = np.ones((num_points + 1, 3))
    points3d_in_camera[:-1, 0] = ideal_points[:, 0, 0]
    points3d_in_camera[:-1, 1] = ideal_points[:, 0, 1]

    # the origin of the camera
    points3d_in_camera[-1, 0] = 0.0
    points3d_in_camera[-1, 1] = 0.0
    points3d_in_camera[-1, 2] = 0.0

    # rospy.loginfo_throttle(6.0, f"\n{points2d}")
    # rospy.loginfo_throttle(6.0, f"\n{points3d_in_camera}")

    # put numpy/cv2 points into a point cloud
    field_points = []
    for ind in range(points3d_in_camera.shape[0]):
        x = points3d_in_camera[ind, 0]
        y = points3d_in_camera[ind, 1]
        z = points3d_in_camera[ind, 2]
        pt = [x, y, z]
        field_points.append(pt)

    fields = [PointField('x', 0, PointField.FLOAT32, 1),
              PointField('y', 4, PointField.FLOAT32, 1),
              PointField('z', 8, PointField.FLOAT32, 1),
              ]

    pc2_in = point_cloud2.create_cloud(msg.header, fields, field_points)
    # pc2_in.header.stamp = msg.header.stamp
    # TODO(lucasw) probably a more efficient transform on a Point array than converting to PointCloud2
    pc2_out = do_transform_cloud(pc2_in, tfs)
    pc2_points = point_cloud2.read_points(pc2_out, field_names=("x", "y", "z"), skip_nans=True)

    marker = Marker()
    marker.header.stamp = msg.header.stamp
    marker.header.frame_id = self.target_frame
    marker.ns = "camera_info"
    marker.id = self.marker_id
    marker.type = Marker.LINE_STRIP
    marker.pose.orientation.w = 1.0
    marker.lifetime = rospy.Duration(0.0)
    marker.frame_locked = False
    marker.scale.x = 0.05
    marker.color.r = 0.7
    marker.color.g = 1.0
    marker.color.a = 1.0

    # convert from generator to list
    pc2_points = [pt for pt in pc2_points]

    # the camera origin in the target frame
    pt0 = pc2_points[-1]
    x0 = pt0[0]
    y0 = pt0[1]
    z0 = pt0[2]

    for pt1 in pc2_points[:-1]:
        x1 = pt1[0]
        y1 = pt1[1]
        z1 = pt1[2]
        # is the ray facing away from the plane, or parallel, and will never intersect?
        if z1 >= z0 and z0 >= 0.0:
            continue
        elif z1 >= z0 and z0 <= 0.0:
            continue

        scale = z0 / (z0 - z1)
        x2 = x0 + (x1 - x0) * scale
        y2 = y0 + (y1 - y0) * scale
        # this should be 0.0
        z2 = z0 + (z1 - z0) * scale

        pt = Point(x2, y2, z2)
        # pt = Point(x1, y1, z1)
        marker.points.append(pt)

    marker_array = MarkerArray()
    marker_array.markers.append(marker)
    self.marker_pub.publish(marker_array)
</code>```

Body:
I've made a ros node for this here: https://github.com/lucasw/vimjay/blob/eliminate_build_warnings/scripts/camera_info_to_plane.py (I'll get it into the main branch later)

https://youtu.be/VOY64AK3-N0

roslaunch vimjay camera_info_to_plane.launch

This is the code, using opencv undistort on an array of points that trace the borders of the image, transforms them into the frame of interest to project onto, then finds the intersection of each point with z == 0.0 on that plane.

It's a little slow, the for-loop for intersection finding is an obvious candidate for vectorization, and there may be a faster way than creating a PointCloud2 to transform a bunch of 3D points from one frame to another (if anyone knows please leave a comment- or submit a PR).  It could also cache results from previous updates if it detects nothing in the relative transform or camera info has changed, or at the least cache the border point strip if the number of points per edge hasn't changed.

def camera_info_callback(self, msg: CameraInfo):
    try:
        tfs = self.tf_buffer.lookup_transform(self.target_frame, msg.header.frame_id,
                                              msg.header.stamp, rospy.Duration(0.3))
    except (tf2_ros.LookupException, tf2_ros.ExtrapolationException) as ex:
        rospy.logwarn_throttle(4.0, ex)
        return

    camera_matrix, dist_coeff, rvec, tvec = camera_info_to_cv2(msg)

    num_per_edge = 8
    num_points = num_per_edge * 4 + 1

    points2d = np.zeros((num_points, 2))
    ind = 0
    # create points in a loop around the edge of the image
    for i in range(num_per_edge):
        fr = i / num_per_edge
        points2d[ind, 0] = fr * msg.width
        ind += 1

    for i in range(num_per_edge):
        fr = i / num_per_edge
        points2d[ind, 0] = msg.width
        points2d[ind, 1] = fr * msg.height
        ind += 1

    for i in range(num_per_edge):
        fr = 1.0 - i / num_per_edge
        points2d[ind, 0] = fr * msg.width
        points2d[ind, 1] = msg.height
        ind += 1

    # complete the loop with the + 1
    for i in range(num_per_edge + 1):
        fr = 1.0 - i / num_per_edge
        points2d[ind, 1] = fr * msg.height
        ind += 1

    # idealized points all at range 1.0
    ideal_points = cv2.undistortPoints(points2d, camera_matrix, dist_coeff)

    points3d_in_camera = np.ones((num_points + 1, 3))
    points3d_in_camera[:-1, 0] = ideal_points[:, 0, 0]
    points3d_in_camera[:-1, 1] = ideal_points[:, 0, 1]

    # the origin of the camera
    points3d_in_camera[-1, 0] = 0.0
    points3d_in_camera[-1, 1] = 0.0
    points3d_in_camera[-1, 2] = 0.0

    # rospy.loginfo_throttle(6.0, f"\n{points2d}")
    # rospy.loginfo_throttle(6.0, f"\n{points3d_in_camera}")

    # put numpy/cv2 points into a point cloud
    field_points = []
    for ind in range(points3d_in_camera.shape[0]):
        x = points3d_in_camera[ind, 0]
        y = points3d_in_camera[ind, 1]
        z = points3d_in_camera[ind, 2]
        pt = [x, y, z]
        field_points.append(pt)

    fields = [PointField('x', 0, PointField.FLOAT32, 1),
              PointField('y', 4, PointField.FLOAT32, 1),
              PointField('z', 8, PointField.FLOAT32, 1),
              ]

    pc2_in = point_cloud2.create_cloud(msg.header, fields, field_points)
    # pc2_in.header.stamp = msg.header.stamp
    # TODO(lucasw) probably a more efficient transform on a Point array than converting to PointCloud2
    pc2_out = do_transform_cloud(pc2_in, tfs)
    pc2_points = point_cloud2.read_points(pc2_out, field_names=("x", "y", "z"), skip_nans=True)

    marker = Marker()
    marker.header.stamp = msg.header.stamp
    marker.header.frame_id = self.target_frame
    marker.ns = "camera_info"
    marker.id = self.marker_id
    marker.type = Marker.LINE_STRIP
    marker.pose.orientation.w = 1.0
    marker.lifetime = rospy.Duration(0.0)
    marker.frame_locked = False
    marker.scale.x = 0.05
    marker.color.r = 0.7
    marker.color.g = 1.0
    marker.color.a = 1.0

    # convert from generator to list
    pc2_points = [pt for pt in pc2_points]

    # the camera origin in the target frame
    pt0 = pc2_points[-1]
    x0 = pt0[0]
    y0 = pt0[1]
    z0 = pt0[2]

    for pt1 in pc2_points[:-1]:
        x1 = pt1[0]
        y1 = pt1[1]
        z1 = pt1[2]
        # is the ray facing away from the plane, or parallel, and will never intersect?
        if z1 >= z0 and z0 >= 0.0:
            continue
        elif z1 >= z0 and z0 <= 0.0:
            continue

        scale = z0 / (z0 - z1)
        x2 = x0 + (x1 - x0) * scale
        y2 = y0 + (y1 - y0) * scale
        # this should be 0.0
        z2 = z0 + (z1 - z0) * scale

        pt = Point(x2, y2, z2)
        # pt = Point(x1, y1, z1)
        marker.points.append(pt)

    marker_array = MarkerArray()
    marker_array.markers.append(marker)
    self.marker_pub.publish(marker_array)


问:
Why does joint links pile up to origin after running the control launch file?
This is the spherical bot which I am working on from the last question. everything seems to run properly while launching the gazebo and rviz launch files. but the moment I launch the control launch file, the joints and links pile up at the origin. What could be the reason?

Also I am not able to control the joint after launching that control launch file

PLEASE LET ME KNOW IF THERE IS ANY MISTAKE FROM MY SIDE OR IF YOU NEED MORE INFORMATION.

Thanks for your time :)

Here's the gazebo with plugin file

```<code><robot
  name="spb">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="-0.00028921 0 0"
        rpy="0 0 0" />
      <mass
        value="0.38748" />
      <!-- <inertia
        ixx="2.1656E-05"
        ixy="0"
        ixz="0"
        iyy="4.4343E-05"
        iyz="-0"
        izz="2.4037E-05" /> -->
        <inertia
        ixx="3.94176E-05"
        ixy="0"
        ixz="0"
        iyy="3.94176E-05"
        iyz="-0"
        izz="8.09748E-05" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://spb/meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://spb/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="hull_link">
    <inertial>
      <origin
        xyz="0 0 -0.03481"
        rpy="0 0 0" />
      <mass
        value="0.15035" />
      <!-- <inertia
        ixx="3.1656E-05"
        ixy="0"
        ixz="0"
        iyy="5.0531E-05"
        iyz="0"
        izz="3.188E-05" /> -->
        <inertia
        ixx="2.506E-02"
        ixy="0"
        ixz="0"
        iyy="2.506E-02"
        iyz="0"
        izz="2.506E-02" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 -0.03481"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://spb/meshes/hull_link.STL" />
      </geometry>
      <material
        name="brick_red_mf">
        <color
          rgba="0.7 0.0 0.1 1.0" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 -0.03481"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://spb/meshes/hull_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint1"
    type="continuous">
    <origin
      xyz="0.03481 0.0 0"
      rpy="-1.5708 0.011859 -1.5708" />
    <parent
      link="base_link" />
    <child
      link="hull_link" />
    <axis
      xyz="0 0 1" />
  </joint>
  <!--                GAZEBO RELATED PART                             -->
  <!-- ROS Control plugin for Gazebo -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/spb</robotNamespace>
    </plugin>
  </gazebo>

  <!-- transmission -->
  <transmission name="transmis1">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="joint1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>

</robot>```

HERE'S THE RVIZ LAUNCH FILE

``` <launch>
  <arg
    name="model" />
  <param
    name="robot_description"
    textfile="$(find spb)/urdf/spb.urdf" />
  <node
    name="joint_state_publisher_gui"
    pkg="joint_state_publisher_gui"
    type="joint_state_publisher_gui" />
  <node
    name="robot_state_publisher"
    pkg="robot_state_publisher"
    type="robot_state_publisher" />
  <node
    name="rviz"
    pkg="rviz"
    type="rviz"
    args="-d $(find spb)/urdf.rviz" />
</launch> ```

HERE'S CONTROL LAUNCH FILE

``` <launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find spb_control)/config/spb_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/spb" args="joint_state_controller
                      joint1_position_controller"/>
                      <!-- joint2_position_controller"/> -->

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/spb/joint_states" />
  </node>

</launch>

HERE'S THE CONFIGURATION FILE OF THE CONTROLLER

```spb:
  # 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: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  # joint2_position_controller:
  #   type: effort_controllers/JointPositionController
  #   joint: joint2
  #   pid: {p: 100.0, i: 0.01, d: 10.0}
```

答: It's really difficult to diagnose the issue without the full URDF/SDF of your world, but my guess is that the joint controller is somehow putting the world in an invalid state by applying large joint forces. I would first disable/remove the controller and see if you still have the issue. If the issue goes away, you probably have a link with invalid or unrealistic inertial properties. You can visualize inertia properties for each link to help with debugging this.</p>