问:
Velocity control with position setpoints
I'm trying to control the toyota HSR with a higher level whole-body controller that computes velocities, but the robot only offers JointPositionInterfaces in it's ros_control HardwareInterface implementation.

As none of the standard ROS controllers accept a velocity and output a position my approach so far included to write an integrator as an ROS controller plugin that continuously integrates the velocity command into an position set point.

This works more or less, but some of the joints react with shaky movements to this, and none of the joints reach the desired velocity.

I would assume that i have to tune the integrator gain for each joint to track the desired velocities, but i'm wondering if there exist other methods to achieve this?

As the robot offers position and velocity feedback i was thinking about using the velocity error to improve the position set point calculation, but somehow do not think that a classical PID loop will work. Has anyone encountered a similar situation or knows some literature on that topic?

答:
As the robot offers position and velocity feedback i was thinking about using the velocity error to improve the position set point calculation, but somehow do not think that a classical PID loop will work.

Why do you think a PID won't help you out here?

I would try it, actually:

$$
q_c = \frac{\dot{q}_d}{s} + \text{PID} \left( \dot{q}_d - \dot{q} \right),
$$

where $q_c$ is the position command, $\dot{q}_d$ is the desired velocity provided by the whole-body controller and $\dot{q}_d-\dot{q}$ is the velocity error, with $\dot{q}$ being the velocity feedback.

It's easy to recognize the feed-forward and the feedback terms in the law. $\text{PID}(\cdot)$ can be a simple integrator to begin with.

The control performance will depend on:

the quality of $\dot{q}_d$, hence of the whole-body controller.

the quality of $\dot{q}$ (you may think of replacing the standard velocity feedback offered by the system with a more accurate ad-hoc observer).

Assuming that the whole-body controller is, well, a controller and not a planner, you'll end up with two nested closed loops, the inner and the outer loop. The property of closed-loop systems to compensate for unmodelled quantities and impairments will help you track the desired velocity quite well.

Make sure that the inner loop (i.e., the feed-forward+PID) can run faster and be more reactive than the outer loop (i.e., the whole-body controller).


问:
what does free or forced torque mean in robot arm dynamics?
I'm trying to understand the dynamics (torque , velocity, etc )  of how a simple 2 DoF robotic arm rotates.
I am using the RoboAnalyzer simulator 1 .

It has a Inverse Dynamics setting of "Free" or "Forced" Torque.
You select "Free" or "Forced".

What doe that mean ?
What gets changed if that setting is changed ?

Many thanks

答:
Free torque is a torque that is applied to a joint without any external load or resistance. This type of torque is used to model the behavior of a joint that is moving freely, such as an arm swinging back and forth with no external forces acting upon it. In a robot simulation, free torque may be used to test the limits of a joint's range of motion or to evaluate the performance of a control algorithm in the absence of external disturbances.

Forced torque, on the other hand, is a torque that is applied to a joint in response to an external load or disturbance. This type of torque is used to model the behavior of a joint that is interacting with its environment, such as a robot arm lifting a heavy object or pushing against an obstacle. In a robot simulation, forced torque may be used to test the ability of a robot to handle different types of loads or to evaluate the effectiveness of a control algorithm in the presence of external disturbances.

ContentLicense: CC BY-SA 4.0


问:
How to set the rate of a node that subscribes to a topic in ROS2 Foxy
I have made a node to run some calculations using data from another topic (odom in this case) and publish them to another topic.
How do I set the rate of this node to be slower than the subscribed topic?

Here is what I have but I can't get it to work:

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

import rclpy
from rclpy.node import Node
import traceback

from std_msgs.msg import Float64MultiArray
from nav_msgs.msg import Odometry

class CalculatedNode(Node):

    def __init__(self):
        super().__init__('calculated_node')
        # Create publisher
        self.calculated_node = self.create_publisher(Float64MultiArray, '/calculated_stuff', 10)

        # Subscribe to node
        self.subscription = self.create_subscription(
            Odometry,
            '/odom',
            self.update_calculations,
            1)

    def update_calculations(self, msg):
        try:
            # DO SOME CALCULATIONS WITH MSG DATA
            # [...]
            # Publish Odom
            self.calculated_node.publish(Float64MultiArray())
        except Exception as e:
            traceback.print_exc()
            print(self.get_clock().now())
            # print(e)

def main(args=None):
    rclpy.init(args=args)
    calculated_node = CalculatedNode()

    try:
        rate = calculated_node.create_rate(2)
        rclpy.spin(calculated_node)

        while rclpy.ok:
            rate.sleep()
    except KeyboardInterrupt:
        pass

    calculated_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
</code>```

This was simpler than I though. Here is an example using the chatter tutorial:

```<code>#!/usr/bin/env python3
import rclpy
from rclpy.node import Node

from std_msgs.msg import String

class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.msg_store = None

        self.subscription = self.create_subscription(
            String,
            'topic',
            self.listener_callback,
            1)
        self.subscription  # prevent unused variable warning

        self.create_timer(2, self.timed_call)

    def listener_callback(self, msg):
        self.msg_store = msg

    def timed_call(self):
        self.get_logger().info('I heard: "%s"' % self.msg_store.data)

def main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = MinimalSubscriber()
    rclpy.spin(minimal_subscriber)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
