问:
ROS Package Dependencies Installation for Begginer
I am new with ROS, just started few days ago basically. I have one question regarding my workspace and packages.

Let's say I created my workspace catkin_ws and I cloned in src folder git repository. That git repository have multiple packages. Each package has some dependencies (i.e. other packages that he depends on). There is big probability that I will not have all of those dependencies installed and that I will need to install some.

Let's say that I have run now catkin_make in my catkin_ws directory. Because I do not have specific packages (i.e. dependencies) installed, error will be generated. At this point I am confused. I found three ways of dealing this problem, and I will describe them now and possible issues:

Cloning git repository of that specific dependency (i.e. package) that is missing into the src folder of my workspace.

This would mean that I would have to clone every git repository for every dependency that I am missing, and this can go nested. So in my opinion this solution is not optimal.

Installing each specific dependency (i.e. package) that is missing into using apt-get.

Also this would require to install every dependency that is missing manually, and also this can go nested.

But all dependencies that are needed for certain ros project are given by manufacturer so I could simple install them in one go.

Installing all missing dependencies using rosdep as follows: rosdep install --from-paths src --ignore-src -r -y from my workspace folder catkin_ws

So this command would go to my src folder, look at each package's package.xml file and install dependencies that are missing (theoreticaly).

But apparently even if I run this command, and then catkin_make there are still some dependencies missing that cannot be installed with this command. Then I applied 1st method and it worked.

Also is it better to install dependencies in my workspace rather than in system directory /opt/ros/noetic/share? But I do not know how to install them in my workspace using method number 3 (rosdep). Even though I run that command in my workspace folder dependencies are installed in system directory.

Also I did not used method number 2 so far, because I wanted to try to install dependencies in my workspace, and this method number 2 is installing somewhere else I suppose, but not in my directory obviously.

Can someone explain me these things, and tell me is my understanding of this correct? Can you tell me also how to install dependencies in most secure and convenient way.

All these things I was experimenting when I tried to run gmapping SLAM with turtlebot3.

答:
It looks like you have a question about best practicies for managing dependencies in ROS 1. Using rosdep is the prefferred route during development (option 3). However, option 1 (building everything from source) is viable when you need to patch one of your dependencies.

The reason rosdep is preferred is because will use the right package manager on your operating system. If you use Ubuntu it will use apt-get. If you (or someone else) uses Fedora, then rosdep will use dnf or yum.

You're correct that rosdep install --from-paths src --ignore-src -r -y will look at all packages in your src folder, look at each package.xml, and install the missing dependencies. If there are any missing dependencies after running that command, then it wasn't specified in any package.xml file in the workspace.

The fix is to add the dependency to the package.xml of the package that needs it, but how you do that depends on what type of dependency it is. There are two kinds of dependencies:

ROS packages (packages installed from http://packages.ros.org)

System dependencies (packages installed from the regular apt repos)

ROS packages (such as gmapping or move_base), are packages that are released into ROS. All you need to do is add the name of the package as specified in its package.xml. If all you know is the debian package name, remove ros-<rosdistro> and replace any dashes - with underscores _. The name to put in your package.xml for ros-noetic-gmapping would be gmapping for the gmapping package, or move_base for ros-noetic-move-base, etc.

System packages are normal packages that you could install without ROS, such as libgflags-dev. All you need to know is the name of the rosdep key for that dependency. The rosdep key is an alias that will map to the right package name on your platform. You can find rosdep keys by searching for them on https://index.ros.org . If you use gflags in your package, searching for gflags on index.ros.org will tell you the name of the rosdep key is libfglags-dev. Generally the rosdep key name is the same as the Debian/Ubuntu package name.

If there is no rosdep key for your package, then in the short term use apt-get, but also consider contributing a rosdep rule for it.


问:
Inverted pendulum LQR controller using motor
In my lab, I am trying to balance an inverted pendulum using simple LQR. To balance the arm I am using a DC motor. Now I have the A, B, C, D matrices. Using those I am able to find the feedback gain 'K' using MATLAB 'lqr' command. There are several parameters needed to find A, B, C, D. Among these are 3 parameters related to electrical characteristics of the DC motor:

motor torque constant;

motor back-emf constant;

motor armature resistance.

There are two DC motors that I am using:

one is 24 V, 3600 RPM, and;

the other is rated 12 V but lesser RPM (exact RPM not known).

I have the three electrical characteristics mentioned above for the 12 V rated motor, and using that motor I have successfully balanced the pendulum by a lots of tuning of Q, R matrices.

My problem here is that for the 24 V rated motor I don't have those 3 parameters. And hence my A, B, C, D matrices cannot be found for system using 24 V motor.

My question is will those three parameters really affect the system matrices that much? Or since every other parameter in system remains same except those 3 when I change motor, so even if I use the 12 V motor's electrical characteristics for 24 V motor also, meaning the same A, B, C, D matrices, I should still be able to balance the system?

The reason I am asking this is, because LQR 'K' depends on A, B matrices of system, so if I use the wrong A, B matrices then the resulting 'K' can never actually balance it.

Note: I will use 24 V motor at 12 V supply only since the objective requires to keep 12 V as max control effort.

答:
My question is will those three parameters really affect the system matrices that much? Or since every other parameter in system remains same except those 3 when I change motor, so even if I use the 12 V motor's electrical characteristics for 24 V motor also, meaning the same A, B, C, D matrices, I should still be able to balance the system?

My first comment is that it's a lab - try it! See what happens. You can write lab reports (or entire research papers) on experiments that went wrong if you can explain why intuition would lead you to think one outcome would happen and the phenomena that's occurring that causes the unexpected outcome to happen.

Second comment is also that it's a lab, so if your question is:

My problem here is that for the 24 V rated motor I don't have those 3 parameters

motor torque constant;

motor back-emf constant;

motor armature resistance.

I'd encourage you to run the tests you need to run to calculate those parameters.

I've also spent about 10 years working professionally in simulation and would strongly encourage you to simulate your system. Matlab will give you a controller for your system, so try applying that controller to the same system, then vary the motor parameters (25% different, 50% different, etc.) but apply the same controller. See what the simulated/predicted impact is on performance. Try the experiment with the 12V motor and see how that compares with simulation. Try the 24V motor and see if you can estimate parameters by comparing actual performance to your simulated parameter variations. Measure the motor parameters and see how your estimates compare.

Labs are your chance to apply what you've learned to a real system. Sometimes those "frictionless" assumptions hold, sometimes they bite you. The motor speeds you can perfectly calculate are now noisy encoder readings. Your continuous systems are now discrete.

Go try it. See if you can use your knowledge to predict the outcomes before they happen, and if you find the experiment is surprising then think critically about the differences between reality and theory and explain why the results are surprising.

ContentLicense: CC BY-SA 4.0


问:
When assigning frames in Denavit-Hartenberg (DH) parameters, where do I put the frame on end effector?
I have a simple question, for the last link (end effector) I need to estimate the parameters but when I put the reference frames in the center of the end effector, I just don't know.

Last link (end effector) will be from origin of previous link to where? I don't know why I can't understand this one.

答:
Here are some comments for the DH method:

Certain parameters of the frame attached to the first and last links need not be defined. They can be chosen arbitrarily.

But generally, make as many parameters as possible to zero.


问:
Why does the laser cut through walls? ray_plugin
I have build a robot with a urdf to visualize in gazebo. After some struggle being new to gazebo I got it working. I then decided to make a video series for myself and others on how I did it.

Sadly the new urdf's "ray" plugin taking from gazebo wiki won't play nice. The laser visualize and the rostopic echo the laser but the laser cuts through everything so it publishes inf for all data point on "echo rostopic".I went online but could not fix it.

Spawning this "test" bot in the other working robot workspace heralds the same result. for the live of me I can't see why?

```<code><link name="laser_link">
<visual>
    <geometry>
        <cylinder radius="0.02" length="0.05"/>
    </geometry>
    <material name="Blue">
        <color rgba="0.31 0.18 2.07 1.0"/>
    </material>
</visual>
<collision>
    <geometry>
        <cylinder radius="0.02" length="0.05"/>
    </geometry>
</collision>
<inertial>
    <mass value="0.002"/>
    <inertia ixx="0.000015417" ixy="0.0" ixz="0.0" iyy="0.000015417" iyz="0.0"  izz="0.000004"/>
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
</inertial>
</link>

<joint name="laser_joint" type="fixed">
   <parent link="base_link"/>
   <child link="laser_link"/>
   <origin xyz="0 0.0 0.275" rpy="0 0 0"/>
<!--axis xyz="0 0 0"/-->
</joint>

<gazebo reference="laser_link">
     <sensor type="ray" name="LD06_laser">
         <pose>0 0 0 0 0 0</pose>
         <visualize>true</visualize>
         <update_rate>20</update_rate>
     <ray>
        <scan>
            <horizontal>
                <samples>362</samples>
                <resolution>1</resolution>
                <min_angle>-3.141592654</min_angle>
                <max_angle>3.141592654</max_angle>
            </horizontal>
        </scan>
        <range>
            <min>0.02</min>
            <max>12.0</max>
            <resolution>0.01</resolution>
        </range>
    </ray>
    <plugin name="laser_controler" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>laser_link</frameName>
    </plugin>
</sensor>
</gazebo>
</code>```

Any help appreciated.

Used the plugin from here:plugin from tutorial
and using a roslaunch file from here:the launch file used

Heres the buggy urdf: buggy laser
The working urdf: working urdf
the ros_launch files for both are the same only the path to urdf's are different and both loads the gas station or willows garage just fine. the Buggy laser does have a 2/3 second black screen before loading where working one loads up instantly.

incase the launch files: faulty laser launch and working laser launch

答: The "ray" sensor in Gazebo detects collision shapes. If you want to use the visual shapes, you need to use "gpu_ray" in the sensor type. Also, since "ray" and "gpu_ray" are deprecated, you should use "lidar" or "gpu_lidar"</p>


问:
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.

答: After struggling with this for a while, the best repo I have found for offline reconstruction/pose graph optimization and merging multisession data is interactive slam. https://github.com/SMRT-AIST/interactive_slam</p>

Title: Multisession odometry


问:
Follow wall and avoid obstacles using lidar
I want to follow a wall and avoid any obstacles on the way.
My robot has a 360 degree sensor (lidar). I want to calculate a line (vector) to follow.

I tried a lot of different solutions but I didn't achieve the goal.
I also checked this article, Calculate robot heading to follow wall and avoid obstacles, but it didn't really help or explain things.

My robot sometimes stuck between an obstacle (turning in both sides). I think PID can't help here.

What can I do to avoid obstacles on the way?

Here is the code.

```<code>#! /usr/bin/python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
import numpy as np
import time

class Wall(Node):
    def __init__(self):
        super().__init__("PID")
        self.forward_speed = 0.25
        self.turning_speed = 0.5 # rad/s

        self.publisher = self.create_publisher(Twist, "/cmd_vel", 10)
        self.subscriber = self.create_subscription(LaserScan, "/scan", self.scan_callback, 10)

    def scan_callback(self, scan: LaserScan):
        msg = Twist()
        msg.linear.x = 0.0
        msg.linear.y = 0.0
        msg.linear.z = 0.0
        msg.angular.x = 0.0
        msg.angular.y = 0.0
        msg.angular.z = 0.0

        let = 4.0 #range of an obstacle, wall ..
        left = Average(scan.ranges[225:315]) # 90 degrees from left (get more precise values from range)
        front = Average(scan.ranges[160:200]) # 40 degrees from front (get more precise values from range)

        if (left < let and front < let): #left and front are locked
            msg.angular.z = -self.turning_speed #move right
        elif (left > let and front > let): #left and front are unlocked
            msg.angular.z = self.turning_speed / 2 #move left
        elif (left > let and front < let): #left is unlocked, front is locked
            msg.angular.z = self.turning_speed / 2 #move left
        else:
            msg.linear.x = self.forward_speed

        self.publisher.publish(msg)

def Average(lst):
    # return sum(lst) / len(lst)
    res = np.sum(np.ma.masked_invalid(lst)) / len(lst) #ignore Inf
    res =  res.__float__() # '--' to nan
    if (np.isnan(res)):
        res = 10.0 #assume there is a lot space
    return res

def check_wall(args=None):
    rclpy.init(args=args)
    node = Wall()
    rclpy.spin(node)
    rclpy.shutdown()
</code>```

答:
From this code you might only get to solve the wall following behavior but to add another behavior  of obstacle avoiding you need to define in states. So you can easily shift between them . PID is just for smoothing things .

Look into Bug 2 Algorithm -> heads straight for the target until it hits an obstacle, then follows the obstacle's contour until the straight-line path it was initially following is optimal (i.e., it won't get any shorter by continuing to follow the obstacle's contour). This kind of algorithm could be applied to your problem.


问:
Fixed point arithmetic on microcontrollers
Often we use microcontrollers to do things in our robots, but need to make some calculations in decimal. Using floating point variables is very slow, because a software floating point library is automatically included (unless you have a high-end microcontroller). Therefore, we generally use fixed point arithmetic.

Whenever I do this, I just use an integer, and remember where the decimal place is. However, it does take some care to ensure that everything is consistent, especially when calculations involve variables where the decimal point is in a different place.

I have implemented a fixed point atan2 function, but because I was trying to squeeze every last drop of limited precision (16 bits), I would often change the definition of where the decimal point is, and it would change as I tweaked it. In addition, I would have some constants, as a quasi look-up table, which themselves have an implied decimal point somewhere.

I want to know if there is a better way. Is there a library, or set of macros, that can simplify the use of fixed point variables, making multiplication and division between mixed variables easier, and allowing declaration of decimal numbers or constant expressions, but automatically converting to the desired fixed point representation at compile time?

答:
It would be nice if we could tell the compiler the range and precision of each fixed-point input variable (perhaps no two having the radix point in the same location), and it would automagically -- at compile time -- use the correct range and precision and rescaling operations for the intermediate values and final values in a series of calculations.
I've heard rumors that it may be possible to do that in the Ada programming language or in C++ templates.

Alas, the closest I've seen is fixed-point arithmetic libraries that require you, the programmer, to manually choose the correct representation and manually verify that each operation maintains adequate range and precision.
Sometimes they make multiplication and division between mixed variables easier.
Such as:

AVRfix: a library for fixed point calculation in s15.16, s7.24 and s7.8 format, entirely written in ANSI C

Embedded Systems: fixed point FFT lists some libraries for fixed-point FFT calculation

AN617: fixed point routines for the Microchip PICmicro

"fixed point" projects on SourceForge.

gcc has built-in fixed-point libraries a b

TI IQMath Library ( and source -- Thank you, embedded.kyle ).

ContentLicense: CC BY-SA 3.0


问:
How do I record video from webots simulated cameras?
I'm currently trying to find a way to record the output from Webots cameras to video files. Currently I'm saving frames every cycle and converting them to videos afterwards using Shotcut. Is there a smoother way of doing this? I need to record from 6 different cameras so I can emulate the real cameras on my system and make simulated video streams in different simulated scenarios (for some algorithm testing).

Does anyone have an idea of how to do this? Or is saving to frames using wb_camera_save_image the only way?

Much appreciated!

答:
Webots uses ffmpeg to create movies out of JPG images. The code snippet in Webots is located here. But since Webots embeds ffmpeg, you should be able to use it pretty easily from your controller program:

In your controller program, save every camera image as a JPG image in a specific folder. You should use a counter to name each image, e.g., image-001.jpg, image-002.jpg, etc.

At some point in your controller program, e.g., when you have enough images to create a movie, run ffmpeg with the following command line or similar: ffmpeg -framerate 32 -i image-%03d.jpg -c:a copy -crf 20 -c:v libx264 -pix_fmt yuv420p movie.mp4 (replace 32 by your actual camera frame rate).

Do this for every camera (you may use a different folder for each camera).

Disclaimer: I am a Webots developer working at Cyberbotics.


问:
From Euler angles to Rotation matrix Staubli
I want to know the transformation matrix between End effector and Robot base (FK). Staubli gives me x,y,z and Rx,Ry,Rz. Those three Rotations are Euler angles? How could I transform those Euler angles to Rotation matrix? What convention of Euler does Staubli use? It should be R = Rz(φ)Ry(θ)Rx(ψ)?</p>

答:
Consider using elemental rotation matrices as defined on this wikipedia.org page:

... then substituting the angle into the trigonometric functions in the matrix and finally cross multiplying the X, Y & Z matrix with the rotation matrix. For example to rotate a vector [1, 0, 0] 90 degrees about the Z axis perform the following calculations:


问:
Missing Fixed Frame in My Model
Why isn't Gazebo 9 displaying the tip's frame? Using RViz, you can view the frame as shown in the below image. In addition, I can't see the link listed under the model in Gazebo. There seems to be no visualisation of fixed frames. Gazebo only displays one fixed joint, which connects the world and base link.

```<code><link name="tip"/>
<joint name="tip_joint" type="fixed">
   <parent link="mid_link"/>
   <child link="tip"/>
   <origin xyz="0 0 ${height2}" rpy="0 0 0"/>
</joint>
</code>```

答:
Haven't used Gazebo (still), but in looking around online it looks like:

You need the inertial tag to properly load a model in Gazebo (but not for rivz, that's why you are able to visualize the urdf with rviz)

There's another question that asks, "How to use Gazebo without the inertial tag" and the answer there is:

You can try just using rviz instead of using gazebo. The rviz models don't require physics attributes.

Any other answers that I find looking around online for "visualizing reference frames in Gazebo" all seem to refer to visualizing link frames in Gazebo, and again it looks like the links need to have physics to be represented.

Your joint may not be showing up because your link isn't getting loaded, which may then cause the joint to fail; here's another question that mentions an error when Gazebo tries to load their URDF:

Failed to build tree: child link [ired1] of joint [ired1_joint] not found This is what I get when I try to launch my gazebo simulation. It is strange because I have declared all the joints and links:

```<code>[ERROR] [1537878063.001524419]: Failed to build tree: child link [ired1] of joint [ired1_joint] not found
</code>```

And a comment in response says,

The Gazebo URDF parser plainly ignores links without the required dynamics-related elements. Whether they are necessary or not, it is a common cause for error messages relating to missing links and/or joints with Gazebo.

Is Gazebo throwing any errors?

Title: Missing Fixed Frame in My Model

Body:
Haven't used Gazebo (still), but in looking around online it looks like:

You need the inertial tag to properly load a model in Gazebo (but not for rivz, that's why you are able to visualize the urdf with rviz)

There's another question that asks, "How to use Gazebo without the inertial tag" and the answer there is:

You can try just using rviz instead of using gazebo. The rviz models don't require physics attributes.

Any other answers that I find looking around online for "visualizing reference frames in Gazebo" all seem to refer to visualizing link frames in Gazebo, and again it looks like the links need to have physics to be represented.

Your joint may not be showing up because your link isn't getting loaded, which may then cause the joint to fail; here's another question that mentions an error when Gazebo tries to load their URDF:

Failed to build tree: child link [ired1] of joint [ired1_joint] not found This is what I get when I try to launch my gazebo simulation. It is strange because I have declared all the joints and links:

[ERROR] [1537878063.001524419]: Failed to build tree: child link [ired1] of joint [ired1_joint] not found

And a comment in response says,

The Gazebo URDF parser plainly ignores links without the required dynamics-related elements. Whether they are necessary or not, it is a common cause for error messages relating to missing links and/or joints with Gazebo.

Is Gazebo throwing any errors?


问:
Decision behind ROS2's Python tutorial switched from src layout to flat layout
As far as I can see, the entire tutorial suite of ROS2 (citing Creating a package (docs.ros.org) to begin with) has switched from what is called "src layout" to "flat layout" (i.e. Python packages are directly placed at the top level directoryin a ROS/Ament package) regarding folder structuring in a package that hosts Python modules and even executables (Ref. "src layout vs flat layout" (python.org)). Back in ROS1, Python binding tutorials were predominantly using "src layout" (i.e. Putting Python packages under a sub-folder called src) if I'm not mistaken.

Arguing about the decision is not my intention. I'm curious to see what was the decision behind the switch, as I haven't found anything. Personally I'm undecided if I choose either side, choice may depend on the type of package.

UPDATE-1: Just FYI, though this should be irrelevant from the "src/flat layout" decision, I found the current implementation of ament(/colcon) doesn't support "src layout" usecase well -- Using ament_python_install_package(%PACKAGE_TOPDIR%), if I do ament_python_install_package(src) in order to let ament know where my Python modules/executables are in, then I get src installed, which is not a Python package I am making. The same cmake macro doesn't seem to support slash in the argument so passing src/mypackage errors.

答:
I expect that the rationale behind that exact change is likely lost to time. However, my intuition says this is likely a change related to one or more of:

The removal of "devel" workspaces in ament. Python documentation notes that the "src layout" requires an installation step. With catkin, we had this going on, but ament only provides the --symlink-install option. Catkin had some really crazy CMake going on, and one of the main goals of Ament was to streamline things.

ROS2 supports pure Python packages (with no CMakeLists.txt, just setup.py)

Developer preferences. While there is some overlap in the team that created Catkin and the team that created Ament, the lead developers at inception of each project were different people.


问:
Odom only works with fixed joints
I'm simulating a robot with 4 omnidirectional wheels and I'm using the planar move plugin in the gazebo to simulate the behavior of the omnidirectional wheels.

To simulate the wheels I was using continuous joints however with this plugin in rviz2 and with the fixed frame in odom, the wheels do not move with the robot. Although if I'm using a fixed joint and controlling the robot with teleop_twist_keyboard the wheels move together with the robot.

Is this a plugin problem or am I missing something to declare?

Follows URDF code to create the robot.

robot_core:

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

    <xacro:include filename="inertial_macros.xacro"/>
    <xacro:include filename="gazebo_control.xacro" />

    <material name="white">
        <color rgba="1 1 1 1"/>
    </material>
    <material name="blue">
        <color rgba="0.2 0.2 1 1"/>
    </material>
    <material name="black">
        <color rgba="0 0 0 1"/>
    </material>
    <material name="red">
        <color rgba="1 0 0 1"/>
    </material>

    <!--BASE LINK-->
    <link name="base_link">
    </link>

    <!--CHASSI-->
    <joint name="chassi_joint" type="fixed">
        <parent link="base_link"/>
        <child link="chassi_link"/>
        <origin xyz="0 0 0"/> <!--VIRADO PARA X-->
        <!--<origin xyz="0 0 0" rpy="0 0 ${pi/2}"/> VIRADO PARA Y-->
    </joint>

    <link name="chassi_link">
        <visual>
            <origin xyz="0 0 0.085"/>
            <geometry>
                <cylinder radius="0.275" length="0.17"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin xyz="0 0 0.085"/>
            <geometry>
                <cylinder radius="0.275" length="0.17"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.17" radius="0.275">
            <origin xyz="0 0 0.085" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="chassi_link">
        <material>Gazebo/White</material>
    </gazebo>

    <!--BODY-->
    <joint name="body_joint" type="fixed">
        <parent link="chassi_link"/>
        <child link="body_link"/>
        <origin xyz="-0.1 0 0.18" rpy="0 0 0"/>
    </joint>

    <link name="body_link">
        <visual>
            <origin xyz="0 0 0.55" rpy="0 0 <span class="math-container">${pi/2}"/>
        <geometry>
            <box size="0.2 0.05 1.1"/>
        </geometry>
        <material name="white"/>
    </visual>
    <collision>
        <origin xyz="0 0 0.55" rpy="0 0 $</span>{pi/2}"/>
            <geometry>
                <box size="0.2 0.05 1.1"/>
            </geometry>
        </collision>
        <xacro:inertial_box mass="0.1" x="0.2" y="0.05" z="1.1">
            <origin xyz="0 0 0.55" rpy="0 0 0"/>
        </xacro:inertial_box>
    </link>

    <gazebo reference="body_link">
        <material>Gazebo/White</material>
    </gazebo>

    <!--WHEELS-->

    <!--LEFT FRONT-->
    <joint name="left_front_joint" type="fixed">
        <parent link="chassi_link"/>
        <child link="left_front_link"/>
        <origin xyz="0.2 0.2 0" rpy="-<span class="math-container">${pi/2} 0 -$</span>{pi/4}"/>
        <!--<axis xyz="0 0 1"/>-->
    </joint>

    <link name="left_front_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.1">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="left_front_link">
        <material>Gazebo/Blue</material>
        <mu1 value="0.001"/>
        <mu2 value="0.001"/>
    </gazebo>

    <!--LEFT BACK-->
    <joint name="left_back_joint" type="fixed">
        <parent link="chassi_link"/>
        <child link="left_back_link"/>
        <origin xyz="-0.2 0.2 0" rpy="0 -<span class="math-container">${pi/2} -$</span>{pi/4}"/>
        <!--<axis xyz="0 0 1"/>-->
    </joint>

    <link name="left_back_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.1">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="left_back_link">
        <material>Gazebo/Black</material>
        <mu1 value="0.001"/>
        <mu2 value="0.001"/>
    </gazebo>

    <!--RIGHT FRONT-->
    <joint name="right_front_joint" type="fixed">
        <parent link="chassi_link"/>
        <child link="right_front_link"/>
        <origin xyz="0.2 -0.2 0" rpy="0 <span class="math-container">${pi/2} -$</span>{pi/4}"/>
        <!--<axis xyz="0 0 -1"/>-->
    </joint>

    <link name="right_front_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.1">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="right_front_link">
        <material>Gazebo/Blue</material>
        <mu1 value="0.001"/>
        <mu2 value="0.001"/>
    </gazebo>

    <!--RIGHT BACK-->
    <joint name="right_back_joint" type="fixed">
        <parent link="chassi_link"/>
        <child link="right_back_link"/>
        <origin xyz="-0.2 -0.2 0" rpy="<span class="math-container">${pi/2} 0 -$</span>{pi/4}"/>
        <!--<axis xyz="0 0 -1"/>-->
    </joint>

    <link name="right_back_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.1" length="0.04"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.1">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="right_back_link">
        <material>Gazebo/Black</material>
        <mu1 value="0.001"/>
        <mu2 value="0.001"/>
    </gazebo>

    <xacro:omni_steering/>

</robot>
</code>```

gazebo_control:

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

    <xacro:macro name="omni_steering" params="">
        <gazebo>

             <plugin name="omni_steering" filename="libgazebo_ros_planar_move.so">
                <commandTopic>cmd_vel</commandTopic>
                <odometry_rate>50.0</odometry_rate>
                <odometry_frame>odom</odometry_frame>
                <robot_base_frame>base_link</robot_base_frame>

                <publish_odom>true</publish_odom>
                <publish_odom_tf>true</publish_odom_tf>
                <publish_wheel_tf>true</publish_wheel_tf>
            </plugin>

        </gazebo>
  </xacro:macro>

</robot>
</code>```

robot_xacro:

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

    <xacro:include filename="robot_core.xacro" />

</robot>
</code>```

rsp.launch.py:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node

import xacro

def generate_launch_description():

```<code># Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time')

# Process the URDF file
pkg_path = os.path.join(get_package_share_directory('charmie_bot'))
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
robot_description_config = xacro.process_file(xacro_file)

# Create a robot_state_publisher node
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    output='screen',
    parameters=[params]
)

# Launch!
return LaunchDescription([
    DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use sim time if true'),
    node_robot_state_publisher
])
</code>```

launch_sim.launch.py:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node

def generate_launch_description():

```<code>package_name='charmie_bot' #<--- CHANGE ME

rsp = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory(package_name),'launch','rsp.launch.py'
            )]), launch_arguments={'use_sim_time': 'true'}.items()
)

 # Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
        )

# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                    arguments=['-topic', 'robot_description',
                               '-entity', 'my_bot'],
                    output='screen')

# Launch them all!
return LaunchDescription([
    rsp,
    gazebo,
    spawn_entity
])
</code>```

答:
This usually happens when you do not set the use_sim_time parameter as True in the simulation.

Attached below is the launch file I used to run rviz2 with your robot with the set parameters:

#! /usr/bin/env python3
"""Spawn Robot in RViz2."""

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, Command
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node

def generate_launch_description():
    """Launch Function."""
    pkg_dir = get_package_share_directory('your_ros2_pkg')
    rviz_config = os.path.join(pkg_dir, 'config', 'rviz', 'urdf.rviz')

    return LaunchDescription([
        # Launch Arguments
        DeclareLaunchArgument("rviz_config",
                              default_value=rviz_config,
                              description="RViz Config"),

        # Nodes
        Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': True}],
        ),

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[
                {'use_sim_time': True},
                {'robot_description': Command(
                    ['xacro ', os.path.join(pkg_dir, 'urdf/robot.xacro')])}
                ]
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            output='screen',
            arguments=['-d', LaunchConfiguration("rviz_config")],
        ),
    ])
```
<ul>
<li>Below is the run of the planar move plugin with continuous joints enabled:

<img src="https://i.stack.imgur.com/I3DcF.gif"/>
</li>
<li>Additionally, your plugin definition has incorrect XML header tags. Refer to the official gazebo_ros_pkgs migration doc to get the updated plugin definition.</li>
</ul>
<h2>Edit:</h2>
<p>It seems that you have not included <code>joint_state_publisher</code> in the <code>rsp.launch.py</code> script.<br/>
Modify the launch file to add the node:</p>
<pre class="lang-py prettyprint-override"><code>node_joint_state_publisher = Node(
    package='joint_state_publisher',
    executable='joint_state_publisher',
    name='joint_state_publisher',
    output='screen',
    parameters=[{'use_sim_time': use_sim_time}],
)
</code>```

Title: Odom only works with fixed joints

Body:
This usually happens when you do not set the use_sim_time parameter as True in the simulation.

Attached below is the launch file I used to run rviz2 with your robot with the set parameters:

#! /usr/bin/env python3
"""Spawn Robot in RViz2."""

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, Command
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node

def generate_launch_description():
    """Launch Function."""
    pkg_dir = get_package_share_directory('your_ros2_pkg')
    rviz_config = os.path.join(pkg_dir, 'config', 'rviz', 'urdf.rviz')

    return LaunchDescription([
        # Launch Arguments
        DeclareLaunchArgument("rviz_config",
                              default_value=rviz_config,
                              description="RViz Config"),

        # Nodes
        Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': True}],
        ),

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[
                {'use_sim_time': True},
                {'robot_description': Command(
                    ['xacro ', os.path.join(pkg_dir, 'urdf/robot.xacro')])}
                ]
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            output='screen',
            arguments=['-d', LaunchConfiguration("rviz_config")],
        ),
    ])

Below is the run of the planar move plugin with continuous joints enabled:

Additionally, your plugin definition has incorrect XML header tags. Refer to the official gazebo_ros_pkgs migration doc to get the updated plugin definition.

Edit:

It seems that you have not included joint_state_publisher in the rsp.launch.py script.

Modify the launch file to add the node:

node_joint_state_publisher = Node(
    package='joint_state_publisher',
    executable='joint_state_publisher',
    name='joint_state_publisher',
    output='screen',
    parameters=[{'use_sim_time': use_sim_time}],
)


问:
What’s this gearbox called?
I took an apart a child’s toy because it looked like it had a neat gearbox in it, it did. The problem is I have no idea what type of gearbox is and I wanna see more things used with it.

It rotates both wheels forward when the motor goes in the forward direction. Thing weird thing is, when going in reverse it has one wheel go forward and one go backward is to turn the car.

答:
I don't know what this is called, but my suspicion is that this mechanism makes use of something very similar to a differential gear.

When the drive shaft is rotating in one direction, the pinion gear (green) engages with the ring gear (blue) as normal, but when the drive shaft is rotating in the other direction, the pinion gear engages with an extended side gear (cyan) instead, while locking the ring gear in place.

Since the ring gear is fixed in place, the torque going into the side gear directly would be translated via the spider gears (orange) into torque in the  opposite direction on the other half shaft (red).

Thanks to Mallaky for the animated gif.

Title: What’s this gearbox called?


问:
The 'stomp' configuration parameter was not found
I am trying to use STOMP Planner by following the documentation. As per the documentation, I installed the STOMP from the source in ROS Melodic (Ubuntu 18.04.6 LTS). Unfortunately, the command roslaunch panda_moveit_config demo_stomp.launch is throwing following error:

```<code>[ERROR] [1684923835.899294129]: The 'stomp' configuration parameter was not found
terminate called after throwing an instance of 'std::runtime_error'
  what():  Unable to initialize planning plugin