</code>```

This however has an issue. The timed callback "timed_call" gets called every other trigger event.
This is the output:

```<code>[INFO] [1683201706.818698434] [minimal_subscriber]: I heard: "Hello World: 6"
[INFO] [1683201708.805167207] [minimal_subscriber]: I heard: "Hello World: 10"
[INFO] [1683201710.804572917] [minimal_subscriber]: I heard: "Hello World: 14"
[INFO] [1683201712.804249021] [minimal_subscriber]: I heard: "Hello World: 18"
</code>```

答:
I recommend using a timer to trigger a callback at the frequency you want to publish. And in that callback send the output message based on stored data.

In the subscribers callback you should do any necessary computation and then save the resulting data for the timer callback to access.


问:
Is it possible to import a .dae mesh file, with its materials, into a webots world?
I don't have an example right now (not at my computer), but I'm hoping to be able to import a world designed, modelled, and textured in Blender (poly count isn't an issue).

I can import the model as a Solid -> Shape -> Mesh -> .dae file. Then I have to add an Appearance node to give it any kind of shading. But the .dae file already has colours and textures (as appearing in Blender 3D), but I haven't seen it show up in Webots.

Am I maybe missing a step? I'm after even simple colours so the entire environment isn't a single colour.

答:
You should use a CadShape instead of a Mesh, so that both the mesh and the material are taken from the Collada (.dae file): Solid -> CadShape -> .dae file. You should also ensure that the textures referred to by the Collada file can be found relatively to the Collada file (which should be normally the case if you export the Collada file from Blender directly inside your local protos/meshes folder). If that's not the case, you can open the Collada file in a text editor and fix the paths to the textures.

Disclaimer: I am a Webots developer working at Cyberbotics.


问:
Cannot solve extrinsic matrix between camera and robot
I am triying to calibrate a RGB-D Realsense camera , with an eye in hand configuration. I have the following set-up:

I know the transformation  matrix between base/ef and cam/obj. I want to solve transformation between ef/cam.

I am using the following method:

Where AX=XB. I have those questions:

To solve transformation between base/ef, I have the cartesian pose of the robot (x,y,z, Rx,Ry,Rz). Those rotations are euler angles? If there are not, how coul I convert those rotations to a rotation  matrix?

If I have A and B matrix, which way should I use to solve X matrix?

I have used cv2.calibrateHandEye method in python and it doesn gave me the expected solution so I want to solve with AX=XB. I don not have the robot in ROS.

答:
Edit: I was mistaken about the specific question you asked. As asked, your question is not fully determined.

You need to know the base/obj relationship as well for this to be fully determined, otherwise there are an infinite number of possible transforms.

Assuming that the base is at the origin of your world space, and the object is at transform "O" in world space, and C is cam/obj, and E is base/ef, then we know:

E = X * (C-1) * O

Then in turn

E * (O-1) * C = X


问:
Quadrocopter first build: how to tell if components play well together?
I'm building my first quadrocopter, and I'm trying to come up with a parts list that is suitable for a first build.

I will use this to learn how to fly a quadrocopter manually (lots of crashes!), and to do some experiments with running AI for piloting it.

A couple questions about the below list of parts:

Is this a good choice for a first build? Are we missing any crucial parts?

Do these components work together? Is this battery strong enough to fuel all the components that need power?

Here's the current list of parts:

Frame - 450 mm

Propellers - 10x4.5", two pairs

Motor (4x) - 900kv brushless outrunner motor; max current: 18A; ESC: 25-30A; cell count:3s-4s lipoly

Electronic speed controllers (4x) - 20A constant current, 25A burst current; battery: 2-4S lipoly

Battery - 3300mAh lipoly, 11.1v, 3 cell; constant discharge: 30C, peak discharge: 40C. charge plug: JST-XH. 3300 mAh x 30C = 99 amps?

Charger - lipoly, 50W, 6A, 12v power supply

Power supply - input: AC 100-240v 50/60Hz; output: DC15v 5A

Arduino board

Gyroscope for arduino

Accelerometer for arduino

GPS sensor for arduino?

RC transmitter for arduino

RC controller

答:
Are you trying to learn about electronics, flying quadcopters, or both? If it's just flying, then I would consider buying one of the little models in a wireframe cage for practicing flying. If it's both, then here are a couple of things to consider:

There are "pusher" and "puller" rotors. You will need two of each.

Don't make your own flight controller. Buy an already-made one. They are complex, and do a lot. And you can get a great one for $50 or less.

Voltage levels matter. You'll want most things regulated to 3.3 or 5 volts. A good flight controller will do this for you.

Buy an air frame with ESCs and motors already attached. While you don't have to do this, it's easier and there will be less things that can go wrong.

(Shameless self promotion alert) You can take a look at my build blog  for some step-by-step guidance. I'm not completely done yet, but I'm getting there.

You will want a telemetry radio. They are sometimes called 3DR (after the company that builds the standard one). Take a look at my parts list and you'll see links to it.

If you use an ArduPilot or MegaPirateNG (my recommendation) compatible flight controller, then you can use a wonderful free program called Mission Planner. I highly recommend it.

If you use one of the flight controllers I spoke about in item 7, your firmware is already written for you. Just download, set your board style, build, and upload to your AVR. It's wonderful.

ContentLicense: CC BY-SA 3.0


问:
URScript - how to get the actual TCP pose in tool space coordinate system?
I am writing a simple script for universal robots UR10 robotic arm.

I am trying to find a way to to see if the tool current position is outside of a 3D bounding box. the 3D bounding box is defined in the coordinate system of the tool. (y is up, x right, z backwards).

The get_actual_tcp_pose() function returns the tool pose in the 'base frame' coordinate system. it is my understanding that this is the coordinate system where the origin is at the base of the robotic arm.

So my questions are:

How do I get the actual tcp pose in the TCP coordinate system (tool space)?

Alternatively, how do I convert the pose returned by get_actual_tcp_pose() to the cooridnate system of the TCP using URScript?

答: If you know the translation vector of TCP between base (0,0,0), and you know the position of the box respect the base, you can be able to calculate if TCP is outside(inside the box by knowing the transformation matrix between base and TCP.


问:
Could not initialize robot simulation interface
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

答: Try to write your robot name inside gazebo plugin in robotNamespace</p>


问:
BNO055 mounted orientation configuration

For this BNO055, I am trying to figure out the correct transpose and sign tuples (or the byte values) for reorienting the sensor so that it will tell me the heading "forward" (from the perspective of this photographer), pitch forward/backward (+- don’t care which), and roll left/right (+- don’t care which).  That is, the board is mounted on the parked vehicle such that the plane of the breakout board is perpendicular to the ground.

I have read the chip's datasheet (near page 25, though it doesn't look to me like it should be any of those examples) and tried to study the orientation handedness to choose correct values of transpose and sign, but for whatever reason I cannot figure it out.  Here I think I'm trading -Z for X, X for Z.  After calibration, the heading is wrong and pitch and roll sensitivity are swapped.

```<code>imu = BNO055(i2c, transpose=(2, 1, 0), sign=(1, 0, 0))
# imu.set_offsets(bytearray(ubinascii.unhexlify('e1ff0400ebff4bfe1d0232ff0000feff0000e803a702')))
</code>```

What are the correct transpose and sign tuples for this mounting orientation?

Are the sensor offsets/calibration that I extract and reuse relative to the raw, untransposed sensors or are those adjustments applied after the transposition?  I’d guess raw, but any time I change the transpose & sign values, the existing calibration seems bad because the chip stays in calibration mode for much longer than when I reuse offsets from the same orientation settings.

Update

The bno055 quickstart guide has extra info on the heading interpretation which I find helpful:

https://www.bosch-sensortec.com/media/boschsensortec/downloads/application_notes_1/bst-bno055-an007.pdf

答:
By my best reading, this doesn't match the documentation, but this seems to give me good heading, roll, pitch values for my mount orientation.

imu = BNO055(i2c, transpose=(1, 2, 0), sign=(1, 1, 0))
```

and I also believe that the calibration must be done specifically for each transpose/sign reconfiguration.  I hope this helps someone.

Body:
By my best reading, this doesn't match the documentation, but this seems to give me good heading, roll, pitch values for my mount orientation.

imu = BNO055(i2c, transpose=(1, 2, 0), sign=(1, 1, 0))