[move_group-5] process has died [pid 22522, exit code -6, cmd /home/ravi/ws_moveit/devel/lib/moveit_ros_move_group/move_group --debug __name:=move_group __log:=/home/ravi/.ros/log/12f2b1ec-fa1d-11ed-89a8-181deaf411ae/move_group-5.log].
log file: /home/ravi/.ros/log/12f2b1ec-fa1d-11ed-89a8-181deaf411ae/move_group-5*.log
</code>```

Please note that I already have panda_moveit_config, as mentioned here. Furthermore, I can run the demo.launch based on the documentation, as shown below:

```<code>roslaunch panda_moveit_config demo.launch rviz_tutorial:=true
</code>```

I assume the STOMP error is due to an installation/configuration issue. Surprisingly I have not touched anything. Moreover, the error appears even when MoveIt is installed from the source.

Below is a screenshot of RViz:

BTW, I can see stomp, stomp_moveit, and stomp_plugins packages in my workspace as shown below:

```<code>$ catkin build
...
[build] Found 52 packages in 0.0 seconds.
[build] Package table is up to date.
...
Starting  >>> moveit_core
Finished  <<< moveit_resources                     [ 0.1 seconds ]
Finished  <<< ros_industrial_cmake_boilerplate     [ 0.1 seconds ]
Starting  >>> stomp
Finished  <<< stomp                                [ 0.4 seconds ]
Finished  <<< moveit_core                          [ 0.7 seconds ]
Starting  >>> chomp_motion_planner
Starting  >>> moveit_controller_manager_example
Starting  >>> moveit_visual_tools
Starting  >>> stomp_moveit
Finished  <<< moveit_kinematics                    [ 0.4 seconds ]
Finished  <<< moveit_ros_robot_interaction         [ 0.3 seconds ]
Starting  >>> moveit_ros_benchmarks
Finished  <<< stomp_moveit                         [ 0.3 seconds ]
Starting  >>> stomp_plugins
Finished  <<< moveit_ros_perception                [ 0.8 seconds ]
Finished  <<< stomp_plugins                        [ 0.3 seconds ]
Finished  <<< moveit_ros_move_group                [ 0.6 seconds ]
Finished  <<< moveit                               [ 0.1 seconds ]
...
[build] Summary: All 52 packages succeeded!
</code>```

Any workaround to fix this issue, please?

Update

I am debugging the issue. So I would like to share the progress below:

The error is coming from stomp_planner.cpp#L756

I added the following snippet just above it to see the values:
```<code>std::string param_value;
nh.getParam(param, param_value);
ROS_INFO_STREAM("The '" << param << "' parameter is: " << param_value << " and namespace: "<< nh.getNamespace());
</code>```
It printed the following:
```<code>[ INFO] [1685011022.181549319]: The 'stomp' parameter is:  and namespace: /move_group/planning_pipelines/stomp
</code>```

Suprisingly, I can see the param via rosparam
```<code>$ rosparam list | grep stomp
/move_group/planning_pipelines/stomp/jiggle_fraction
/move_group/planning_pipelines/stomp/manipulator/group_name
/move_group/planning_pipelines/stomp/manipulator/optimization/control_cost_weight
/move_group/planning_pipelines/stomp/manipulator/optimization/initialization_method
/move_group/planning_pipelines/stomp/manipulator/optimization/max_rollouts
/move_group/planning_pipelines/stomp/manipulator/optimization/num_iterations
/move_group/planning_pipelines/stomp/manipulator/optimization/num_iterations_after_valid
/move_group/planning_pipelines/stomp/manipulator/optimization/num_rollouts
/move_group/planning_pipelines/stomp/manipulator/optimization/num_timesteps
/move_group/planning_pipelines/stomp/manipulator/task/cost_functions
/move_group/planning_pipelines/stomp/manipulator/task/noise_generator
/move_group/planning_pipelines/stomp/manipulator/task/noisy_filters
/move_group/planning_pipelines/stomp/manipulator/task/update_filters
/move_group/planning_pipelines/stomp/panda_arm/group_name
/move_group/planning_pipelines/stomp/panda_arm/optimization/control_cost_weight
/move_group/planning_pipelines/stomp/panda_arm/optimization/initialization_method
/move_group/planning_pipelines/stomp/panda_arm/optimization/max_rollouts
/move_group/planning_pipelines/stomp/panda_arm/optimization/num_iterations
/move_group/planning_pipelines/stomp/panda_arm/optimization/num_iterations_after_valid
/move_group/planning_pipelines/stomp/panda_arm/optimization/num_rollouts
/move_group/planning_pipelines/stomp/panda_arm/optimization/num_timesteps
/move_group/planning_pipelines/stomp/panda_arm/task/cost_functions
/move_group/planning_pipelines/stomp/panda_arm/task/noise_generator
/move_group/planning_pipelines/stomp/panda_arm/task/noisy_filters
/move_group/planning_pipelines/stomp/panda_arm/task/update_filters
/move_group/planning_pipelines/stomp/planning_plugin
/move_group/planning_pipelines/stomp/request_adapters
/move_group/planning_pipelines/stomp/start_state_max_bounds_error
</code>```

Moreover, this param is not empty and contains long values as shown below
```<code>$ rosparam get /move_group/planning_pipelines/stomp | wc -l
115
</code>```

The similar question has was asked here but no response.

答:
Based on the several responses on the ticket upstream I suspect that there's a bug in the configuration that you're trying to launch. https://github.com/ros-planning/panda_moveit_config/issues/131 Most likely there was a change in beahvior over time such that the full name or type of the parameter changed and the config file needs to be adjusted.

I'm writing this as an answer as I don't think that we can effectively provide you with more information that will do more than guess at things that could help you debug. The main thing is to follow up on that ticket and since that ticket hasn't been responded to in a while, you'll likely need to dig into it yourself.


问:
What is the most suitable sensor fusion filter for my application?
I am currently working on a hobby project involving a device that will be attached to a golf driver club. Its primary function is to continuously measure the angles (pitch, roll, yaw) throughout the swing. This will allow me to monitor specific parameters, such as pitch at 60 degrees, roll at 15 degrees, and yaw at 10 degrees.

For this purpose, I am currently utilizing an accelerometer and gyroscope sensor (LSM6DSR) and contemplating the addition of a magnetometer (LIS3MDL).

Various fusion algorithms can be employed to combine the data from each sensor, including the Complementary filter, Kalman Filter, Extended Kalman Filter, Mahony Filter, and Madgwick Filter, among others.

However, my initial attempt with a simple complementary filter proved inadequate, as even minor vibrations caused the angles to become highly unpredictable.

Hence, my first question is: which filter would be best suited for my application? It is worth noting that I am utilizing an STM32 microcontroller with a clock frequency of 72MHz and a Floating Point Unit (FPU), enabling me to perform complex calculations during the swing if necessary.

Moving on to my second question, regarding the calculation of the pitch: can the accelerometer and gyroscope alone provide accurate measurements, or should I consider incorporating magnetometer values as well? Alternatively, is the magnetometer primarily utilized for yaw calculations?

答:
In my experience, The Madgwick Filter is one of the fastest ways to implement an AHRS system, as there are many open source implementations and requires very little tuning effort.

If your IMU is cheap, you could consider fusing the magnetometer data for heading correction but it must be properly calibrated, especially if the IMU is placed nearby iron materials (the golf club?) or undergo heavy magnetic field distortions.


问:
How do you calculate this integral term in this PI Controller Formula?
This formula is the PI Control given in Eqn. 11.6, Pg. 419 of Chapter 11 in book Modern Robotics by Kevin M Lynch and Frank C Park.

Here,
Vb is the twist ==> Vb = (angular velocity, linear velocity)       ===> (6, 1) Matrix

X  is the SE(3) representation consisting of (Rotation, Position)  ===> (4, 4) Matrix

(Xe is the error term)

This book heavily uses Screw Theory, Product of Exponentials and Lie Algebra.

Can anyone tell me how to integrate the integral term ? What is the formula to integrate the SE(3) Matrix Xe(t) ?

答:
The term $X_e$ is not a matrix in SE(3) but a twist, as defined in the paragraph following the equation where it states that "... the configuration error $X_e(t)$ is not simply $X_d(t)-X(t)$, since it does not make sense to subtract elements of SE(3)."

It's actually the twist that, if followed for a unit time step, would shift $X$ to $X_d$. As stated in the book at the end of the paragraph: "The se(3) representation of this twist, expressed in the end-effector frame, is $[X_e]=log(X^{-1}X_d)$."

Now to answer your question: To calculate the integral term, you simply need to integrate the twist. Since it is a coordinate vector with 6 elements, it is directly integrable over time.


问:
How are exoskeletons controlled?
It is my understanding that there are 3 fundamental control methods : velocity, position and torque.

In exoskeletons meant to augment an healthy user's strength, a typical control method used is sensitivity amplification control, more specifically torque amplification. I am currently selecting a motor controller and trying to build a control framework for an exoskeleton that would use such a system.

According to the research that I have done, an user applied force would be detected by a sensor and the movement would then be assisted by an actuator according to this relation : (amplification ratio * detected force). In this scenario, is it fair to assume that velocity and position control are not needed?

答:
To start off I think that it's best to clarify that position, velocity, and torque control are not necessarily fundamental types of control, but common types of control mechanisms.

There's already a lot of good resources related to that such as Position Control vs Velocity Control vs Torque Control

A control algorithm's purpose is to bring the state of a system to it's goal state. If you measure the torque and control the torque only, any errors in the system will accumulate and the system's kinematics will diverge to the point of being useless. This if more of an amplifier than a control method. Typically if you're controlling something such as an exoskeleton and you want force/torque controls, they will be operating at the same time as the position controls or as an integrated control algorithm. This will make sure that the state of the controlled system continuously match that of the remote operation control.

In the case of an exoskeleton it would be possible to rely on the fact that the two systems are collocated and simply apply torque amplification of the measured arm torques to the analogous joints. This clearly has some safety concerns raised in the comments. Before embarking on controlling the whole or much of the exoskeleton I'd suggest that you start by testing out different methods on single link joints on a test bench.

Title: How are exoskeletons controlled?


问:
kinetic and potential energy of 3 DOF robot having 2 cylindrical links
Assume two uniform cylindrical links robot has 3 DOF as shown in the figure. Where link one has 1 dof that is rotation on its main axis. The second link has 2 DOF yaw and pitch movements. I can calculate forward kinematic, Jacobian, kinetic and potential energy of it using below code. However, I am not sure how can I verify that the computed kinetic and potential energy equations are correct? Moreover, the computed equations seem not right, for example if we check PE for q2 joint (which is m2g((r2_0(1,1))) becomes zero with this code. However, we know it should be equal to somewhat like m2*g(L2/2)*sinq2.

It would be great if someone can help me in this. Thank you in advance.

DH parameters of 3 DOF robot

```<code>% DH parameters
% theta, d, a, alpha
dh_params = [q1,       0,  0,     0;
             q2,      L1,  0,  pi/2;
             q3+pi/2,  0,  0,  pi/2;
             0,        0, L2,    0];
</code>```

homogeneous transformation matrices

```<code>% Define homogeneous transformation matrices
T = sym(zeros(4,4,4));     %T = sym(zeros(4,4,2));
for i=1:4
    T(:,:,i) = [cos(dh_params(i,1)), -sin(dh_params(i,1))*cos(dh_params(i,4)), sin(dh_params(i,1))*sin(dh_params(i,4)), dh_params(i,3)*cos(dh_params(i,1));
                sin(dh_params(i,1)), cos(dh_params(i,1))*cos(dh_params(i,4)), -cos(dh_params(i,1))*sin(dh_params(i,4)), dh_params(i,3)*sin(dh_params(i,1));
                0, sin(dh_params(i,4)), cos(dh_params(i,4)), dh_params(i,2);
                0, 0, 0, 1];
end

% Calculate forward kinematics
T01 = simplify(T(:,:,1));
T12 = simplify(T(:,:,2));
T02 = simplify(T01*T(:,:,2));
T23 = simplify(T(:,:,3));
T03 = simplify(T02*T(:,:,3));
T34 = simplify(T(:,:,4));
T04 = simplify(T03*T(:,:,4));
</code>```

Then

```<code>px = T04(1,4);
py = T04(2,4);
pz = T04(3,4);

%ri_i represents the position of the mass center of link i relative to its own coordinate frame
r1_1 = [0; 0; L1/2];
r2_2 = [0; L2/2; 0];   %r2_2 = [0; -L2/2; 0]; may be also
r3_3 = [L2/2; 0; 0];

%ri_0 is the distance of ith link mass center to base coordinates frame
r1_0 = simplify(T01 * [r1_1; 1]);
r2_0 = simplify(T02 * [r2_2; 1]);
r3_0 = simplify(T03 * [r3_3; 1]);
</code>```

Calculate Jacobian matrix

```<code>J = simplify([diff(px, q1), diff(px, q2), diff(px, q3), diff(px, q4); diff(py, q1), diff(py, q2), diff(py, q3), diff(py, q4); diff(pz, q1), diff(pz, q2), diff(pz, q3), diff(pz, q4)]);
</code>```

Output:

```<code>J =
[ L2*sin(q1 + q2)*sin(q3),  L2*sin(q1 + q2)*sin(q3), -L2*cos(q1 + q2)*cos(q3), 0]
[-L2*cos(q1 + q2)*sin(q3), -L2*cos(q1 + q2)*sin(q3), -L2*sin(q1 + q2)*cos(q3), 0]
[0,                                               0,              -L2*sin(q3), 0]
</code>```

Calculate Kinetic energy

```<code>% Calculate kinetic energy
KE = simplify(0.5*(m1*(diff(px, q1)^2 + diff(py, q1)^2) + m2*(diff(px, q1)^2 + diff(py, q1)^2 + diff(px, q2)^2 + diff(py, q2)^2) ...
    + m2*(diff(px, q1)^2 + diff(py, q1)^2 + diff(px, q2)^2 + diff(py, q2)^2) + diff(px, q3)^2 + diff(py, q3)^2)...
    + I1*dq1^2 + I2*(dq1 + dq2)^2 + I3*(dq1 + dq2 +dq3)^2);
</code>```

Calculate potential energy

```<code>% Calculate potential energy
PE = simplify(m1*g*(r1_0(1,1)) + m2*g*((r2_0(1,1))+m2*g*((r3_0(1,1)))));
</code>```

PE output:

```<code>PE =-(L2*g^2*m2^2*cos(q1 + q2)*sin(q3))/2
</code>```

答:
Welcome to Robotics, DANAISH! I'm sorry I don't have access to Matlab anymore, so I'm not able to follow along with the symbolic math you've posted here. I'm assuming you've double-checked your math for the DH parameters and creating the 4x4 transforms from the DH parameters, so I'm looking for things that could be wrong elsewhere in your work.

In looking at it from that perspective, I see you making transforms like:

```<code>T01 = simplify(T(:,:,1));
</code>```

Which looks like the transform from frame 0 to frame 1. Then, when I see:

```<code>T02 = simplify(T01*T(:,:,2));
</code>```

I think you're using the transform from frame 0 to frame 1 times the transform from frame 1 to frame 2 to get the transform from frame 0 to frame 2.

In looking at this though, later you have:

```<code>%ri_i represents the position of the mass center of link i relative to its own coordinate frame
r1_1 = [0; 0; L1/2];
</code>```

so r1_1 is the center of mass of L1 in the F1 frame. That's fine, but then if you have:

```<code>%ri_0 is the distance of ith link mass center to base coordinates frame
r1_0 = simplify(T01 * [r1_1; 1]);
</code>```

I think this is incorrect because now it looks to me like you're using T01, the transform FROM frame 0 TO frame 1 to convert r1_1, which is the center of mass as defined in frame 1.

I don't think you're applying the transforms correctly here, I think you should be converting r1_1 from 1 to 0, meaning I think you should be doing:

```<code>r1_0 = simplify(T10 * [r1_1; 1]);
</code>```

with the transform T10 being the inverse of T01:

```<code>T10 = T01^(-1);
</code>```

Another problem you're going to have though is that you've got a degenerate system, in that there's no link for Frame 2. You have Frame 2 go directly into Frame 3, and then there's a definition for the center of mass for L2 with respect to Frame 3. You seem to be guessing about what the definition is for Frame 2's link mass:

```<code>r2_2 = [0; L2/2; 0];   %r2_2 = [0; -L2/2; 0]; may be also
</code>```

The way it's defined, for a right-handed convention, the position would be L2/2, but it's also dependent on the position of q3, so (as drawn, if it's drawn for all the joints in the zero position) more like (L2/2)*cos(q3), because at q3=0 you have the offset at L2/2, but if q3 rotates 90 degrees then you have the center of mass for L2 along Frame 2's z-axis.

Finally I'll caution you that your rotation conventions aren't all right-handed, but you don't reflect the polarity difference in your DH table. If you point your right thumb along the z-axis (or any rotation axis) then a positive rotation is the direction your fingers curl to make a fist. This means that your q1 rotation is left-handed, q2 rotation is right-handed, and q3 rotation is left-handed. If you want to get the motions as indicated then you can throw some negative signs in the DH table to account for the left-handed q1 and q3 rotations.

In looking at your potential energy result, it looks wrong to me not because of q2, because I'm not smart enough to intuit what that impact should be, but because of q3. If L2 is a cylindrical link, then q3 has no impact at all on the potential energy because there is nothing q3 can do to move the center of mass of L2. Joint q2 can move the center of mass of L2, and because q1 can move q2, it also has some impact on the potential energy. q3's rotation is concentric about the center of mass though, so q3 should work its way out of the potential energy equation, but in the output provided the potential energy becomes zero if q3 is zero, regardless of the values of q1 or q2, which is plainly incorrect.

Body:
Welcome to Robotics, DANAISH! I'm sorry I don't have access to Matlab anymore, so I'm not able to follow along with the symbolic math you've posted here. I'm assuming you've double-checked your math for the DH parameters and creating the 4x4 transforms from the DH parameters, so I'm looking for things that could be wrong elsewhere in your work.

In looking at it from that perspective, I see you making transforms like:

T01 = simplify(T(:,:,1));

Which looks like the transform from frame 0 to frame 1. Then, when I see:

T02 = simplify(T01*T(:,:,2));

I think you're using the transform from frame 0 to frame 1 times the transform from frame 1 to frame 2 to get the transform from frame 0 to frame 2.

In looking at this though, later you have:

%ri_i represents the position of the mass center of link i relative to its own coordinate frame
r1_1 = [0; 0; L1/2];

so r1_1 is the center of mass of L1 in the F1 frame. That's fine, but then if you have:

%ri_0 is the distance of ith link mass center to base coordinates frame
r1_0 = simplify(T01 * [r1_1; 1]);

I think this is incorrect because now it looks to me like you're using T01, the transform FROM frame 0 TO frame 1 to convert r1_1, which is the center of mass as defined in frame 1.

I don't think you're applying the transforms correctly here, I think you should be converting r1_1 from 1 to 0, meaning I think you should be doing:

r1_0 = simplify(T10 * [r1_1; 1]);

with the transform T10 being the inverse of T01:

T10 = T01^(-1);

Another problem you're going to have though is that you've got a degenerate system, in that there's no link for Frame 2. You have Frame 2 go directly into Frame 3, and then there's a definition for the center of mass for L2 with respect to Frame 3. You seem to be guessing about what the definition is for Frame 2's link mass:

r2_2 = [0; L2/2; 0];   %r2_2 = [0; -L2/2; 0]; may be also

The way it's defined, for a right-handed convention, the position would be L2/2, but it's also dependent on the position of q3, so (as drawn, if it's drawn for all the joints in the zero position) more like (L2/2)*cos(q3), because at q3=0 you have the offset at L2/2, but if q3 rotates 90 degrees then you have the center of mass for L2 along Frame 2's z-axis.

Finally I'll caution you that your rotation conventions aren't all right-handed, but you don't reflect the polarity difference in your DH table. If you point your right thumb along the z-axis (or any rotation axis) then a positive rotation is the direction your fingers curl to make a fist. This means that your q1 rotation is left-handed, q2 rotation is right-handed, and q3 rotation is left-handed. If you want to get the motions as indicated then you can throw some negative signs in the DH table to account for the left-handed q1 and q3 rotations.

In looking at your potential energy result, it looks wrong to me not because of q2, because I'm not smart enough to intuit what that impact should be, but because of q3. If L2 is a cylindrical link, then q3 has no impact at all on the potential energy because there is nothing q3 can do to move the center of mass of L2. Joint q2 can move the center of mass of L2, and because q1 can move q2, it also has some impact on the potential energy. q3's rotation is concentric about the center of mass though, so q3 should work its way out of the potential energy equation, but in the output provided the potential energy becomes zero if q3 is zero, regardless of the values of q1 or q2, which is plainly incorrect.


问:
Triggering Only Error Messages with ROS 2 service call to /rosbag2_recorder/snapshot
I have been using the ROS 2 rosbag2_recorder package to record message data for analysis and testing purposes. Recently, I've been exploring the possibility of capturing only error messages in my bag files.

I noticed that the /rosbag2_recorder/snapshot service provided by the package allows me to take a snapshot of the ROS 2 system's message data at a particular moment. However, I would like to know if it is possible to configure this service call in a way that it captures only error messages.

I am aware that the rosbag2_interfaces/Snapshot service request message has parameters like topics and max_cache_size, but I couldn't find any specific option or flag related to capturing only error messages.

Could someone please guide me on how I can modify the rosbag2_recorder service call to capture only error messages? Are there any additional parameters or configurations that can be used for this purpose? Or is there an alternative approach to achieve this goal?

details about snapshot: here

Any suggestions, insights, or examples would be greatly appreciated.

Thank you in advance for your help!

答:
Welcome to Robotics, Dragon. You're mentioning wanting to make a rosbag, but then you also mention wanting to "capture error messages." Error messages are logged, and it's not super clear to me what you're trying to do, so my first suggestion would be that you investigate logging options and see if a log file is sufficient for your needs.

If you need to have logs included with your rosbag, you could also add the /rosout_agg topic to be included with your bagfile and you'd have all of the logs included.

If you need only the error messages and you need them included in the bagfile, then you could consider writing a re-publisher that checks the log level from messages published to /rosout_agg and then, if they're at or above ERROR, write them to a new topic like /rosout_errors. Then you can add /rosout_errors to your list of topics for the rosbag recording.

You're mentioning ROS2, which I'm not currently using, but for Python and ROS this would look like the following:

```<code>import rospy
from rosgraph_msgs.msg import Log