and I also believe that the calibration must be done specifically for each transpose/sign reconfiguration.  I hope this helps someone.


问:
DH parameters of a serial manipulator RRRRRR
I am trying to find the forward kinematics of this manipulator in the picture using DH notation. All of the joints are rotational.

I managed to get to the third joint. The parameter table is below.

Joint
Theta
d
a
alpha

1
q_1
d_1
0
pi/2

2
q_2
0
d_2
0

3
q_3
0
d_3

4

5

6

I am not sure what the alpha for joint 3 should be. Any help would be appreciated.

答:
I would suggest watching Robotics 1 U1 (Kinematics) S5 (HTM) P2 (HTM by Denavit Hartenberg), to learn how to find DH-parameter table.

Alpha values can be found out by the rotation of the first frame to the second frame's x-axis to match the z-axis with second frame. Therefore, here the alpha value for joint-3 would be pi/2 when we rotate frame 3 around frame 4's x-axis to match the z-axis.


问:
Drifting issue in ROS2 using SLAM-Toolbox
I'm slightly new to robotics and I'm trying to prototype using ROS2-Galactic, I am using built-in packages such as Slam Toolbox and others.
Now that I'm trying to run it, I'm having some issues with the created map as well in tf perhaps. I don't know how to point out where I am getting the problem, I am completely noob to debugging this stuff. the scenario is, I'm trying to do SLAM and I put the robot in-place without doing nothing, but it seems that the robot is rotating clockwise by its own as shown in rviz.
I hope someone is interested to help me, you can check clip here,
https://youtu.be/UVhMtrMc2ZU</p>

答:
Beyond the fact that the robot's not moving, so I can't know if there are other TF issues, the most obvious problem I see is that you're not using a laser scanner. This is a 2D laser scanner SLAM method. It needs 2D laser scanner like angular data coverage. You're not going to get away with RGBD sensors unless you integrated 2-3 of them to get a reasonable angular band to scan match against.

If you have an RGBD sensor, there are specialized RGB-D SLAM's or visual SLAMs that exist that you should utilize. We're not in 2010 anymore where there aren't many or good options for RGBD cameras.


问:
Odometry into EKF for localization
I want to fuse a Lidar-Odometry into my EKF where I'm already using encoder, IMU, GPS. The odometry has as output a delta pose, how to put that into the filter? How to deal with the covariances?</p>

答:
The best paper on this topic is SC-KF Mobile Robot Localization: A Stochastic
Cloning-Kalman Filter for Processing Relative-State
Measurements.

In the related approaches section it explains some of the other ways you can deal with it, and then proceeds to explain what it considers to be the best way. I highly recommend you read it. It is a quite important paper, though I'll summarize the different approaches.

The different methods are:

Treat your LIDAR measurements like a pseudo GPS/ absolute pose measuring device.

At every LIDAR measurement you compute the sum of all the previous deltas to give you a position. You then use that position just as if you were to use a GPS sensor.

The covariance is easy since it is just summing your delta pose covariances. Though it basically becomes useless since it just keeps on growing. However, you can reset it whenever you get a GPS measurement.

Convert your delta pose measurements to an average velocity.

Given your delta pose $T_{xy}$ you know the time delta between Pose $X$ and Pose $Y$ and can convert it to velocity. The velocity should be in your state vector, and can now be updated in your Lidar measurement step.

This is probably the most common method people use, since it is the easiest to implement, and doesn't have all the correlation problems the above approach might have.

For covariance you just have to convert your pose covariances to velocity covariances.

Use stochastic cloning as described in the paper.

Basically given a delta pose $T_{xy}$ you have a copy of your state vector at Pose $X$ and Pose $Y$. You can now apply the delta pose directly on your state vector since you have the expected observation(the delta between your Pose $X$ and Pose $Y$ copies), and your measurement the Lidar delta pose.

Is the best method since it properly handles all of the correlations/covariances, and doesn't make assumptions like the average velocity approach.

This is a rough explanation. I recommend reading the paper, for a better one.

Title: Odometry into EKF for localization


答:
I have had good luck with using the lidar for the first two lines, and using wheel odometry for the second pair of lines.

Lidar: t, t, f, f, f, t (position)

wheel: t, t, f, f, f, t (velocity)

This is for a robot that runs on flat ground. (2d movement)

Title: Odometry into EKF for localization

Body: I want to fuse a Lidar-Odometry into my EKF where I'm already using encoder, IMU, GPS. The odometry has as output a delta pose, how to put that into the filter? How to deal with the covariances?</p>


问:
How does BA work for 3d lidar? Particularly VLP16/VLP32 aren't dense enough
How do you run bundle adjustment on a sequence of 3d lidar recordings if the scan is relatively sparse? I read something about attempting to match up planes/edges but there wasn't much detail given. I think NDT and ICP can't work due to the sparseness of the scan.</p>

答: These lidar units can be sped up. That will give more detail in the output cloud. VLP16 can do from 300 to 1200 rpm, I believe.</p>


问:
Red [ERROR] output in python in ROS
In ROS, I cannot get [ERROR] logs to print in red when I use python. How can I make them appear in red instead of black?

For example, the following Python:

```<code>rospy.logerr("No analog input received.")
</code>```

produces this output in black:

[ERROR] [WallTime: 135601422.876123] No analog input received.

whereas the following C++:

```<code>ROS_ERROR("No analog input received.");
</code>```

produces the following output in red.

[ERROR] [135601551.192412]: No analog input received.

答:
Please see this bug ticket: https://code.ros.org/trac/ros/ticket/3691

I filed this bug report (with patch to fix) 13 months ago and am pretty disappointed in the ROS team that they have not put this in trunk.

Title: Red [ERROR] output in python in ROS


问:
Nav2-ROS2 4 wheels robot unable to reach goal
I would like to work with a 4 wheels robot (picture below) but as far as I tried it never worked. I am new to Nav2 so I first tried the turtlebot3 which worked fluently but when I moved to a "custom" two wheels and then "custom" 4 wheels it does not reach the goal anymore.

The navigation stack is running without any issues but the robot does not drive to the goal and miss it. As showed on the picture below the coordinate in the purple line is the goal, the purple line is the planned path, red dots are the taken path by the robot, the coordinate in the middle of the red dots heap is the starting point of the robot and the coordinate in the middle of the purple circle is the current position of the robot when the screenshot was taken.

As seen, the planned path looks correct but for some reason the robot is unable to reach the goal nor follow the path (at the starting point the planned path was a direct line from start to goal).

Another try:

Picture of the 2 wheels robot:

Picture of the 4 wheels robot :

The simulation software is called Isaac Sim (from Nvidia Omniverse) and the turtlebot3 worked during the simulation. Of course the two other simulated robots could have been misconceived but the real robot (which looks like the 4 wheels robot) does the same random pattern as the simulated one.