def callback(data):
    if data.level >= Log.ERROR:
        pub.publish(data)

pub = rospy.Publisher('rosout_errors', Log, queue_size=10)
rospy.init_node('rosout_republisher', anonymous=True)
rospy.Subscriber("rosout_agg", Log, callback)
rospy.spin()
</code>```

I believe the primary difference you'd have to make here is that in ROS2 the Log message is in rcl_interfaces instead of rosgraph_msgs, but again I don't have ROS2 and can't verify that. The code I posted above works for ROS.

Body:
Welcome to Robotics, Dragon. You're mentioning wanting to make a rosbag, but then you also mention wanting to "capture error messages." Error messages are logged, and it's not super clear to me what you're trying to do, so my first suggestion would be that you investigate logging options and see if a log file is sufficient for your needs.

If you need to have logs included with your rosbag, you could also add the /rosout_agg topic to be included with your bagfile and you'd have all of the logs included.

If you need only the error messages and you need them included in the bagfile, then you could consider writing a re-publisher that checks the log level from messages published to /rosout_agg and then, if they're at or above ERROR, write them to a new topic like /rosout_errors. Then you can add /rosout_errors to your list of topics for the rosbag recording.

You're mentioning ROS2, which I'm not currently using, but for Python and ROS this would look like the following:

import rospy
from rosgraph_msgs.msg import Log

def callback(data):
    if data.level >= Log.ERROR:
        pub.publish(data)

pub = rospy.Publisher('rosout_errors', Log, queue_size=10)
rospy.init_node('rosout_republisher', anonymous=True)
rospy.Subscriber("rosout_agg", Log, callback)
rospy.spin()

I believe the primary difference you'd have to make here is that in ROS2 the Log message is in rcl_interfaces instead of rosgraph_msgs, but again I don't have ROS2 and can't verify that. The code I posted above works for ROS.


问:
generate ros2 interfaces (messages, services) directly in Python?
Is there a way to create ros2 interfaces (messages, services) directly from Python, without going through translating .msg and .srv files?</p>

答:
You can, but you shouldn't. When ROS message, service, and action files are built they are simply turned into a class. When creating a publisher it is simply expecting a reference to a class. Take the following example:

```<code>from rclpy.node import Node

class SomeMessage:
    some_field = 1
    other_field = 2

basic_node = Node('some_node')
basic_node.create_publisher(SomeMessage, '/some_topic')
</code>```

This is perfectly valid and you can publish messages like this. The main issue is that this message definition is local to the script you create it in. Any subscriber would have to either reach out and import from this script, or redefine the message.

Body:
You can, but you shouldn't. When ROS message, service, and action files are built they are simply turned into a class. When creating a publisher it is simply expecting a reference to a class. Take the following example:

from rclpy.node import Node

class SomeMessage:
    some_field = 1
    other_field = 2

basic_node = Node('some_node')
basic_node.create_publisher(SomeMessage, '/some_topic')

This is perfectly valid and you can publish messages like this. The main issue is that this message definition is local to the script you create it in. Any subscriber would have to either reach out and import from this script, or redefine the message.


问:
rcli tools uses rostime or system(wall-time)?
I'm trying to collect data from ros2 commands: ros2 topic hz & ros2 topic bw.
I'm able to get data perfectly for any topic. But i just want to know whether these rcli tools or commands mentioned above uses rostime or system (wall-time) in their calculation?

If answer to above question is rostime. Then how can I get that specific "rostime at which the frequency or bandwidth" are calculated?

Thanks

答:
ROS2 CLI tools such as hz that you mentioned supports using ros time and wall time. That said, for current releases it will default to using ros time unless wall time is specified.

The time itself is provided via the Node class and the clock it uses can be retrieved with get_clock(). See the following example:

```<code>from rclpy.node import Node
some_node = Node(“NodeName”)
node_clock = some_node.get_clock()
</code>```

Body:
ROS2 CLI tools such as hz that you mentioned supports using ros time and wall time. That said, for current releases it will default to using ros time unless wall time is specified.

The time itself is provided via the Node class and the clock it uses can be retrieved with get_clock(). See the following example:

from rclpy.node import Node
some_node = Node(“NodeName”)
node_clock = some_node.get_clock()


问:
Replacing AX-12A motors with AX-18A motors causing issues with ROS Arbotix
I'm currently working on a project with an oculomotor robot head interfaced with ROS and Arbotix-M (https://github.com/combra-lab/spiking-oculomotor-head-control/tree/main). The robot head comprises two eyes, each controlled by two motors (one for horizontal and the other for lateral movement). Additionally, there are two motors for the vertical movement of the neck and one for its horizontal movement, making a total of seven AX-12A motors(https://emanual.robotis.com/docs/en/dxl/ax/ax-12a/). All motors are servo motors.

Recently, two of the AX-12A motors got damaged and I had to replace them with AX-18A motors(https://emanual.robotis.com/docs/en/dxl/ax/ax-12a/). However, after the replacement, the robot head does not move at all and no motors move.

I believe the issue isn't related to the neural network code, as no changes were made there. Rather, I suspect it might be with the ROS Arbotix configuration or the uploaded Arduino code. The ros package I uploaded to the Arduino uses the ax12.h library (https://github.com/Interbotix/arbotix/tree/master/libraries/Bioloid) along with four other files (https://github.com/vanadiumlabs/arbotix_ros/tree/noetic-devel/arbotix_firmware/src). These four files are Arbotix sketches that need to be uploaded to Arduino.

Has anyone else faced a similar issue when replacing AX-12A with AX-18A motors? Could this be a compatibility issue between AX-18A motors and the ax12.h library? Or could there be some specific ROS Arbotix configurations needed for AX-18A motors? I couldn't find any ax18.h file.

I would appreciate any insights or advice on how to solve this problem. Thanks in advance!

答:
Thanks for the update! Things I'd look at for troubleshooting:

If I can take some excerpts from your problem statement, you say:

a total of seven AX-12A motors [...] Recently, two of the AX-12A motors got damaged and I had to replace them with AX-18A motors. However, after the replacement, the robot head does not move at all and no motors move.

If I'm understanding this correctly, you had 7 motors, replaced two of them, and now nothing works. If this is correct then it's almost certainly unrelated to the new motors. To be sure, I would unplug the new motors and see if you can get the original motors to respond. Whatever damaged the two motors that got replaced could have caused some other failure, and the new replacement motors are a red herring and not actually related.

In looking at the datasheet for the AX-12A and AX-18A (thanks for that!) it looks like the communication specs are basically the same, so I would expect the new motors to be a drop-in replacement for the old motors. The only item I'd caution you about here are the addresses. If you unplug the new motors and the originals work, then everything stops working when you plug in the new motors, then you may have address conflicts. From the datasheet:

2.4 Control Table Description

2.4.3 ID (3)

The ID is a unique value in the network to identify each DYNAMIXEL with an Instruction Packet. 0~253 (0xFD) values can be used as an ID, and 254(0xFE) is occupied as a broadcast ID. The Broadcast ID(254, 0xFE) can send an Instruction Packet to all connected DYNAMIXEL simultaneously.

NOTE : Please avoid using an identical ID for multiple DYNAMIXEL. You may face communication failure or may not be able to detect DYNAMIXEL with an identical ID.

(emphasis added)

If you put the new motors in without first setting their addresses then I would expect some conflicts. Probably the two new motors have the same default addresses and they're conflicting with each other, so I'd expect the other five/original motors to work fine and the new motors to conflict if this is the case, but if one of the five original motors also has the same default address then they might all be conflicting. In any event, I would expect some of the original motors to function as expected if it's a device ID issue.

If you unplug the new motors and the old ones still don't work then it's probably not the new motors. Did a ribbon cable get pulled out? Is the battery low? Inrush current from trying to start a motor can cause a circuit to brown out if the battery is low, but I would expect jitter, resets, etc. if this is the case and not just a failure to respond.

I think you'll discover something if you work through these steps, but if you do and you're still stuck then please edit your question again with the steps you took and what you saw!


答:
Kinetic Energies

In general, the kinetic energy of a link will be:

$$K = \frac{1}{2}mv^2 + \frac{1}{2}I\omega^2$$

For the link of length $l_1$:

$$K_1 = \frac{1}{2}m_1{v_1}^2 + \frac{1}{2}I_1{\omega_1}^2$$

with

$$v_1 = \left|
\frac{1}{2}{\bf r_1}{\dot{\theta}}_1
\right|$$

$${\bf r_1} = l_1\left(\hat{x}\cos\theta_1+\hat{y}\sin\theta_1\right)$$

$$I_1=\frac{1}{12}m_1{l_1}^2$$

$$\omega_1 = {\dot{\theta}}_1$$

For the link of length $l_2$:

$$K_2 = \frac{1}{2}m_2{v_2}^2 + \frac{1}{2}I_2{\omega_2}^2$$

with

$$v_2 = \left|
{\bf r_1}{\dot{\theta}}_1 +
\frac{1}{2}{\bf r_2}\left({\dot{\theta}}_1+{\dot{\theta}}_1\right)
\right|$$

$${\bf r_2} = l_2\left[\hat{x}\cos\left(\theta_1+\theta_2\right)+
\hat{y}\sin\left(\theta_1+\theta_2\right)\right]$$

$$I_2=\frac{1}{12}m_2{l_2}^2$$

$$\omega_2 = {\dot{\theta}}_2$$

Potential Energies

$$P_1 = \frac{1}{2}m_1l_1g\sin\theta_1$$

$$P_2 = m_2\left(
l_1 + \frac{1}{2}l_2
\right)
g
\sin(\theta_1+\theta_2)
$$

Title: kinetic and potential energy of 3 DOF robot having 2 cylindrical links

Body:
Assume two uniform cylindrical links robot has 3 DOF as shown in the figure. Where link one has 1 dof that is rotation on its main axis. The second link has 2 DOF yaw and pitch movements. I can calculate forward kinematic, Jacobian, kinetic and potential energy of it using below code. However, I am not sure how can I verify that the computed kinetic and potential energy equations are correct? Moreover, the computed equations seem not right, for example if we check PE for q2 joint (which is m2g((r2_0(1,1))) becomes zero with this code. However, we know it should be equal to somewhat like m2*g(L2/2)*sinq2.

It would be great if someone can help me in this. Thank you in advance.

DH parameters of 3 DOF robot

```<code>% DH parameters
% theta, d, a, alpha
dh_params = [q1,       0,  0,     0;
             q2,      L1,  0,  pi/2;
             q3+pi/2,  0,  0,  pi/2;
             0,        0, L2,    0];
</code>```

homogeneous transformation matrices

```<code>% Define homogeneous transformation matrices
T = sym(zeros(4,4,4));     %T = sym(zeros(4,4,2));
for i=1:4
    T(:,:,i) = [cos(dh_params(i,1)), -sin(dh_params(i,1))*cos(dh_params(i,4)), sin(dh_params(i,1))*sin(dh_params(i,4)), dh_params(i,3)*cos(dh_params(i,1));
                sin(dh_params(i,1)), cos(dh_params(i,1))*cos(dh_params(i,4)), -cos(dh_params(i,1))*sin(dh_params(i,4)), dh_params(i,3)*sin(dh_params(i,1));
                0, sin(dh_params(i,4)), cos(dh_params(i,4)), dh_params(i,2);
                0, 0, 0, 1];
end

% Calculate forward kinematics
T01 = simplify(T(:,:,1));
T12 = simplify(T(:,:,2));
T02 = simplify(T01*T(:,:,2));
T23 = simplify(T(:,:,3));
T03 = simplify(T02*T(:,:,3));
T34 = simplify(T(:,:,4));
T04 = simplify(T03*T(:,:,4));
</code>```

Then

```<code>px = T04(1,4);
py = T04(2,4);
pz = T04(3,4);

%ri_i represents the position of the mass center of link i relative to its own coordinate frame
r1_1 = [0; 0; L1/2];
r2_2 = [0; L2/2; 0];   %r2_2 = [0; -L2/2; 0]; may be also
r3_3 = [L2/2; 0; 0];

%ri_0 is the distance of ith link mass center to base coordinates frame
r1_0 = simplify(T01 * [r1_1; 1]);
r2_0 = simplify(T02 * [r2_2; 1]);
r3_0 = simplify(T03 * [r3_3; 1]);
</code>```

Calculate Jacobian matrix

```<code>J = simplify([diff(px, q1), diff(px, q2), diff(px, q3), diff(px, q4); diff(py, q1), diff(py, q2), diff(py, q3), diff(py, q4); diff(pz, q1), diff(pz, q2), diff(pz, q3), diff(pz, q4)]);
</code>```

Output:

```<code>J =
[ L2*sin(q1 + q2)*sin(q3),  L2*sin(q1 + q2)*sin(q3), -L2*cos(q1 + q2)*cos(q3), 0]
[-L2*cos(q1 + q2)*sin(q3), -L2*cos(q1 + q2)*sin(q3), -L2*sin(q1 + q2)*cos(q3), 0]
[0,                                               0,              -L2*sin(q3), 0]
</code>```

Calculate Kinetic energy

```<code>% Calculate kinetic energy
KE = simplify(0.5*(m1*(diff(px, q1)^2 + diff(py, q1)^2) + m2*(diff(px, q1)^2 + diff(py, q1)^2 + diff(px, q2)^2 + diff(py, q2)^2) ...
    + m2*(diff(px, q1)^2 + diff(py, q1)^2 + diff(px, q2)^2 + diff(py, q2)^2) + diff(px, q3)^2 + diff(py, q3)^2)...
    + I1*dq1^2 + I2*(dq1 + dq2)^2 + I3*(dq1 + dq2 +dq3)^2);
</code>```

Calculate potential energy

```<code>% Calculate potential energy
PE = simplify(m1*g*(r1_0(1,1)) + m2*g*((r2_0(1,1))+m2*g*((r3_0(1,1)))));
</code>```

PE output:

```<code>PE =-(L2*g^2*m2^2*cos(q1 + q2)*sin(q3))/2
</code>```


问:
How to resolve 'nullptr not declared in this scope' compiling error in Ros_lib blink example and other ros_lib examples Ubuntu 20.04 with ROS Noetic?
I am new to ros and I am using virtual machine with Ubuntu 20.04 and ros neotic in it. I have installed rosserial arduino package step by step from this.

After this I open ros_lib blink example and it shows the following error:

```<code>/home/rospc/sketchbook/libraries/ros_lib/ros/node_handle.h:121:45: error: ‘nullptr’ was not declared in this scope
   Publisher * publishers[MAX_PUBLISHERS] = {nullptr};
                                             ^
/home/rospc/sketchbook/libraries/ros_lib/ros/node_handle.h:122:47: error: ‘nullptr’ was not declared in this scope
   Subscriber_ * subscribers[MAX_SUBSCRIBERS] {nullptr};
here is the verbose output
In file included from Blink.pde:7:0:
/home/rospc/sketchbook/libraries/ros_lib/std_msgs/Empty.h:20:53: warning: override controls (override/final) only available with -std=c++11 or -std=gnu++11
     virtual int serialize(unsigned char *outbuffer) const override

</code>```

And other examples despite of ros_lib examples are successfully compiled and uploaded to the arduino board. And this error message has shown on all ros_lib examples.

答:
You have to add add_compile_options(-std=c++11) or set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") to your CMakeLists.txt (assuming you installed an Arduino IDE version with a compiler supporting C++11).

The project doesn't specify a C++ version, so one of the "latest" commits destroyed the examples if the gcc used doesn't define C++11 or newer as default.


问:
writing a simple program for processing RGBD video with OpenNI
I would like to write a simple program which processes the depth feed from an Asus xtion depth sensor using OpenNI. The sensor will be fixed like a CCTV camera and count multiple targets moving around.

The processing would involve some simple functions on each frame (background subtraction, level sets, connected components filter), and some multi-target tracking across frames. I have searched the web, but it is hard to see how best to get started (and I'm also quite new to programming in C).

Can anyone recommend any existing code that can help to get started / any libraries which would be suitable for this real-time application? Or perhaps there is some opensource code which already does such a thing? Would really appreciate any pointers from anyone with experience. Thanks!

答:
I work with ASUS Xtion as well. The best library that I found so far and I am using daily is Point Cloud Library (PCL). You can find, download, and Install it from here.

After installing and getting it to work, you can go to the tutorial section of the website, and there is a very good tutorial similar to what you want. There is a pcl_people module that you can start from.

This tutorial aims at explaining how to detect people from RGB-D data with the pcl_people module. With the proposed method, people standing/walking on a planar ground plane can be detected in real time with standard CPU computation. This implementation corresponds to a people detection algorithm for RGB-D data.


问:
What's the most accurate way to obtain a locational fix using GPS?
Obviously GPS is the most obvious and accessible technology for obtaining a locational "fix" for a robot at any particular time. However, while it's great sometimes, in other locations and situations it's not as accurate as I'd like, so I'm investigating whether there's a relatively easy way to improve on this accuracy (or not, if that turns out to be the case.)

I've considered the following options, but found limited information online:

Would using a much better antenna help, especially for low signal areas? I'm thinking yes to this, but if so how would I construct such an antenna and know that it's an improvement? Are there any good guides on how to do this? I could use a ready made antenna if they're not too expensive.

Would using multiple separate receivers in tandem help, or would they likely all be off by a similar amount, or would I not be able to extract a meaningful average with this approach?

What sort of characteristics should I look for when choosing a good GPS receiver to help accuracy?

Is there anything else I should consider which I've missed?

答:
I actually work as a programmer for the tractors that bit-pirate mentioned.

There are several ways mentioned that can get you more accurate results. It depends largely on your application and what you are trying to accomplish.

WASS uses a signal to augment not as accurate as some other methods but you will get decent.

If your platform is fixed you can just average the position over time and get good results.

RTK is very accurate, and the library mentioned by Jakob looks pretty cool. Some states like Iowa have a government run RTK system that you can use.

I found this the other day for dead reckoning.

On a mobile system the antennae offset to the center of gravity will effect the accuracy. E.G. On a steep hill the GPS antennae will be off center.

What are you trying to accomplish? The atmospheric distortion for multiple antennas in the same location would be very similar.


问:
Can motion model noise be zero?
Can I assume the noise of motion model to be zero? If so, what are the consequences of doing so?</p>

答:
I'm assuming this is with respect to a Kalman filter?

Mathematically, yes it can be zero. The effect of this is that model is assumed to be perfect and estimation uncertainty is due 100% to the uncertainty in the initial state. In the extreme case, if you assume 0 initial uncertainty you will never have any model uncertainty and all your measurements will be ignored.

Even assuming non-zero initial uncertainty the behavior will probably not be what is expected. Consider a simple position+velocity state vector. Over time, the position uncertainty will grow a little bit due to initial velocity uncertainty, but the velocity uncertainty will be constant. No matter how long you wait until the first measurement arrives the velocity estimate will have low uncertainty so not much weight will be given to the measurement.

So, all usage scenarios that I've seen have non-zero dynamic noise.

Title: Can motion model noise be zero?


问:
VeX - Keeping arm at an angle
So my team made a Vex robot for the toss-up competition, but we need the arm up during the autonomous. The problem is that it's too heavy to stay up on its own. I was going to use encoders to count what angle the arm is at. I was going to use this code, but I'm not sure if there's a better way.

```<code> while(MotorEncoder[rightMotor] < 1000)
  {
    motor[rightMotor] = 80;
    motor[leftMotor] = 80;
  }