My Nav2 parameter file:

```<code>amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "odom"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: odom
    robot_base_frame: base_link
    odom_topic: /odom
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "turtlebot3_world.yaml"

map_saver:
  ros__parameters:
    use_sim_time: True
    save_map_timeout: 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: False

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "back_up", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    back_up:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: True
</code>```

ROS 2 version: Foxy and Humble (I have tried both)

答:
You may like to work with the code in Linorobot2. You can base your robot on this. There is simulation and actual robot routines. It works very well.
The repo is on Github.</p>


问:
ROS 2 tutorial test : Failing System generated tests?
I was trying to do the beginner level testing of ROS 2 packages by following the official ROS 2 Documentation, Writing Basic Tests with C++ with GTest, and when I tried to build the package and run the dummy test I am getting the following errors,

details about the package I created :
package name : test_package

command I tried to run

colcon build --packages-select test_package
```
<p>The package got successfully build. and then when I tried the next command given bellow I am getting errors</p>
<p><strong>COMMAND :</strong>  <code>colcon test --packages-select test_package --ctest-args tests</code></p>
<p><strong>RESULT</strong></p>
<pre class="lang-bash prettyprint-override"><code>jishnu@Legion:~/testros/workspace$ colcon test --packages-select test_package --ctest-args tests
Starting >>> test_package
--- stderr: test_package
Errors while running CTest
Output from these tests are in: /home/jishnu/testros/workspace/build/test_package/Testing/Temporary/LastTest.log
Use "--rerun-failed --output-on-failure" to re-run the failed cases verbosely.
---
Finished <<< test_package [1.41s]       [ with test failures ]

Summary: 1 package finished [1.59s]
  1 package had stderr output: test_package
  1 package had test failures: test_package
</code>```

COMMAND : colcon test --packages-select test_package --event-handlers console_direct+

RESULT

jishnu@Legion:~/testros/workspace$ colcon test --packages-select test_package --event-handlers console_direct+
Starting >>> test_package
UpdateCTestConfiguration  from :/home/jishnu/testros/workspace/build/test_package/CTestConfiguration.ini
Parse Config file:/home/jishnu/testros/workspace/build/test_package/CTestConfiguration.ini
   Site: Legion
   Build name: (empty)
 Add coverage exclude regular expressions.
Create new tag: 20230425-1728 - Experimental
UpdateCTestConfiguration  from :/home/jishnu/testros/workspace/build/test_package/CTestConfiguration.ini
Parse Config file:/home/jishnu/testros/workspace/build/test_package/CTestConfiguration.ini
Test project /home/jishnu/testros/workspace/build/test_package
Constructing a list of tests
Done constructing a list of tests
Updating test list for fixtures
Added 0 tests to meet fixture requirements
Checking test dependency graph...
Checking test dependency graph end
test 1
    Start 1: test_package_tutorial_test

1: Test command: /usr/bin/python3.10 "-u" "/opt/ros/humble/share/ament_cmake_test/cmake/run_test.py" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/test_package_tutorial_test.gtest.xml" "--package-name" "test_package" "--output-file" "/home/jishnu/testros/workspace/build/test_package/ament_cmake_gtest/test_package_tutorial_test.txt" "--command" "/home/jishnu/testros/workspace/build/test_package/test_package_tutorial_test" "--gtest_output=xml:/home/jishnu/testros/workspace/build/test_package/test_results/test_package/test_package_tutorial_test.gtest.xml"
1: Test timeout computed to be: 60
1: -- run_test.py: invoking following command in '/home/jishnu/testros/workspace/build/test_package':
1:  - /home/jishnu/testros/workspace/build/test_package/test_package_tutorial_test --gtest_output=xml:/home/jishnu/testros/workspace/build/test_package/test_results/test_package/test_package_tutorial_test.gtest.xml
1: [==========] Running 1 test from 1 test suite.
1: [----------] Global test environment set-up.
1: [----------] 1 test from package_name
1: [ RUN      ] package_name.a_first_test
1: [       OK ] package_name.a_first_test (0 ms)
1: [----------] 1 test from package_name (0 ms total)
1:
1: [----------] Global test environment tear-down
1: [==========] 1 test from 1 test suite ran. (1 ms total)
1: [  PASSED  ] 1 test.
1: -- run_test.py: return code 0
1: -- run_test.py: inject classname prefix into gtest result file '/home/jishnu/testros/workspace/build/test_package/test_results/test_package/test_package_tutorial_test.gtest.xml'
1: -- run_test.py: verify result file '/home/jishnu/testros/workspace/build/test_package/test_results/test_package/test_package_tutorial_test.gtest.xml'
1/5 Test #1: test_package_tutorial_test .......   Passed    0.09 sec
test 2
    Start 2: cppcheck

2: Test command: /usr/bin/python3.10 "-u" "/opt/ros/humble/share/ament_cmake_test/cmake/run_test.py" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/cppcheck.xunit.xml" "--package-name" "test_package" "--output-file" "/home/jishnu/testros/workspace/build/test_package/ament_cppcheck/cppcheck.txt" "--command" "/opt/ros/humble/bin/ament_cppcheck" "--xunit-file" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/cppcheck.xunit.xml" "--include_dirs" "/home/jishnu/testros/workspace/test_package/include"
2: Test timeout computed to be: 300
2: -- run_test.py: invoking following command in '/home/jishnu/testros/workspace/test_package':
2:  - /opt/ros/humble/bin/ament_cppcheck --xunit-file /home/jishnu/testros/workspace/build/test_package/test_results/test_package/cppcheck.xunit.xml --include_dirs /home/jishnu/testros/workspace/test_package/include
2: cppcheck 2.7 has known performance issues and therefore will not be used, set the AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS environment variable to override this.
2: -- run_test.py: return code 0
2: -- run_test.py: verify result file '/home/jishnu/testros/workspace/build/test_package/test_results/test_package/cppcheck.xunit.xml'
2/5 Test #2: cppcheck .........................   Passed    0.23 sec
test 3
    Start 3: lint_cmake

3: Test command: /usr/bin/python3.10 "-u" "/opt/ros/humble/share/ament_cmake_test/cmake/run_test.py" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/lint_cmake.xunit.xml" "--package-name" "test_package" "--output-file" "/home/jishnu/testros/workspace/build/test_package/ament_lint_cmake/lint_cmake.txt" "--command" "/opt/ros/humble/bin/ament_lint_cmake" "--xunit-file" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/lint_cmake.xunit.xml"
3: Test timeout computed to be: 60
3: -- run_test.py: invoking following command in '/home/jishnu/testros/workspace/test_package':
3:  - /opt/ros/humble/bin/ament_lint_cmake --xunit-file /home/jishnu/testros/workspace/build/test_package/test_results/test_package/lint_cmake.xunit.xml
3:
3: No problems found
3: -- run_test.py: return code 0
3: -- run_test.py: verify result file '/home/jishnu/testros/workspace/build/test_package/test_results/test_package/lint_cmake.xunit.xml'
3/5 Test #3: lint_cmake .......................   Passed    0.20 sec
test 4
    Start 4: uncrustify

4: Test command: /usr/bin/python3.10 "-u" "/opt/ros/humble/share/ament_cmake_test/cmake/run_test.py" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/uncrustify.xunit.xml" "--package-name" "test_package" "--output-file" "/home/jishnu/testros/workspace/build/test_package/ament_uncrustify/uncrustify.txt" "--command" "/opt/ros/humble/bin/ament_uncrustify" "--xunit-file" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/uncrustify.xunit.xml"
4: Test timeout computed to be: 60
4: -- run_test.py: invoking following command in '/home/jishnu/testros/workspace/test_package':
4:  - /opt/ros/humble/bin/ament_uncrustify --xunit-file /home/jishnu/testros/workspace/build/test_package/test_results/test_package/uncrustify.xunit.xml
4: Code style divergence in file 'src/SystemTests.cpp':
4:
4: --- src/SystemTests.cpp
4: +++ src/SystemTests.cpp.uncrustify
4: @@ -5 +5 @@
4: -    ASSERT_EQ(4, 2 + 2);
4: +  ASSERT_EQ(4, 2 + 2);
4: @@ -8 +8 @@
4: -int main(int argc, char **argv)
4: +int main(int argc, char ** argv)
4: @@ -10,3 +10,3 @@
4: -    testing::InitGoogleTest(&argc, argv);
4: -    return RUN_ALL_TESTS();
4: -}
4: +  testing::InitGoogleTest(&argc, argv);
4: +  return RUN_ALL_TESTS();
4: +}
4:
4: 1 files with code style divergence
4: -- run_test.py: return code 1
4: -- run_test.py: verify result file '/home/jishnu/testros/workspace/build/test_package/test_results/test_package/uncrustify.xunit.xml'
4/5 Test #4: uncrustify .......................***Failed    0.27 sec
test 5
    Start 5: xmllint

5: Test command: /usr/bin/python3.10 "-u" "/opt/ros/humble/share/ament_cmake_test/cmake/run_test.py" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/xmllint.xunit.xml" "--package-name" "test_package" "--output-file" "/home/jishnu/testros/workspace/build/test_package/ament_xmllint/xmllint.txt" "--command" "/opt/ros/humble/bin/ament_xmllint" "--xunit-file" "/home/jishnu/testros/workspace/build/test_package/test_results/test_package/xmllint.xunit.xml"
5: Test timeout computed to be: 60
5: -- run_test.py: invoking following command in '/home/jishnu/testros/workspace/test_package':
5:  - /opt/ros/humble/bin/ament_xmllint --xunit-file /home/jishnu/testros/workspace/build/test_package/test_results/test_package/xmllint.xunit.xml
5: File 'package.xml' is valid
5:
5: No problems found
5: -- run_test.py: return code 0
5: -- run_test.py: verify result file '/home/jishnu/testros/workspace/build/test_package/test_results/test_package/xmllint.xunit.xml'
5/5 Test #5: xmllint ..........................   Passed    1.60 sec

80% tests passed, 1 tests failed out of 5

Label Time Summary:
cppcheck      =   0.23 sec*proc (1 test)
gtest         =   0.09 sec*proc (1 test)
lint_cmake    =   0.20 sec*proc (1 test)
linter        =   2.30 sec*proc (4 tests)
uncrustify    =   0.27 sec*proc (1 test)
xmllint       =   1.60 sec*proc (1 test)

Total Test time (real) =   2.42 sec

The following tests FAILED:
          4 - uncrustify (Failed)
Errors while running CTest
Output from these tests are in: /home/jishnu/testros/workspace/build/test_package/Testing/Temporary/LastTest.log
Use "--rerun-failed --output-on-failure" to re-run the failed cases verbosely.
--- stderr: test_package
Errors while running CTest
Output from these tests are in: /home/jishnu/testros/workspace/build/test_package/Testing/Temporary/LastTest.log
Use "--rerun-failed --output-on-failure" to re-run the failed cases verbosely.
---
Finished <<< test_package [5.11s]       [ with test failures ]

Summary: 1 package finished [5.29s]
  1 package had stderr output: test_package
  1 package had test failures: test_package
```

Is there a way to by pass this system generated test and only conduct the custom tests provided in the test file?

答:
I have found out the solution for this, the system tests are coming from the automatically generated boiler plate code during ros2 pkg create used to create a ROS 2 package.

Below given is the BUILD_TESTING part of the boiler plate code

```<code>if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()
</code>```

In the above code the last two lines, are responsible for including the cpp_lint tests, and commenting it out solved my issue.

```<code>set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
</code>```

I still wonder, why the error still prevailed even after formatting the problematic code according to the code formats.

Body:
I have found out the solution for this, the system tests are coming from the automatically generated boiler plate code during ros2 pkg create used to create a ROS 2 package.

Below given is the BUILD_TESTING part of the boiler plate code

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

In the above code the last two lines, are responsible for including the cpp_lint tests, and commenting it out solved my issue.

set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()

I still wonder, why the error still prevailed even after formatting the problematic code according to the code formats.


问:
How do I calculate reference trajectory for a robot following a parabolic path at a constant speed?
I have a 3 DOF robot arm and have been tasked with making it follow a path of $y=6-x^2$. Normally to compute the reference trajectory, I would simply create a linearly spaced vector of $x$ values and then compute a vector of $y$ values using the given equation. However, for this task, I need the reference trajectory to have a constant speed. Our textbook has given us this hint, but so far, I have been unable to do anything about it.

How to use the Leibnitz rule to solve for reference trajectory:

Calculate the length of an arc of the parabola at a generic point whose horizontal position is $x(t)$;

Differentiate the length of the arc of the parabola at time $t$ to deduce the translational velocity along the arc. This velocity is constant and equal to $V$. Thus, you can solve for $\dot{x}(t)$.

Once you have $\dot{x}(t)$, you can deduce the reference position and velocity in $x$ and $y$.

This advice confuses me, mostly because I do not understand how an arc can have a length at one specific point.

Thank you for any help you can give me with this problem.

答:
The arc length can by calculated by the following formula(section: finding arc length by integration):
$$ s=\int_a^b \sqrt{1+ \left(\frac{dy}{dx}\right)^2} dx $$

So each segment in your trajectory has a length that can be calculated from the equation above.


答:
If the norm of the velocity shall remain constant along the arc of the parabola, then it holds $\sqrt{\dot{x}^2+\dot{y}^2}=V$.

Plugging $\dot{y}=-2x\cdot\dot{x}$ into the equation above, we end up with the nonlinear differential equation:

$$
\dot{x}\sqrt{1+4x^2}=V.
$$

It is not possible to solve the system symbolically, but we can integrate it numerically by relying for example on the model below:

The simulation provides us with the trajectories $x\left(t\right)$, $y\left(t\right)$, $\dot{x}\left(t\right)$, $\dot{y}\left(t\right)$.

The following snippet plots the data (obtained for $V=0.1$ m/s):

% simulate the model
mdl = 'model';
load_system(mdl);
cs = getActiveConfigSet(mdl);
simOut = sim(mdl, cs);

x = find(simOut.yout, 'x');
y = find(simOut.yout, 'y');
xdot = find(simOut.yout, 'xdot');
ydot = find(simOut.yout, 'ydot');

mdlWks = get_param(mdl, 'ModelWorkspace');
V = getVariable(mdlWks, 'V');

% plot the trajectory
figure('color', 'white');
tiledlayout(3, 2);

nexttile([3 1]);
stairs(x.Values.Data, y.Values.Data);
xlabel('x [m]');
ylabel('y [m]');
grid('minor');

nexttile;
hold('on');
stairs(x.Values.Time, x.Values.Data);
stairs(y.Values.Time, y.Values.Data);
xlabel('time [s]');
ylabel('[m]');
legend({'$x$' '$y$'}, 'Interpreter', 'latex');
grid('minor');

nexttile;
hold('on');
stairs(xdot.Values.Time, xdot.Values.Data);
stairs(ydot.Values.Time, ydot.Values.Data);
xlabel('time [s]');
ylabel('[m/s]');
legend({'$\dot{x}$' '$\dot{y}$'}, 'Interpreter', 'latex');
grid('minor');

% verify the constant speed along the arc
nexttile;
hold('on');
yline(V.Value);
vel = sqrt(xdot.Values.Data.^2 + ydot.Values.Data.^2);
stairs(xdot.Values.Time, vel);
ylim([0.9 1.1]*V.Value);
xlabel('time [s]');
ylabel('[m/s]');
legend({'$V$' '$\sqrt{\dot{x}^2+\dot{y}^2}$'}, 'Interpreter', 'latex');
grid('minor');
```