</code>```

Would anyone recommend a better solution or is this the best way? This is untested by the way.

答:
Some clarifications before i answer.  when you say "its too heavy to stay up on its own" do you mean that the arm is too heavy for the motor, or the arm doesn't stay at an angle when the motor is not powered?  also, why are you controlling two motors, and reading only one encoder?  perhaps you should provide a better description of your arm.

that being said, i would use simple proportional control for this.  (proportional control being the "P" in what is more commonly called PID control).  what you have above is called "bang" control.

basically, you just set up a linear relationship between encoder readings and motor current.  (actually the difference between encoder readings of where the arm is and where you want it to be).  For example:

```<code> void set_arm_to_angle(int angle)
 {
     int P = 200;
     int error = angle - MotorEncoder[rightMotor];
     motor[rightMotor] = P * error
 }
</code>```

you should be able to find lots of info on proportional control on the web.  but basically, if the arm is lower than where you want, positive current is applied to raise the arm.  the farther away from your desired angle, the more current.  if the arm is too high, current is reversed.

it can be a little tricky figuring out the right gain (P).  too low, and the arm won't move, too high and the arm will oscillate.  i wouldn't worry about the I and D terms.  but adding some friction to the arm might help.

and if you want more info on more advanced robot arm topics, this page give a gentle overview: http://www.societyofrobots.com/robot_arm_tutorial.shtml

Title: VeX - Keeping arm at an angle

Body:
Some clarifications before i answer.  when you say "its too heavy to stay up on its own" do you mean that the arm is too heavy for the motor, or the arm doesn't stay at an angle when the motor is not powered?  also, why are you controlling two motors, and reading only one encoder?  perhaps you should provide a better description of your arm.

that being said, i would use simple proportional control for this.  (proportional control being the "P" in what is more commonly called PID control).  what you have above is called "bang" control.

basically, you just set up a linear relationship between encoder readings and motor current.  (actually the difference between encoder readings of where the arm is and where you want it to be).  For example:

 void set_arm_to_angle(int angle)
 {
     int P = 200;
     int error = angle - MotorEncoder[rightMotor];
     motor[rightMotor] = P * error
 }

you should be able to find lots of info on proportional control on the web.  but basically, if the arm is lower than where you want, positive current is applied to raise the arm.  the farther away from your desired angle, the more current.  if the arm is too high, current is reversed.

it can be a little tricky figuring out the right gain (P).  too low, and the arm won't move, too high and the arm will oscillate.  i wouldn't worry about the I and D terms.  but adding some friction to the arm might help.

and if you want more info on more advanced robot arm topics, this page give a gentle overview: http://www.societyofrobots.com/robot_arm_tutorial.shtml


问:
Getting started with robotic arm design
I would like to design a robotic arm to hold a weight X at length Y (in my case I want to hold X=2.5 lbs at Y = 4 inches). Starting out simply, I would like try building an arm with a gripper plus one servo joint.

[Servo Joint] ----- Y ------ [Gripper]

When designing an arm, would I want to say that the gripper has to have enough torque to hold the desired weight (e.g. 2.5 lbs) at a minimal distance (however long the fingers are) then design the servo joint to bear the weight of the gripper + the load?

I would like to be able to hold the object at full extension

答: this page gives a nice overview of joint torque requirements for robot arms.  and even provides a simple calculator. http://www.societyofrobots.com/robot_arm_tutorial.shtml</p>

Title: Getting started with robotic arm design


问:
How are color sensors used for line following?
I have to build a line following robot that will be able to detect a selected colored line on the floor and start following it.

How do color sensors do this after detecting the specific colored line?

答:
It might require a little bit of soldering to create such a color sensor, but you can probably handle it.

The easiest way to create a color sensor for such an application is by using a reflective optical sensor such as the following:
Optical Sensor

It is basically just a light detector and a LED. The light emitted from the LED gets reflected by the surface and hits the detector.

The amount of reflected light is dependent on many things, but mainly on the color of the surface. If the surface is black all the light will be absorbed and the detector won't see any light. If the surface is white the detector will see a lot of light and so on.

To create an linetracking sensor for the robot i suggest that you use several (e.g. 6) of these sensors and place them pointing downwards on the front bumper of the robot. You can now detect where the line is by looking at the individual sensors and detecting which one (or more) sees less or more light.

This will give you a relatively cheap sensor that should be sufficient for a line tracking robot.

The downside is however that it requires a bit of soldering. On google i have found the following diagram that you might use.


问:
Properly flashing the firmware on a Lego Mindstorms NXT
I am attempting to upload a custom firmware to a Lego Mindstorms NXT and am having issues.

First of all, I'm attempting to use nxtOSEK, which would let me run C++ programs on it.  The problem is, everytime I put it into firmware update mode, the download doesn't seem to actually occur.

What I mean by this is that, according to the output in my terminal (both Mac and Windows), the download was successful, however when the NXT reboots, I still see the normal logo (not nxtOSEK).

So, what I'm doing is first holding down the Reset button for a few seconds, then hitting the orange button, giving me that tic-tic-tic sound.  Then I run the firmware update (either using the Windows NextTool or Mac OSX GUI NextTool) and attempt the download.  I get a success message, yet the robot is still using the old firmware.

What could be the cause of this problem and how can I solve it?

答:
The reason why you can't see the nxtOSEK logo, is that the enhanced firmware looks the same as the regular firmware, but with several performance and feature enhancements. It isn't specific to nxtOSEK.

To check if the firmware installed properly, visit the Settings menu on the NXT, and then the NXT Version submenu. The top line should read: FW NBC/NXC x.xx, where x.xx is the version number of the firmware.


问:
Can inverse dynamics control be regarded as a function?
I know that inverse kinematics ($p \rightarrow q$, p: desired pose of the end-effector, q: joint angles) is not a function because there might be multiple joint angle vectors q that result in the same pose p.

By inverse dynamics control I mean the mapping $(q, \dot{q}, \ddot{q}) \rightarrow u$ (u: required torques. I am not very experienced with these kind of problems. Is the mapping a function, i.e. for each triple $(q, \dot{q}, \ddot{q})$ there is a unique solution u? My intuition says it is. But I am not sure. If there is not, would it always be possible to obtain a solution by averaging two or more solutions?

答:
I think you are confusing 2 issues.  Inverse dynamics is the process of mapping end effector position, velocity, and acceleration to joint torques.  as described in this book, page 298: http://books.google.com/books?id=jPCAFmE-logC&lpg=PR2&pg=PA298#v=onepage&q=inverse%20dynamics&f=false

But the paper you posted is simply modeling and calibrating their robot's non-geometric parameters.

So i think there can be multiple solutions to the inverse dynamics problem as i define above.  because when only given the end effector parameters, the arm can potentially be in different configurations to realize this.  a simple example is a 2 link planar arm where the elbow can be on either side.  as seen in figure 2.31, page 93: http://books.google.com/books?id=jPCAFmE-logC&lpg=PR2&pg=PA93#v=onepage&q=two-link%20planar%20arm&f=false

but i still think the problem as you describe, mapping joint position, velocity, and acceleration to joint torques is a low-level control problem and probably has a unique solution.  however, when factoring in nonlinearities like friction can probably make the answer non-unique.  for example, imagine a joint with lots of friction.  a range of joint torques will be sufficient to hold the joint at a given angle.


问:
Android phone + ADK + Arducopter APM 2.5 for autonomous quadcopter
for a project for a robotic lab, i'd like to build an automous quadcopter able to follow a path and land on its own. I'd like to use an onboard android phone to do the image processing and recognition part, so that I avoid to send the video stream to a control station, process it and send back the commands.

As I need to use it in an indoor environment (so no GPS coordinate), I need the phone to guide the quadcopter giving it relative directions like FORWARD and after 1 sec STOP. This is something a normal pilot would do via the RC radio.

I already have a arducopter APM 2.5 and an arduino mega ADK and I was thinking to connect the phone to the ADK and then the ADK to the APM to guide the copter.

I think I have 2 options: either having the ADK to generate PPM/PWM signals as a RC receiver would do or use the mavlink protocol.

Which is the best/easiest solution?

Other info:

-I have already read checked some UAV related websites, but I couldn't find something close to what I want. In most of them, try to build a new type of controller, or use only ab Android phone + ADK. I'd like to stick to something already tested and known to work (as the APM & arducopter software) and I don't want to use the phone as IMU as i don't trust its sensors

-I already have built the quad (a hexa actually)

-I have already set up the connection and protocol between the phone and the adk so i'm able to send commands like, i.e. forward, turn, hover etc...

-I have already checked the andro-copter project and similar ones.

-I might consider other platforms than the APM 2.5 if there's something easier to use

-It'd be nice to keep the RC receiver in the loop to regain control of the quad if something goes wrong.

答:
I have done something similar to this. I used a Samsung Galaxy onboard an APM2.6-based quadcopter. As a starting point, I used DroidPlanner and it was a pretty simple matter to use the same libraries it does. Then I used another Galaxy with a screen-mirroring application over wifi to run the app.

It's good that you realize the importance of keeping RC receiver control available -- this is important for safety. You should set up your code so that it runs in "auto" mode and have switches on your receiver set up so that you can change the flight mode back to stabilize easily.

It's worth noting that APM's mavlink implementation is very incomplete. It simply ignores some of the parameters of some of the commands. For example, when you send a waypoint, it ignores the yaw parameter. Here's a pull request (never got into trunk) I submitted that fixes this and might be useful for you if you want to modify APM's mavlink implementation. Still, I think this is probably a better solution than generating PWM with the ADK.