As shown in the bottom right graph, the reconstructed velocity along the path is constant and equates to $V$.

You can find the model and the snippet hosted at https://github.com/pattacini/se-robotics-24695.

Title: How do I calculate reference trajectory for a robot following a parabolic path at a constant speed?

Body:
I have a 3 DOF robot arm and have been tasked with making it follow a path of $y=6-x^2$. Normally to compute the reference trajectory, I would simply create a linearly spaced vector of $x$ values and then compute a vector of $y$ values using the given equation. However, for this task, I need the reference trajectory to have a constant speed. Our textbook has given us this hint, but so far, I have been unable to do anything about it.

How to use the Leibnitz rule to solve for reference trajectory:

Calculate the length of an arc of the parabola at a generic point whose horizontal position is $x(t)$;

Differentiate the length of the arc of the parabola at time $t$ to deduce the translational velocity along the arc. This velocity is constant and equal to $V$. Thus, you can solve for $\dot{x}(t)$.

Once you have $\dot{x}(t)$, you can deduce the reference position and velocity in $x$ and $y$.

This advice confuses me, mostly because I do not understand how an arc can have a length at one specific point.

Thank you for any help you can give me with this problem.

Body:
If the norm of the velocity shall remain constant along the arc of the parabola, then it holds $\sqrt{\dot{x}^2+\dot{y}^2}=V$.

Plugging $\dot{y}=-2x\cdot\dot{x}$ into the equation above, we end up with the nonlinear differential equation:

$$
\dot{x}\sqrt{1+4x^2}=V.
$$

It is not possible to solve the system symbolically, but we can integrate it numerically by relying for example on the model below:

The simulation provides us with the trajectories $x\left(t\right)$, $y\left(t\right)$, $\dot{x}\left(t\right)$, $\dot{y}\left(t\right)$.

The following snippet plots the data (obtained for $V=0.1$ m/s):

% simulate the model
mdl = 'model';
load_system(mdl);
cs = getActiveConfigSet(mdl);
simOut = sim(mdl, cs);

x = find(simOut.yout, 'x');
y = find(simOut.yout, 'y');
xdot = find(simOut.yout, 'xdot');
ydot = find(simOut.yout, 'ydot');

mdlWks = get_param(mdl, 'ModelWorkspace');
V = getVariable(mdlWks, 'V');

% plot the trajectory
figure('color', 'white');
tiledlayout(3, 2);

nexttile([3 1]);
stairs(x.Values.Data, y.Values.Data);
xlabel('x [m]');
ylabel('y [m]');
grid('minor');

nexttile;
hold('on');
stairs(x.Values.Time, x.Values.Data);
stairs(y.Values.Time, y.Values.Data);
xlabel('time [s]');
ylabel('[m]');
legend({'$x$' '$y$'}, 'Interpreter', 'latex');
grid('minor');

nexttile;
hold('on');
stairs(xdot.Values.Time, xdot.Values.Data);
stairs(ydot.Values.Time, ydot.Values.Data);
xlabel('time [s]');
ylabel('[m/s]');
legend({'$\dot{x}$' '$\dot{y}$'}, 'Interpreter', 'latex');
grid('minor');

% verify the constant speed along the arc
nexttile;
hold('on');
yline(V.Value);
vel = sqrt(xdot.Values.Data.^2 + ydot.Values.Data.^2);
stairs(xdot.Values.Time, vel);
ylim([0.9 1.1]*V.Value);
xlabel('time [s]');
ylabel('[m/s]');
legend({'$V$' '$\sqrt{\dot{x}^2+\dot{y}^2}$'}, 'Interpreter', 'latex');
grid('minor');

As shown in the bottom right graph, the reconstructed velocity along the path is constant and equates to $V$.

You can find the model and the snippet hosted at https://github.com/pattacini/se-robotics-24695.


答:
Assuming the system sample interval $\Delta t$ is small enough, the elements of the $x$ and $y$ vectors are given by:

$$ x_n = x_{n-1} + \dot{x}_{n-1}\Delta t$$
$$ y_n = 6 - x_n^2$$

As observed in another answer:

$$
\sqrt{\left(\frac{dx}{dt}\right)^2 +
\left(\frac{dy}{dt}\right)^2} = V
$$

To put in terms of just $dx/dt$:

$$\frac{dy}{dt} = \frac{dy}{dx}\frac{dx}{dt}
=-2x\frac{dx}{dt}
$$

so that:

$$
\frac{dx}{dt} \sqrt{1 + 4x^2} = V
$$

or
$$
\frac{dx}{dt} = \frac{V}{\sqrt{1 + 4x^2}}
$$

Substituting and going back to dot notation:

$$ x_n =  x_{n-1} + \frac{V\Delta t}{\sqrt{1 + 4{x_{n-1}^2}}}
$$

$$ y_n = 6 - x_n^2$$

We are essentially starting a uniform vector in $t$  not $x$ and going from there.


问:
GPS Amplifier - Is this reliable?
I'm multimedia developer who is searching for a way to get GPS signal inside buildings/structures. Is amplification a reliable way to fix this GPS signal issue?

Will a "GPS Amplifier" work as perfectly as using GPS outside?

答:
GPS relies on trilateration to work. That is, the calculated distance from at least three satellites, to fix. So, for working exactly like in an outdoor GPS covered signal environment I doubt it will be feasible.

What you get with that repeaters, is that the indoor devices will get the position of the external antenna/receiver.¹. So if you have a big space and move inside it you will get the same position from the GPS.

So it will depend on your application if this is sufficient or not.
One way it will work is like some repeaters within a big mine for example, so it will show some location points, but this is for legacy compatibility of the signal. If the devices are able to read the location from other types of signals a simple beacon transmitting a programmed location will work, and without any external antennas.

Note 1: the repeater inside the building will relay the exact co-ordinates of the outdoor antenna to the interior of the building - it will not provide co-ordinates for the repeater unit itself. From http://www.gps-repeaters.com

OwnerDisplayName: fiskolin

Title: GPS Amplifier - Is this reliable?

OwnerDisplayName: Diego C Nascimento


问:
How do I implement accurate distance measuring to ground on a rover?
I need the end effector to always be at a fixed distance from the ground, I need that the end effector is as close as possible to the ground, the closer the better.

Given this proximity I need to know if there will be obstacles (such as pebbles) at the point where the end effector will be.

I know little or nothing about sensors and therefore I don't know which one is the best to use (ultrasound, lidar or other) to be able to measure the distance from the end effector to the ground and then adjust the robotic arm.

The sensor could also be placed on the rover and not on the arm.

What solution do you recommend?

答:
You might consider an ultrasonic solution.  They are small, lightweight, inexpensive and accurate to within a few cm at close range.  Their disadvantage is very short range, but perhaps they would work in your application.

If you need something more accurate you will probably need lidar or camera sensors, but these will tend to be much heavier and will end up requiring you to support a heavier effective payload.  That in itself may degrade the accuracy of positioning your arm so that any gain you made in resolution would be washed out in the end.


问:
Marginalization vs Dropping states for sliding window VO
When doing fixed lag smoothing or Windowed Smoothing for visual odometry and map construction, how does marginaliztion differ from dropping past states? How does each work?

I assumed fixed lag smoothing always optimized over a fixed number of images using the most probable state state of the robot during the prior window but it looks like marginalization is different than this? Can someone explain?

答:
Short answer: Marginalization is a fancy way of applying a prior on certain nodes of your factor graph.

Note that Marginalization vs Dropping data is not only specific to the sliding window case, but falls under the general category of how to remove data from a factor graph. To visualize the two approaches you can see in the image below. Circles are our robot poses, Stars are the landmarks, and purple marks the edges/constraints between them. In order to reduce our problem size we want to remove the nodes bounded by the green box.

Dropping data is easy. You simply remove the nodes you are no longer interested in, and solve the smaller graph. However, this should intuitively seem off to you, because we are throwing away information. Those nodes we are removing do provide constraints on our reduced graph, so it would be cool if we could keep that information without the extra computation required for all the nodes. This is where marginalization comes in. It provides a way to summarize the removed nodes impact, and still constrain the remaining nodes. This is done by computing a marginalization node which constrains all of the nodes that were connected to the removed nodes.

Now how is this done?

Dropping Data

For dropping data you just remove the nodes, and solve the graph containing just the nodes you are interested in.

Marginalization

One of the disadvantages of Smoothing/Factor graphs is that our uncertainty is stored in the information form ($I=cov^{-1}$) the inverse of the covariance matrix. Now in the covariance form marginalization is easy. You can simply remove the covariance rows and columns that belong to the states you want to remove. In the information form it is more tricky.

Method 1: Convert our Information matrix $I$ to the covariance form ($I^{-1} = cov$) and remove the rows and columns of the states you want to remove. Then invert again to get back the Information form. Nobody uses this approach since it is very computationally expensive.

Method 2: Schur complement.

This takes advantage of you being able to setup the Information matrix as a block matrix composed of the blocks containing the variables you want to keep and the variables you want to remove. You can then use the Schur complement to remove the unwanted variables, and apply their affect on the kept variables. I know not the best answer, but there are a lot of good resources on how to do this online, and examples in code such as Okvis, and Vins Mono.

As for applying these approaches to fixed lag smoothing. As you have correctly deduced this approach works on a limited number of images, to keep the computation size bounded. Typically you use some sort of removal algorithm to pick nodes you want to remove. A very typical algorithm being removal of the oldest node first. When you have selected which nodes you want to remove you can pick between dropping or marginalization. With marginalization being a better approach because you are retaining some of the information. This leads to better results, because your remaining nodes are constrained in such a manner as if you had not removed those nodes.

Just some final thoughts/comments:

Marginalization is not as good, as retaining the nodes, since it fixes their estimates. Whereas if you had kept those nodes they would also improve over time.

A naive marginalization approach with just the Schur complement can introduce some consistency problems. You typically also have to fix the linearization points of all nodes connected to the Marginalization prior node.

This advantage of Marginalization being easy in Kalman filters, is one of the reason's they are sometimes used over Smoothing based approaches.

Math example

\begin{equation*}
(J^T J) \hat{x} = J^T y
\end{equation*}

The solution of our problem is gotten by solving the following linear equation. This is internally what our factor graph solves. $J$ is the Jacobian, $\hat{x}$ is what we are trying to solve for and $y$ is our observations/error. $J^T*J$ is called the information or Hessian matrix, and is equivalent to the inverse covariance matrix you know from Gaussian math.

This information matrix can be broken up into blocks that corresponds to the various nodes we are trying to solve. In the image below it breaks it up into the individual nodes. The block is only filled if there is a connection between nodes, else it is white or all 0s.

We can group those blocks arbitrarily. For our purpose we are going to group it into 3 parts
$$\begin{equation*}
\begin{bmatrix}
A & B & C \\
B^T & D & E \\
C^T & E^T & F \
\end{bmatrix}  \begin{bmatrix}
x_A  \\
x_D  \\
x_F \
\end{bmatrix} = \begin{bmatrix}
y_A  \\
y_D  \\
y_F \
\end{bmatrix}
\end{equation*}$$

$A$ are the variables we want to remove. $D$ are the variables we want to keep. $F$ will be the new variable we just added to our factor graph. $B,C,E$ are all the cross information terms. $x_X$ are the states we are solving for and $y_X$ are the observations. The subscript designates which variable set it is responsible for. For purpose of our marginalization we can remove $F$ from our matrix since it is not connected to the variables we want to remove. The $C$ part will just be all 0s. So we are left with the following matrix.

$$\begin{equation*}
\begin{bmatrix}
A & B  \\
B^T & D  \\
\end{bmatrix}  \begin{bmatrix}
x_A  \\
x_D  \\
\end{bmatrix} = \begin{bmatrix}
y_A  \\
y_D  \\
\end{bmatrix}
\end{equation*}$$

Now we marginalize away $A$ which will modify the part responsible for variables we are keeping $D$. We now do the schur complement on our block matrix. Which will result in a new information matrix that represents the constraints of $A$ but applied to the $D$ parts of our equation.

$$S =  D - BA^{-1}B^T$$

We can use this now to solve a smaller version of the above problem.

$$\begin{equation*}
\begin{bmatrix}
D - BA^{-1}B^T
\end{bmatrix}  \begin{bmatrix}
x_D  \\
\end{bmatrix} = \begin{bmatrix}
y_D -BA^{-1}*y_A \\
\end{bmatrix}
\end{equation*}$$

Yes $A$ is still involved, but we are only solving for $x_D$ so the problem we are solving is smaller. We get to skip solving for $x_A$. If you check the dimensions of the problem it will be the size of the $D$ matrix, not $A+D$.

Now we can put $F$ back into our equation(our new state), but we will keep this new equation for $D$.

$$\begin{equation*}
\begin{bmatrix}
D - BA^{-1}B^T & E  \\
E^T & F  \\
\end{bmatrix}  \begin{bmatrix}
x_D  \\
x_F  \\
\end{bmatrix} = \begin{bmatrix}
y_D -BA^{-1}*y_A  \\
y_F  \\
\end{bmatrix}
\end{equation*}$$

And this is the setup of our system if we marginalize away the $A$ parts. ( I think the $E$ term also changes, but blanking on that right now)

You can also compare it to if we had dropped $A$ instead. Our linear system would just be
$$\begin{equation*}
\begin{bmatrix}
D & E  \\
E^T & F  \\
\end{bmatrix}  \begin{bmatrix}
x_D  \\
x_F  \\
\end{bmatrix} = \begin{bmatrix}
y_D  \\
y_F  \\
\end{bmatrix}
\end{equation*}$$

which is different.

Extra notes:

You can add as many new terms to $F$ as needed as long as they don't touch any variables from $A$.

You can actually keep $F$ in the equation while we do marginalization. You will ended up with the exact same result, because $F$ and $A$ connections are all $0$.

By looking at these equations you can see some of the problems we mentioned. This marginalization is only valid for 1 iteration. After we solve the marginalized problem our Jacobians change, and our marginalization is no longer valid. For computation sake we accept that, since if we had to remarginalize every single iteration there is no point to it.

Bonus: Marginalization of the map.

Not related to the sliding window filter, but since you mentioned optimizing the map away in Graph SLAM decided to give you those equations to. This approach is very popular in solving Bundle adjustment problems because our map is so much bigger than our poses. E.g thousands of points vs hundreds of camera poses.

Let our system equations for a Bundle Adjustment be the following.

$$\begin{equation*}
\begin{bmatrix}
M & C  \\
C^T & P  \\
\end{bmatrix}  \begin{bmatrix}
x_M  \\
x_P  \\
\end{bmatrix} = \begin{bmatrix}
y_M  \\
y_P  \\
\end{bmatrix}
\end{equation*}$$

$M$ corresponds to the jacobians of the map(the landmarks we are trying to estimate), $P$ are our camera poses, and $C$ are the cross correlation terms.

Eliminate the map using the schur complement.

$$\begin{equation*}
\begin{bmatrix}
P - CM^{-1}C^T
\end{bmatrix}  \begin{bmatrix}
x_P  \\
\end{bmatrix} = \begin{bmatrix}
y_P -CM^{-1}*y_M \\
\end{bmatrix}
\end{equation*}$$

This is the equation to solve for our camera poses $x_P$. However, if we know our camera poses $x_P$ we can very easily figure out the locations of our landmarks. Intuitively you could think of the fact, that knowing camera poses means you can just triangulate the landmarks. That intuitive idea is actually already constrained in our Jacobians. So we can just back-substitute to solve for $x_M$. Equation is just done by some matrix math.

$$M*x_M  = y_M-C^T*x_P $$

And now we have solved for both $x_P,x_M$. The reason why we do this is that

number of poses << number of landmarks

Solving a linear system is expensive so the smaller we can make it, then the better. And by doing this we only have to solve a linear system of size # camera poses rather than # of camera poses + # of landmarks. So it ends up being faster, even if we have to take more intermediate steps.


问:
nav2-smac-planner package not available on foxy
I'm trying to install the nav2-smac-planner package on Foxy but apt install can't find it. Is it not available?

Here seem to indicate it's available on Foxy both source and debian.

Thank you in advance

答:
Posted the same question on github and Steve Macenski replied:

https://github.com/ros-planning/navigation2/issues/3549

Its called just the Smac Planner in Foxy https://github.com/ros-planning/navigation2/tree/foxy-devel/smac_planner mostly due to me messing the naming up. I couldn't change the name back after release so we just have to live with it like that in Foxy unfortunately. I would definitely recommend upgrading to at least Galactic though if you're using the Smac Planner. There are many updates made that were not backported to Foxy.


问:
Does Ros2 Nav2 have a module for "teach and repeat?"
Wouldn't this be a good module to add to Nav2? The rover robot would be driven over a route and "learn" it, then repeat the performance through the navigation stack. Is anyone working on such a module?</p>

答:
Not at this very moment, but its something I had planned to have added as part of a larger (but still not really all that large) development effort to provide documented GPS navigation support within Nav2 instead of the more 'wild west, figure it out yourself' that currently exists. Technically nothing about teach & repeat requires GPS, but that was the context I was planning on developing it under and I've not yet had a strong enough reason to work on it yet over more pressing needs.

If that would be something you'd be interested in working on or contributing, feel free to reach out!


答:
The short answer is no.

By amplifying a GPS signal, you are creating a multipath situation. Instead of the proper signal path from the satellite directly to the receiver, you would be creating a triangle inequality (unless your amplifiers just happened to be exactly in line between each satellite and the receiver).

You might decide that having an imperfect signal inside the building is better than no signal at all.  However, the bigger issue is that you'd be magnifying the multipath effects for any GPS receivers outside the building.

You should look at an alternate GPS-like technology, such as radio-frequency TDOA (time-difference of arrival).

Title: GPS Amplifier - Is this reliable?

Body:
I'm multimedia developer who is searching for a way to get GPS signal inside buildings/structures. Is amplification a reliable way to fix this GPS signal issue?

Will a "GPS Amplifier" work as perfectly as using GPS outside?