问:
Digital compass precision in urban environment
I'm working on an application to compute the absolute heading (yaw) using the following hardware: WIT-Motion HWT905 IMU https://www.wit-motion.com/digital-inclinometer/witmotion-hwt905-ttl.html. It can output a quaternion, and I made the transformations to set it in the North-East-Down (NED) reference frame. In my application, I use the IMU on the top of a car and drive it in an urban environment (city).

To determine the precision of the yaw estimation, I use ROS to record the camera attached close to the IMU and the IMU readings. Then, I use the saved data with a dense 3D map of the region and I align the corresponding view of the image with the rendered view of the map, I have a GPS too.

I noticed that the heading estimation has a weird behavior when I placed it close to the car's metal body. Searching deeper about it, I've found that there are two main distortions that affect the magnetometer readings, hard and soft iron. Hard iron distortion is caused by magnetic materials near the IMU. Soft iron is caused by ferromagnetic materials near the IMU, i.e. materials that are attracted by magnets.

So, my solution to overcoming the car's body effect was to calibrate the IMU near the car's body. Performing the same test again, the measurements were still wrong.

After this, I would like to know what are the sources of error in an urban environment that will affect the yaw (heading) estimation. Electrical wires, cars passing by and etc.


问:
How to measure an optical encoder resolution and gear ratio?
I've bought a DC geared motor with an optical encoder. The vendor didn't provide specification. I can read MITSUMI M25N-2R-14 2241 and 25GA-370-12V-330RPM on the motor. There are some pages on the internet about this motor with contradicting information that I can't trust. I need to find a practical method to find exact gear ratio and encoder resolution.

I took a picture of the encoder and in a CAD software measured 1 degree the angle between two lines. I'm not sure about accuracy of this method!

I use this code in Arduino:

```<code>#include <Encoder.h>

Encoder myEncoder(2,3); // Best Performance     2, 3   intrupt
double Position ;

void setup() {
Serial.begin(250000) ;
Serial.println("Encoder Test.") ;
}
long oldPosition = -999 ;
void loop() {
long newPosition = myEncoder.read() ;
if (newPosition != oldPosition){
oldPosition = newPosition ;
Serial.println(newPosition);
}}
</code>```

I can read data from my encoder but how can I use this data?

ContentLicense: CC BY-SA 4.0


问:
How to minimize the error integrating 3D angular velocity data obtained by the IMU to get linear velocity?
I have IMU sensor that gives me the raw data such as orientation, Angular and Linear acceleration. Im using ROS and doing some Gazebo UUV simulation. Furthermore, I want to get linear velocity from the raw IMU data.

So the naive integration method is as follow:
The simplest method for IMU state estimation is the naive integration of the measured data. We estimate the attitude by integrating 3D angular velocity data obtained by the IMU. Assuming that the time step Δ is small, the attitude at each time step can be incrementally calculated.

If I do integration over time there will be accumulated error and will not be accurate with the time when for example the robot makes turns. So Im looking for some methods ( ROS packages or outside ROS framework) or code that can correct that error.

Any help?


问:
six cameras selection for jetson xavier
What six cameras do you recommend for a university project using a jetson xavier nx?

I am getting into self driving from a software point of view and camera selection becomes very difficult for me. I know that things like hdr or wide visual field are important, but there are so many options that I don't know which one to choose to start the project.
In this case the cameras would not be for high speeds aand these are the official options: https://developer.nvidia.com/embedded/jetson-partner-supported-cameras?t1_max-resolution=4K&t1_supported-jetson-products=xavier

Title: six cameras selection for jetson xavier


问:
Gazebo sensor plugins for barometric pressure Sensor with geometry_msgs/PointStamped?
Im doing some underwater UUV Gazebo simulation and would like to use the hector_pose_estimationhector_pose_estimation package to fuse IMU and Fluid pressure sensors input for pose estimation. But when running the node I got ERROR

```<code>Client [/hector_pose_estimation] wants topic /rexrov2/pressure to have datatype/md5sum [geometry_msgs/PointStamped/c63aecb41bfdfd6b7e1fac37c7cbe7bf], but our version has [sensor_msgs/FluidPressure/804dc5cea1c5306d6a2eb80b9833befe]. Dropping connection.
</code>```

So my Pressure Sensor has sensor_msgs/FluidPressure but the package wants pressure_height (geometry_msgs/PointStamped) as input. So I created a node listening FluidPressure msg and publish the PointStamped message in the callback. Here the node

```<code>#include "ros/ros.h"
#include <sensor_msgs/FluidPressure.h>
#include <geometry_msgs/PointStamped.h>

double current_pressure;

geometry_msgs::PointStamped::ConstPtr output:
void pressureCallback(const  geometry_msgs::PointStamped::ConstPtr& msg)
{

     current_pressure= msg->fluid.pressure;
     depth=current_pressure / (997.04*9.8);

     output.z=depth;
     pub_.publish(output);

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pressure_republisher");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/rexrov2/pressure", 10, pressureCallback);
  ros::Publisher pub_ = n.advertise<geometry_msgs::PointStamped>("/pressure_height"", 50);
  ros::spin();
  return 0;
}
</code>```

Would this gonna work?

Thanks


问:
TF broken in Hector Pose Estimation when fusing IMU and Pressure Sensor, frames are not correct and pose also not correct
Im doing some underwater UUV Gazebo simulation and would like to use the

hector_pose_estimation

package to fuse IMU and Fluid pressure sensors input for pose estimation. In the gazebo launch file I have this tf transformation   <node pkg="tf" type="static_transform_publisher" name="world_to_nav" args="0 0 0 0 0 0 /world /nav 100" />
So the tf looks like this. and when using the following hector_pose_estimation launch file

```<code><?xml version="1.0"?>
<launch>
  <node pkg="hector_pose_estimation" type="pose_estimation" name="hector_pose_estimation">
    <remap from="raw_imu" to="/rexrov2/imu" />
    <remap from="pressure_height" to="/pressure_height" />
  </node>

   <node pkg="tf" type="static_transform_publisher" name="nav_to_base_footprint" args="0 0 0 0 0 0 /nav /rexrov2/base_footprint 100" />
</launch>
</code>```

the TF is following  . The both TF from gazebo and hector_pose_estimation are not correct. Any help how to get the correct frames to make it work? And when using the above launch the  hector_pose_estimation /pose is like these

```<code>  header:
  seq: 329
  stamp:
    secs: 781
    nsecs: 118000000
  frame_id: "nav"
pose:
  position:
    x: 0.0
    y: 0.0
    z: -1.64102132948e-09
  orientation:
    x: -8.80621330591e-06
    y: 0.000294167493465
    z: 3.71623915803e-14
    w: 0.999999956694
</code>```

Which is not correct as I have the true pose which if following

```<code>header:
  seq: 15460
  stamp:
    secs: 781
    nsecs: 608000000
  frame_id: "world"
child_frame_id: "rexrov2/base_link"
pose:
  pose:
    position:
      x: 0.083413690347
      y: 0.133535986662
      z: -0.77756440376
    orientation:
      x: -9.34768900238e-06
      y: 0.000294151069482
      z: 0.00184072884918
      w: 0.999998262551
</code>```

In the hector /Pose the  frame_id: "nav" and no child_frame. In the real pose is  frame_id: "world" and  child_frame_id: "rexrov2/base_link" (rexrov2 is the name of the robot) . Why the x and Y component of the Position are zeros? They shouldn't be zero. So any help how to fix it?


问:
Jacobian row of zeros. Why?
I have a Simulink model that has a standard Simulink block that determines the Jacobian for a given Rigid Body Tree model. The Rigid Body Tree model is a built in model of the Franka Emika Panda robot and is loaded into the workspace using:
 panda_matlab = loadrobot("frankaEmikaPanda").

The input to the block consists of the robots configuration (config), which is determined by the joint angles $q$. In this case all the joint angles are $0$. This means that the robot is in an upright position and joints 3 and 5 depend linearly on joint 1 Which means that columns 1, 3 and 5 of the Jacobian should be identical. This is indeed the case, However the Jacobian also has one row of zeros. I do not understand where this row of zeros come from.

I also used my own, simplified Panda model with fewer links. In the same upright position with, the same linearly dependent links. The jacobian matrix of this model is shown below. This also has a row of zeros in the Jacobian.

What is causing the jacobian to have this row of zeros?

Thanks in advance.

Title: Jacobian row of zeros. Why?


问:
What are common method for sensor feedback in teleoperated underwater ROV to close the control loop
I would like to know what are the common method to get sensor sensor feedback in teleoperated underwater  ROV to close the control loop. I mean does it use Velocity or Force and what sensor are used. I tried to use only IMU and get Velocity to control the UUV but velocity error accumulated over time and is not good. So any other options?

Thanks


问:
How to get odometry from propeller rotation signals knowing that they can rotate to maximum value?
m trying to get Velocity in order to control underwater swarm ROV. I can use the acceleration and to Euler integration but the error will be accumulated over time. As we are having a swarm (many small robots) Im limited with sensors on board. So in order to get more accurate Velocity estimation was thinking of sensor Fusing of IMU and odometry. As don't have any odometry sensors came to some out of the box solution

So as I have access to the motor (thruster- propeller) signals and the robot can move to maximum and constant speed. Means the rotation of the propeller have a maximum limit and when they reach that limit they can maintain that maximum. So was thinking of some mathematical model that can be used to provide me with odometry and than use some of the ROS packages for sensor fission(robot_lokalization orh ector_pose_estimation) to get accurate Velocity.

The propeller revolution curve looks like something like this

So I need a help in that mathematical model how to get odometry from the motors-thrusters. Any help with that and possible ROS node (C++) implementation?


问:
Help prove the following identity (Jacobian)
I have encountered the following identity expression. Can please someone elaborate and explain how this equation is manipulated?


问:
Trajectory following of 3-wheel omnidirectional robot
I created a three-wheeled omnidirectional robot on omni-wheels, there is also a program in which I can plot a trajectory when plotting a trajectory, such data are obtained (X, Y, Tangent X, Tangent Y, Theta), but I do not understand what formulas should be used to transform these are given in speed for each wheel


问:
Real-Time, App-Based Tool Tracking and Inventory Management System
For one of my classes, I need to create a sensor system that can connect with an app that will be able to track when specific tools leave a certain area. I do not know which sensor to implement that will connect to an app. I have been looking into different proximity or position sensors but I do not know which one to use. Does anybody have any starting points for me?</p>


问:
Outer Position Loop to Control Inner Velocity Loop
I'm trying to control the position of a brushless DC gearmotor. To drive the motor, I have a controller box that provides a velocity that is proportional to a 0 to 5 V input. The motor has it's own Hall sensors for measuring velocity. I also have a rotary encoder attached to the motor to measure position.

I'm looking for an algorithm to control absolute position/angle of the motor (i.e. the outer loop in the diagram above). The first thing I tried was a PID loop which supplied a 0 to 5 V signal based on the position error calculated from the encoder. I was not able to tune the loop to go to a position quickly and accurately. And I have my doubts as to whether PID is even suitable for this kind of topology.

The algorithm I'm using now is to use KP only, but as my motor gets close to the setpoint, I lower the KP. I think I lower the KP about 4 times before I come to a complete stop. This algorithm kind of works, but it's highly unusual. It is pretty fast.

I've seen this question but found the answers to not be helpful.


问:
How to calculate the workspace of a five bar planar parallel robotic manipulator using MATLAB?
I am trying to calculate the workspace of a five bar planar parallel manipulator using MATLAB. Also, I am trying to plot the exact workspace as shown( kindly check the link). For this I guess I need to model the robot how should it be done?

Perform Forward and Inverse Kinematics on a Five-Bar Robot


问:
I would like to add a prismatic joint to the Z axis of the 6th joint of Urdf
I have a urdf file,urdf of UR5 robot and added a prismatic joint to the end of 6th joint of the robot with the "prismatic_joint" tag (By copying other joints and links)

```<code>    <joint name="wrist_2_joint" type="revolute">
        <parent link="wrist_1_link" />
        <child link="wrist_2_link" />
        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0" />
        <axis xyz="0 0 1" />
        <limit effort="28.0" lower="-6.2831853071" upper="6.2831853071" velocity="3.2" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>
    <link name="wrist_2_link">
        <visual>
            <geometry>
                <mesh filename="package://ur_description/meshes/ur5/visual/wrist2.dae" />
            </geometry>
            <material name="LightGrey">
                <color rgba="0.7 0.7 0.7 1.0" />
            </material>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://ur_description/meshes/ur5/collision/wrist2.stl" />
            </geometry>
        </collision>

<inertial>
            <mass value="1.219" />
            <origin rpy="0 0 0" xyz="0.0 0.0 0.0" />
            <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942" />
        </inertial>

    </link>
    <joint name="wrist_3_joint" type="revolute">
        <parent link="wrist_2_link" />
        <child link="wrist_3_link" />
        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465" />
        <axis xyz="0 1 0" />
        <limit effort="28.0" lower="-6.2831853071" upper="6.2831853071" velocity="3.2" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>
    <link name="wrist_3_link">
        <visual>
            <geometry>
                <mesh filename="package://ur_description/meshes/ur5/visual/wrist3.dae" />
            </geometry>
            <material name="LightGrey">
                <color rgba="0.7 0.7 0.7 1.0" />
            </material>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl" />
            </geometry>
        </collision>

<inertial>
            <mass value="0.1879" />
            <origin rpy="0 0 0" xyz="0.0 0.0 0.0" />
            <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822" />
        </inertial>
    </link>

    <!-- I added this section-->
    <joint name="prismatic_joint" type="prismatic">
        <parent link="wrist_3_link" />
        <child link="prismatic_link" />
        <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465" />
        <axis xyz="0 0 1" />
        <limit effort="28.0" lower="-6.2831853071" upper="6.2831853071" velocity="3.2" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>

    <link name="prismatic_link">
        <visual>
            <geometry>
                <mesh filename="package://ur_description/meshes/ur5/visual/wrist3.dae" />
            </geometry>
            <material name="LightGrey">
                <color rgba="0.7 0.7 0.7 1.0" />
            </material>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://ur_description/meshes/ur5/collision/wrist3.stl" />
            </geometry>
        </collision>

<inertial>
            <mass value="0.1879" />
            <origin rpy="0 0 0" xyz="0.0 0.0 0.0" />
            <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822" />
        </inertial>
    </link>
</code>```

Even though I am not sure if it is correct, I tried it on a inverse kinematic solver library. It seems somehow working. But I would like to resize the prismatic joint length. In the inverse kinematic solver, the prismatic joint size was too high as below:

Did I add the joint correctly? What else should I add to resize the prismatic joint?


问:
How useful is DRAKE toolbox
How useful would it be to learn and interact with DRAKE toolbox that is open source? Are there better toolboxes out there when compared with the DRAKE toolbox?
I am a student interested in robotics and wanted to understand what are things that the DRAKE toolbox can or cannot do when compared with other toolboxes?</p>

Title: How useful is DRAKE toolbox


问:
Efficient control for BLDC 3-phase motor with BEMF using Arduino (Atmega328pb)
Working on developing efficient code for driving a BLDC 3-phase motor with BEMF feedback using Atmega328pb,

The code is based (as a starting point) on this tutorial: Link

The basic understanding is to increase the speed we need to increase the duty of the PWM's

However, the code I currently have works fine BUT is not efficient, (the motor uses much more current than other off the shelf controllers for the same speed)

Note: For this specific project, the main goal is to get a high speed (as the motor is used for an air pump or blower)

The code is simple,
It uses a 6-step commutation to rotate the motor,
It triggers a comparator interrupt to go from one step to the next step, (for every step the interrupt is different, see the code in the above link)

See the link https://www.digikey.com/en/articles/controlling-sensorless-bldc-motors-via-back-emf
 that explains how it works, and for this project, we use this configuration below:
cam.png

Important text from the Digikey tutorial from the link above:
Note that there is a phase difference between an individual Hall sensor changing output in a conventional BLDC motor, and the back EMF zero crossing point for an individual coil in a sensorless unit of 30 degrees. Consequently, in a sensorless motor control circuit, after the zero-crossing point is detected, a 30-degree phase lag is built into the firmware before the next action in the energizing sequence is activated

The question is:
How can we know exactly when to move to the next step, (maybe we can use timers)

In the tutorial code, it implements some form of delay in the ISR code that works somehow, but is very not efficient,


问:
How can I feel feedback with electric power steering (EPS)?
When I am rotating steering wheel and it meets an obstacle, I feel it. I have EPS. How does it work? Is it just explicit sensor and feedback? Can I sniff this feedback on CAN bus?</p>


问:
robot_pose_ekf won't publish any messages
I am having trouble getting the robot_pose_ekf package to publish messages. I launch it with this launch file.

```<code><launch>

  <!-- Extended Kalman Filter from robot_pose_ekf Node-->
  <!-- Subscribe: /odom, /imu/data, /vo -->
  <!-- Publish: /robot_pose_ekf/odom_combined -->

  <remap from="/imu_data" to="/imu/data" />

  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="false"/>
    <param name="debug" value="true"/>
    <param name="self_diagnose" value="true"/>
  </node>

</launch>
</code>```

But then I get lots of these errors:

```<code>[ WARN] [1636570848.059506230]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1636570848.059787913]: Covariance specified for measurement on topic imu is zero
[ERROR] [1636570848.060134410]: filter time older than odom message buffer
[ WARN] [1636570848.074015563]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1636570848.074583539]: Covariance specified for measurement on topic imu is zero
[ERROR] [1636570848.085433183]: Covariance specified for measurement on topic wheelodom is zero
</code>```

And I get this output from wtf, which says I'm getting inputs but am not publishing any messages on topic /robot_pose_ekf/odom_combined.

doug@raspi4:~$ rosrun robot_pose_ekf wtf.py
looking for node robot_pose_ekf...
Input:

Odometry sensor

is used

is active

received 12 messages

listens to topic /odom

IMU sensor

is used

is active

received 152 messages

listens to topic /imu/data

Visual Odometry sensor

is NOT used

is NOT active

received 0 messages

listens to topic

GPS sensor

is NOT used

is NOT active

received 0 messages

listens to topic
Output:

Robot pose ekf filter

is active

sent 0 messages

publishes on topics /robot_pose_ekf/odom_combined and /tf

My IMU data is coming from a BNO055 device publishing with code from https://github.com/RoboticArts/ros_imu_bno055.

Any ideas what might be going on?


问:
3D LiDAR synchronization with RGB Camera
There's a general procedure for estimating projection transformation between camera and 3d LiDAR but there's a secondary problem which is associating pixels with depths in a grid which isn't uniformly sampled.

What is the correct approach to doing this? Both the pre and post transformed depth points lack a grid structure. The pre-transformed points lack a grid structure because the sampling is time- instead of angle-based so you get variations in the angles. The post-transformed grid is obviously not going to be uniformly sampled either.


问:
In MuJoCo why do some XML files have objects fall to the ground while others are just fixed above the ground?
Why do some models, or objects in a MuJoCo XML script fall to the floor while others don't?

In the models files that come with the MuJoCo zip, the robot arm (arm26) does not fall to the ground while the humanoid and many other models have objects that fall to the ground.

I think I've compared every facet of the files and haven't been able to pinpoint what causes this in some files but not for others.

I will include the code of the arm26 file, which does not fall to the ground.

An answer to this question would be a modification to this code that would make it fall to the ground.

```<code><mujoco model="2-link 6-muscle arm">

    <option timestep="0.005" iterations="50" solver="Newton" tolerance="1e-10"/>

    <size njmax="50" nconmax="10" nstack="200"/>

    <visual>
        <rgba haze=".3 .3 .3 1"/>
    </visual>

    <default>
        <joint type="hinge" pos="0 0 0" axis="0 0 1" limited="true" range="0 120" damping="0.1"/>
        <muscle ctrllimited="true" ctrlrange="0 1"/>
    </default>

    <asset>
        <texture type="skybox" builtin="gradient" rgb1="0.6 0.6 0.6" rgb2="0 0 0" width="512" height="512"/>

        <texture name="texplane" type="2d" builtin="checker" rgb1=".25 .25 .25" rgb2=".3 .3 .3" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>

        <material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
    </asset>

    <worldbody>
        <geom name="floor" pos="0 0 -0.5" size="0 0 1" type="plane" material="matplane"/>

        <light directional="true" diffuse=".8 .8 .8" specular=".2 .2 .2" pos="0 0 5" dir="0 0 -1"/>

        <site name="s0" pos="-0.15 0 0" size="0.02"/>
        <site name="x0" pos="0 -0.15 0" size="0.02" rgba="0 .7 0 1" group="1"/>

        <body pos="0 0 0">
            <geom name="upper arm" type="capsule" size="0.045" fromto="0 0 0  0.5 0 0" rgba=".5 .1 .1 1"/>
            <joint name="shoulder"/>
            <geom name="shoulder" type="cylinder" pos="0 0 0" size=".1 .05" rgba=".5 .1 .8 .5" mass="0" group="1"/>

            <site name="s1" pos="0.15 0.06 0" size="0.02"/>
            <site name="s2" pos="0.15 -0.06 0" size="0.02"/>
            <site name="s3" pos="0.4 0.06 0" size="0.02"/>
            <site name="s4" pos="0.4 -0.06 0" size="0.02"/>
            <site name="s5" pos="0.25 0.1 0" size="0.02"/>
            <site name="s6" pos="0.25 -0.1 0" size="0.02"/>
            <site name="x1" pos="0.5 -0.15 0" size="0.02" rgba="0 .7 0 1" group="1"/>

            <body pos="0.5 0 0">
                <geom name="forearm" type="capsule" size="0.035" fromto="0 0 0  0.5 0 0" rgba=".5 .1 .1 1"/>
                <joint name="elbow"/>
                <geom name="elbow" type="cylinder" pos="0 0 0" size=".08 .05" rgba=".5 .1 .8 .5" mass="0" group="1"/>

                <site name="s7" pos="0.11 0.05 0" size="0.02"/>
                <site name="s8" pos="0.11 -0.05 0" size="0.02"/>
            </body>
        </body>
    </worldbody>

    <tendon>
        <spatial name="SF" width="0.01">
            <site site="s0"/>
            <geom geom="shoulder"/>
            <site site="s1"/>
        </spatial>

        <spatial name="SE" width="0.01">
            <site site="s0"/>
            <geom geom="shoulder" sidesite="x0"/>
            <site site="s2"/>
        </spatial>

        <spatial name="EF" width="0.01">
            <site site="s3"/>
            <geom geom="elbow"/>
            <site site="s7"/>
        </spatial>

        <spatial name="EE" width="0.01">
            <site site="s4"/>
            <geom geom="elbow" sidesite="x1"/>
            <site site="s8"/>
        </spatial>

        <spatial name="BF" width="0.009" rgba=".4 .6 .4 1">
            <site site="s0"/>
            <geom geom="shoulder"/>
            <site site="s5"/>
            <geom geom="elbow"/>
            <site site="s7"/>
        </spatial>

        <spatial name="BE" width="0.009" rgba=".4 .6 .4 1">
            <site site="s0"/>
            <geom geom="shoulder" sidesite="x0"/>
            <site site="s6"/>
            <geom geom="elbow" sidesite="x1"/>
            <site site="s8"/>
        </spatial>
    </tendon>

    <actuator>
        <muscle name="SF" tendon="SF"/>
        <muscle name="SE" tendon="SE"/>
        <muscle name="EF" tendon="EF"/>
        <muscle name="EE" tendon="EE"/>
        <muscle name="BF" tendon="BF"/>
        <muscle name="BE" tendon="BE"/>
    </actuator>
</mujoco>
</code>```


问:
torque calculation issues
so i have been trying to draw a free body diagram of a 6 axis robot arm project, in order to know the torque required for each joint. and now am confused about the angular acceleration part which contributes to the amount of torque needed. i was simply using the force times distance to get the torque right from the end effector of the robot arm to the base, but i realised soon, that the base motor would not work with the same formula because it have a CW and CCW movement(angular movement), which causes some acceleration,which in turn affect the torque.
so how do i calculate precisely the torque for the base motor, to avoid any problems later.
the requirements are LENGTH: 1.5m    PAYLOAD : 20kg</p>

Title: torque calculation issues


问:
Dynamics of parallel manipulator
My task is to apply forces to control 3-dof parallel manipulator. Forces are applied to linear  actuators, friction is neglected.  End-effector of a robot is supposed to follow generated path; for this example, let it be a simple circle. So far I have made a simplified 3d model of robot and calculated inverse kinematics.

Promoter of my engineering work don't really know how to do this, but he said that calculating forward dynamics is too complex and i shouldn't go that way. Could you tell me what will be the easiest way to go?

ContentLicense: CC BY-SA 3.0

Title: Dynamics of parallel manipulator


问:
Want to check RANSAC for a noiseless set of points; Error: Not enough inliers
I was just checking the functioning of my code implementing RANSAC sac_model: perpendicular by checking for a noiseless pointcloud. I have a few questions in the steps:
I did the following:
To generate noiseless point cloud:

I collected 3 points from my current point cloud being published , used an online plane equation calculator

The plane equation found here and the plane coefficents I find from RANSAC are same. So it is verified that my usage(implementation of RANSAC is correct I am assuming.)
But I need to verify by checking if all points lie on the plane in rviz

I generate 500 points using the
i) one of the points from pointcloud (also used in above equation to find eqn of plane)
ii) plane coefficients as normal vector
Almost all The 500 points have their z values close to -3;
That 500 points were generated in MATLAB coz I am more familiar in MATLAB coding.
I also visualize these points in MATLAB
I do not understand why enough inliers are not found considering all of them are generated from plane equation.

Now I copy my 500 points in a pcd file and read it in my code to perform RANSAC  on the points. The RANSAC yields following error:


问:
Trajectory optimal for quadcopter
I have defined a quadcopter that I control with a PID controller. The dynamics that define it are such.

$T=mg+m\ddot{z}$

$\theta=\frac{\ddot{x}}{-9.82}$

$\phi=\frac{\ddot{y}}{9.82}$

As you can see I always need acceleration to actuate so I have defined a trajectory that allows via points that I took from a robot manipulator book and applied it to the quadcopter. The trajectories give me a position for PID and an acceleration for dynamics actuating. However, those trajectories (cubic polynomials) do not look good in the 3d world as you can see on the figure. So what trajectory should I use on a quadcopter?

On the figure, I tried to make a reference trajectory to look like a circle but the blends "ruins" it. There are 10 via points here.

Title: Trajectory optimal for quadcopter


问:
Why does AHRS system with Madgwick filter has so big drift?
I have an MPU9250 on my PCB and I use an AHRS system with a Madgwick filter to get yaw angle. I want to get very accurate and non-drifting yaw angle, but, for unknown reasons, I can't.

I calibrated the magnetometer, so I don't think that is the problem. The yaw angle drifts by about 10 degrees every 3 seconds. I use CubeIDE to write firmware for the PCB. And I took the sources from here: https://github.com/sonphambk/MPU9250/tree/master/Src

This is how the yaw angle changes:

I also rewrote a python library for MPU9250 to C (the python library: https://github.com/morgil/madgwick_py):

void mulMat(double mat1[][C1], double mat2[][C2], double mat3[R1][C2]) {
    for (int i = 0; i < R1; i++) {
        for (int j = 0; j < C2; j++) {
            mat3[i][j] = 0;

            for (int k = 0; k < R2; k++) {
                mat3[i][j] += mat1[i][k] * mat2[k][j];
            }
         }
     }
}

void MadgwickAHRSupdatePy(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
    float norm;

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
        return;
    }

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)) && !((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
        norm = sqrt(ax * ax + ay * ay + az * az);
        ax /= norm;
        ay /= norm;
        az /= norm;

        norm = sqrt(mx * mx + my * my + mz * mz);
        mx /= norm;
        my /= norm;
        mz /= norm;

        Quaternion h, qConj, qMag;
        Quaternion_set(0, mx, my, mz, &qMag);
        Quaternion_conjugate(&selfQ, &qConj);

        Quaternion_multiply(&qMag, &qConj, &h);
        Quaternion_multiply(&selfQ, &h, &h);

        double b[4] = {0, sqrt(h.v[0] * h.v[0] + h.v[1] * h.v[1]), 0, h.v[2]};
        double q_ins[4] = {selfQ.w, selfQ.v[0], selfQ.v[1], selfQ.v[2]};

        double f[1][6] = {{
                2 * (q_ins[1] * q_ins[3] - q_ins[0] * q_ins[2]) - ax,
                2 * (q_ins[0] * q_ins[1] + q_ins[2] * q_ins[3]) - ay,
                2 * (0.5 - q_ins[1] * q_ins[1] - q_ins[2] * q_ins[2]) - az,
                2 * b[1] * (0.5 - q_ins[2] * q_ins[2] - q_ins[3] * q_ins[3]) + 2 * b[3] * (q_ins[1] * q_ins[3] - q_ins[0] * q_ins[2]) - mx,
                2 * b[1] * (q_ins[1] * q_ins[2] - q_ins[0] * q_ins[3]) + 2 * b[3] * (q_ins[0] * q_ins[1] + q_ins[2] * q_ins[3]) - my,
                2 * b[1] * (q_ins[0] * q_ins[2] + q_ins[1] * q_ins[3]) + 2 * b[3] * (0.5 - q_ins[1] * q_ins[1] - q_ins[2] * q_ins[2]) - mz
        }};
        double j[6][4] = {
                {-2 * q_ins[2], 2 * q_ins[3], -2 * q_ins[0], 2 * q_ins[1]},
                {2 * q_ins[1], 2 * q_ins[0], 2 * q_ins[3], 2 * q_ins[2]},
                {0, -4 * q_ins[1], -4 * q_ins[2], 0},
                {-2 * b[3] * q_ins[2], 2 * b[3] * q_ins[3], -4 * b[1] * q_ins[2] - 2 * b[3] * q_ins[0], -4 * b[1] * q_ins[3] + 2 * b[3] * q_ins[1]},
                {-2 * b[1] * q_ins[3] + 2 * b[3] * q_ins[1], 2 * b[1] * q_ins[2] + 2 * b[3] * q_ins[0], 2 * b[1] * q_ins[1] + 2 * b[3] * q_ins[3],  -2 * b[1] * q_ins[0] + 2 * b[3] * q_ins[2]},
                {2 * b[1] * q_ins[2], 2 * b[1] * q_ins[3] - 4 * b[3] * q_ins[1], 2 * b[1] * q_ins[0] - 4 * b[3] * q_ins[2],  2 * b[1] * q_ins[1]}
        };
        double step2d[1][4];
        mulMat(f, j, step2d);

        double s1 = step2d[0][0];
        double s2 = step2d[0][1];
        double s3 = step2d[0][2];
        double s4 = step2d[0][3];
        norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);
        s1 /= norm;
        s2 /= norm;
        s3 /= norm;
        s4 /= norm;

        Quaternion qGyro;
        Quaternion_set(0, gx, gy, gz, &qGyro);

        Quaternion_multiply(&qGyro, &selfQ, &h);
        double qDot1 = (h.w * 0.5 - beta * s1) * (1 / sampleFreq);
        double qDot2 = (h.v[0] * 0.5 - beta * s2) * (1 / sampleFreq);
        double qDot3 = (h.v[1] * 0.5 - beta * s3) * (1 / sampleFreq);
        double qDot4 = (h.v[2] * 0.5 - beta * s4) * (1 / sampleFreq);

        q_ins[0] += qDot1;
        q_ins[1] += qDot2;
        q_ins[2] += qDot3;
        q_ins[3] += qDot4;

        norm = sqrt(q_ins[0] * q_ins[0] + q_ins[1] * q_ins[1] + q_ins[2] * q_ins[2] + q_ins[3] * q_ins[3]);
        q_ins[0] /= norm;
        q_ins[1] /= norm;
        q_ins[2] /= norm;
        q_ins[3] /= norm;

        q0 = q_ins[0];
        q1 = q_ins[1];
        q2 = q_ins[2];
        q3 = q_ins[3];

        Quaternion_set(q_ins[0], q_ins[1], q_ins[2], q_ins[3], &selfQ);
    }

}
```

But the result is the same. The yaw angle goes away very fast.

What could be the reason of this? What should I change or calibrate?


问:
offset in gridmap
colleagues, i am currently viewing Cyrill Stachniss's course on SLAM and also doing the exercises. At the moment i am doing an exercise on occupancy grid mapping and there is some strange offset variable in the code and i don't know what is the purpose of it

Here is the code related to it

```<code>border = 30;
robXMin = min(poses(:,1));
robXMax = max(poses(:,1));
robYMin = min(poses(:,2));
robYMax = max(poses(:,2));
mapBox = [robXMin-border robXMax+border robYMin-border robYMax+border];
offsetX = mapBox(1);
offsetY = mapBox(3);
mapSizeMeters = [mapBox(2)-offsetX mapBox(4)-offsetY];
mapSize = ceil([mapSizeMeters/gridSize]);
</code>```

This offset is then used to convert world coordinates into map coordinates

```<code>pntsMap = floor((pntsWorld - repmat(offset,1,size(pntsWorld,2)))./gridSize)
</code>```

Title: offset in gridmap


问:
Consensus Algorithm converges then diverges again
I am trying to implement a consensus algorithm to command a swarm of 50 robots.
The robots 'communication is modeled by a complete undirected graph. when simulating the process I noticed that the robots converge to the average of the initial states, which is expected. However, After converging to the same point, they diverge again from that same point and each robot goes on a different direction. It is like they implode and then they explode.
I am new to this so I don't know what to expect, but I assumed that the simulation would stop when the robots converge.
Is what's happening norman? (I am using matlab , and I am pretty sure that my consensus equation is correct $\dot{x}(t)=-L.x(t)$  where $L=$degree matrix - adjacency matrix is the Laplacian matrix)


问:
Trouble establishing Ethernet connection between UR3 and PC
First of all sorry if this forum is not the most adequate space for this type of question.

I am having a hard time establishing an Ethernet/IP connection between my UR3 and my computer, I am just using an Ethernet cable to directly connect the PC and the UR3.

My objective is to use this connection to send commands through to the UR3 and also to read data from the robot. I am planning to do this on python. I have tried to follow severel guides but to no avail, I am pretty sure that the mistakes I am making are when defining the adresses in the UR3.

I never managed to establish a connection between the PC and the UR3.

I always tried to establish the connection with my Wi-Fi off as it was told in many articles.
I don't know if it helps but when I connect the Ethernet cable a not identified network would appear in my computer with the IP 192.168.73.50 (is that supossed to be my DNS server?)

The last setup I tried is the suggested in this guide, I know it is a guide meant for the UR3e but I am up to trying anything at this point really.

The code I am using to test the connection is as follows:

import socket
import time

HOST = "192.168.1.100"
PORT = 30002

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

s.connect((HOST, PORT))

s.send(("set_digital_out(1,True)" + "\n").encode('utf8'))

time.sleep(2)

s.send(("movej([0, 0, 0, 0, 0, 0, a=1, v=1])" + "\n").encode('utf8'))

s.close()
```

I really am not well versed in networks so I don't really know what adresses I need to change to work with my project or if I need to do something on my PC. I did verify that the IP of the UR3 is set to the one that's in the image, but I don't know if I am supposed to use the same subnet mask/gateway/DNS server. Thank you for reading


问:
DH parameters correct but the MATLAB plot isn't accurate
```<code>%L(i) = Link([theta(i) D(i) a(i) Alpha(i)
L(1) = Link([0 0 10 0]);
L(2) = Link([0 0 10 pi/2]);
L(3) = Link([0 0 20 pi/2]);
L(4) = Link([0 0 10 -pi/2]);

rob = SerialLink(L);

rob.name= "new robot";

rob.plot([0 0 0 0]);
</code>```

given the MATLAB code and DH parameters in the code above, and the robot in the image shown

why does plotting it results in the third joint oriented incorrectly? I'm assuming my parameters are incorrect but I don't know at this point.

any tips or help greatly appreciated :)


问:
How to find fundamental matrix based on other fundamental matrix and camera movement?
I am trying to speed up some multi-camera system that relies on calculation of fundamental matrices between each camera pair.

Please notice the following is pseudocode. @ means matrix multiplication, | means concatenation.

I have code to calculate F for each pair calculate_f(camera_matrix1_3x4, camera_matrix1_3x4), and the naiive solution is

```<code>for c1 in cameras:
    for c2 in cameras:
        if c1 != c2:
            f = calculate_f(c1.proj_matrix, c2.proj_matrix)
</code>```

This is slow, and I would like to speed it up. I have ~5000 cameras.

I have pre calculated all rotations and translations  (in world coordinates) between every pair of cameras, and internal parameters k, such that  for each camera c, it holds that c.matrix = c.k @ (c.rot | c.t)

Can I use the parameters r, t to help speed up following calculations for F?

In mathematical form, for 3 different cameras c1, c2, c3 I have

f12=(c1.proj_matrix, c2.proj_matrix), and I want f23=(c2.proj_matrix, c3.proj_matrix), f13=(c1.proj_matrix, c3.proj_matrix) with some function f23, f13 = fast_f(f12, c1.r, c1.t, c2.r, c2.t, c3.r, c3.t)?

A working function for calculating the fundamental matrix in numpy:

```<code>def fundamental_3x3_from_projections(p_left_3x4: np.array, p_right__3x4: np.array) -> np.array:
    # The following is based on OpenCv-contrib's c++ implementation.
    # see https://github.com/opencv/opencv_contrib/blob/master/modules/sfm/src/fundamental.cpp#L109
    # see https://sourishghosh.com/2016/fundamental-matrix-from-camera-matrices/
    # see https://answers.opencv.org/question/131017/how-do-i-compute-the-fundamental-matrix-from-2-projection-matrices/
    f_3x3 = np.zeros((3, 3))
    p1, p2 = p_left_3x4, p_right__3x4

    x = np.empty((3, 2, 4), dtype=np.float)
    x[0, :, :] = np.vstack([p1[1, :], p1[2, :]])
    x[1, :, :] = np.vstack([p1[2, :], p1[0, :]])
    x[2, :, :] = np.vstack([p1[0, :], p1[1, :]])

    y = np.empty((3, 2, 4), dtype=np.float)
    y[0, :, :] = np.vstack([p2[1, :], p2[2, :]])
    y[1, :, :] = np.vstack([p2[2, :], p2[0, :]])
    y[2, :, :] = np.vstack([p2[0, :], p2[1, :]])

    for i in range(3):
        for j in range(3):
            xy = np.vstack([x[j, :], y[i, :]])
            f_3x3[i, j] = np.linalg.det(xy)

    return f_3x3
<span class="math-container">```
```


问:
Dexterous Manipulation with Robotic Hand
I am currently working for my M.Sc. Thesis with the Allegro Hand with which I have to perform some simple in-hand manipulation tasks, such as lifting or rolling a cube with 2 or 3 fingers. This has to be done with a model-based impedance control scheme.

I have tried to read similar works but unfortunately I don't have the slightest idea how to begin working on it. Also I cant find a ROS compatible simulator to experiment with the existing ROS packages for the Allegro. Any kind of help on how should I start or how to get it going would be greatly appreciated as I have done zero progress so far.


问:
I'm controlling a stepper motor with GPIO pins with a for loop in C++. If I add a sensor reading to the for loop the motor gets slow, loud, and jerky
I have a C++ script actuating a motor and getting an accelerometer sensor reading every 20th or so pulse to the stepper motor. My problem is that the sensor readings are interrupting the pulses. Without the sensor reading the motor runs quietly and smoothly. WITH the sensor readings the  NEMA 17 stepper motor sounds like it's straining. The motor gets jerky and slower.

The sensor reading lines of code and the motor actuating lines of code are in the same for loop.

It's written for an Nvidia Jetson AGX Xavier.

I know it's a lot of code, but the parts in question are just the parts near the middle with the motor and sensor lines in the for loop. They are the most indented lines if that helps you find them.

Before we get to the main code I want to share my attempt to solve this problem, which was to try running the motor code and the sensor code in parallel. I downloaded a few packages. None worked, but here is one attempt.
I was able to paste this attempt at parallelization into the long file below it. It was the jerkiest action from the motor yet and I got segmentation core dumped errors after three seconds when I ran it.

```<code>#include <omp.h>

#pragma omp parallel for schedule(static)
for(int i = 0; i < 200; i++){
    int tid = omp_get_thread_num();
    if (tid%2==0) {
        accel_x = read_raw_data(file, accel_x_register_high);
        accel_y = read_raw_data(file, accel_y_register_high);
        accel_z = (read_raw_data(file, accel_z_register_high));
</code>```

//          cout << accel_x << " " << i << endl;
} else {
std::cout << "This is line TWO" << "\n";

```<code>    // MOTOR
</code>```

//          usleep(motor_speed);
GPIO::output(output_pin, curr_value);
usleep(motor_speed);
GPIO::output(direction_pin, the_clock);
curr_value ^= GPIO::HIGH;

```<code>    }
}
</code>```

The entire file. My CMakeLists.txt is below this.

```<code>// MOTOR INCLUDES
using namespace std;
#include <iostream>
#include <random>
#include <iomanip>
#include <omp.h>
#include <chrono>
#include <map>
#include <string>
#include <thread>
#include <unistd.h>

// for handling random array selection
#include <vector>
#include <signal.h>

#include <JetsonGPIO.h>

// GYRO INCLUDES
extern "C" {
    #include <linux/i2c-dev.h>
    #include <i2c/smbus.h>
}
#include <sys/ioctl.h>
#include <fcntl.h>

// GRYO VARIABLES
int file;
int adapter_nr = 1;
const char* filename = "/dev/i2c-8";
int initialize_mpu(int file);
__s32 high;
__s32 low;
__s32 value;
int read_raw_data(int file, __u8 addr);

__u8 reg = 0x84;
__u8 res;

__s32 accel_x;
__s32 accel_y;
__s32 accel_z;

__s32 gyro_x;
__s32 gyro_y;
__s32 gyro_z;

__s32 gyro_x_register_high = 0x43;
__s32 gyro_y_register_high = 0x45;
__s32 gyro_z_register_high = 0x47;

__s32 accel_x_register_high = 0x3B;
__s32 accel_y_register_high = 0x3D;
__s32 accel_z_register_high = 0x3F;

char buf[10];

// MOTOR VARIABLES
int motor_speed = 700;
int output_pin = 7;
int direction_pin = 11;
float mini = 0;
float maxi = 800;
float spread = maxi - mini;
int current_position = (int) spread / 2;
int the_clock;
int go_here;
int spin;
constexpr int FLOAT_MIN = 0;
constexpr int FLOAT_MAX = 1;

// MOTOR FUNCTIONS
float rand_float() {
    random_device rd;
    default_random_engine eng(rd());
    uniform_real_distribution<float> distr(FLOAT_MIN, FLOAT_MAX);
    return distr(eng);
}
int new_pos(int current_position) {
    float r1 = (rand_float() * spread) + mini;

    return (int) r1;
};

int main() {

    // GYRO
    file = open(filename, O_RDWR);
    if (file < 0) {
        exit(1);
    }
    int addr = 0x68;

    if (ioctl(file, I2C_SLAVE, addr) < 0) {
         exit(1);
    }
    res = i2c_smbus_write_byte_data(file, addr, 0);
    if (res < 0) {
      /* ERROR HANDLING: i2c transaction failed */
    } else {
      /* res contains the read word */
    }

    if (write(file, buf, 3) != 3) {
      /* ERROR HANDLING: i2c transaction failed */
    }
    initialize_mpu(file);

    // MOTOR
    GPIO::setmode(GPIO::BOARD);
    // set pin as an output pin with optional initial state of LOW
    GPIO::setup(output_pin, GPIO::OUT, GPIO::HIGH);
    GPIO::setup(direction_pin, GPIO::OUT, GPIO::HIGH);
    int curr_value = GPIO::HIGH;

//     #pragma omp parallel for schedule(static)
    for (int x = 0; x < 200; x++){
        int tid = omp_get_thread_num();

        go_here = new_pos(current_position);
        int dir_to_pos = go_here - (int) current_position;

        if (dir_to_pos > 0) {
            the_clock = 1;
        } else {
            the_clock = 0;
        };

        current_position = go_here;

//         cout << dir_to_pos << " " << go_here << " " << the_clock << "\n";

        if (dir_to_pos < 0) {
            spin = dir_to_pos * -1;
        } else {
            spin = dir_to_pos;
        };

        for (int i = 0; i < spin; i++){

            if (i % 20==5) {
                accel_x = read_raw_data(file, accel_x_register_high);
                accel_y = read_raw_data(file, accel_y_register_high);
                accel_z = (read_raw_data(file, accel_z_register_high));

//                 gyro_x = read_raw_data(file, gyro_x_register_high) / 131.0;
//                 gyro_y = read_raw_data(file, gyro_y_register_high) / 131.0;
//                 gyro_z = read_raw_data(file, gyro_z_register_high) / 131.0;
//             cout << accel_x << " " << i << endl;

            } else {

            // MOTOR
//             usleep(motor_speed);
            GPIO::output(output_pin, curr_value);
            usleep(motor_speed);
            GPIO::output(direction_pin, the_clock);
            curr_value ^= GPIO::HIGH;

//             cout << gyro_x <<" " << gyro_y << " " << gyro_z << " " << endl;
            };
        };
    };

    GPIO::output(output_pin, 0);
    GPIO::cleanup();

    return EXIT_SUCCESS;
};

// GYRO FUNCTIONS
// Send configurations to the MPU
int initialize_mpu(int file) {
    __u8 sample_rate_register = 0x19;
    __u8 power_mgt_register = 0x6B;
    __u8 config_register = 0x1A;
    __u8 gyro_config_register = 0x1B;
    __u8 interrupt_enable_register = 0x38;

    i2c_smbus_write_byte_data(file, sample_rate_register, 0x07);
    i2c_smbus_write_byte_data(file, power_mgt_register, 0x01);
    i2c_smbus_write_byte_data(file, config_register, 0x00);
    i2c_smbus_write_byte_data(file, gyro_config_register, 0x18);
    i2c_smbus_write_byte_data(file, interrupt_enable_register, 0x01);
}
// Read the data of two 8-bit registers and compile into one 16-bit value
// register_address is the first (high) register, register_address+1 is the low register
// E.g., if the two registers contain the 8-bit values 0x01 and 0x02, this
// function returns the value 0x0102
int read_raw_data(int file, __u8 register_address) {
    high = i2c_smbus_read_byte_data(file, register_address);
    low = i2c_smbus_read_byte_data(file, register_address+1);

    value = (high << 8 | low);

    // This converts it from an unsigned 0-63355 value
    // to a signed value between -32769 and 32768
    if (value > 32768)
        value = value - 65536;
}
</code>```

The CMakeLists.txt file.

```<code>cmake_minimum_required(VERSION 3.1)
project(MyProjecto)
find_package(JetsonGPIO)

add_executable(MyProject range_bound_motor_and_gyro.cpp)

target_link_libraries(MyProject JetsonGPIO::JetsonGPIO i2c)

find_package(OpenMP)
if (OPENMP_FOUND)
    set (CMAKE_C_FLAGS "<span class="math-container">${CMAKE_C_FLAGS} $</span>{OpenMP_C_FLAGS}")
    set (CMAKE_CXX_FLAGS "<span class="math-container">${CMAKE_CXX_FLAGS} $</span>{OpenMP_CXX_FLAGS}")
    set (CMAKE_EXE_LINKER_FLAGS "<span class="math-container">${CMAKE_EXE_LINKER_FLAGS} $</span>{OpenMP_EXE_LINKER_FLAGS}")
endif()
</code>```


问:
Looking for benchmarks on Apriltag pose estimation
we want to know if we can use Apriltags in our project for pose estimation in rather long distances and on moving target. Specifically, correcting errors of an IMU device on an autonomous vehicle.

I am looking for some evaluation reports that would provide some insight on questions like;

-at what distances can we detect tags, and what should be the resolution of these images?

-at what speeds will the tag be still detected?

And other similar potential questions that can apply to a moving distant (at max distance) target.
I have been looking at various papers. I thought I'd find some papers evaluating benchmarks, but I could not manage to find much else than comparisons between different tag libraries (which are not focusing on what we're looking for), benchmarks of close range applications, different light conditions, etc.

I'd be very happy to know if there are already some papers making this analysis.
Thank you


问:
How to find workspace of parallel manipulator by monte carlo method?
I am working on a 3 DOF parallel manipulator, I was successful in solving the inverse kinematics and forward kinematics of my manipulator. But I am facing a problem in generating a workspace for it.

I created a MATLAB script to find workspace by Monte Carlo Simulation Method, the output of the script generates random position coordinates which the end effector can reach with at least one orientation.

After generating random coordinates, I was easily able to solve the inverse kinematics, but when I tried to solve the forward kinematics, I was not able to get the same position coordinates that I used to solve the inverse kinematics.

Also, when I tried to manually change the leg lengths of my parallel robot in SolidWorks according to the output of inverse kinematics, I was not getting the same position coordinates of the end effector, as the values, I used to solve the inverse kinematics.

Note:

(For Inverse Kinematics Input: randomly generated position coordinate; Output: leg lengths)

(For Forward Kinematics Input: leg lengths obtained from inverse solution; Output: position coordinate of end effector)

I don't know where I am going wrong, Can anyone please help me with this?

Thank you in advance, I really appreciate your help !!

Kind Regards,
Shiv


问:
From speed vector to motor control
I'm working with Mona robot (image bellow).
The movement is controlled by settings the speed of each wheel.
I'm trying to find a way to convert a speed vector into motor control so that the robot will follow this vector.
The basic solution would be to orient to robot in the direction of the vector the to move forward. But I'm looking for something smoother.

I have no idea how to process this.

Title: From speed vector to motor control


问:
Precision/Repeatability issue with XArm6
I have a robotic setup with a XArm6 robot and camera facing up with vision algorithms.

I noticed that after a collision or power glitch the robot gets offset. X, Y and Z and angle change according to the camera even though I send the robot at the same location. After that it stayed repeatable until next collision.

A few questions:
1- What is the technical explanation behind what I'm seeing?
2- Should I expect the same behavior from more expansive brands (Universal, Doosan, ABB, ...)?
3- I wouldn't call this a repeatability issue since between collisions it is repeatable. Does this have a name?


问:
Finding 6 angles for UR5 a 6Dof Robot
I am looking for a method to find 6 rotation angles for the 6DOF, 6joint, 5 link UR5 robot. these angles would be used to control UR5 through ROS but unfortunately I cant find any way to find these angles. I have tried making its DH Matrix for base to end T0-6, but it becomes too complex after 3-4 matrix multiplication I cant Track it anymore.

I have T06, although it contains all the information of robot position and orientation but I do not exactly need that. I need the 6 angle which will drive my robot and provide it with rotation angle on each joint. Suppose a 6 joint(6DOF) robotic arm and you have to give specific angles for each of the joints from base to end effector in order for the robotic arm to move to a specific position. I need a way to extract those 6 angles form a given T06 matrix.

This is the link to a pdf explaining the inverse kinematics of UR5, please refer this: https://tianyusongcom.files.wordpress.com/2017/12/ur5_inverse_kinematics.pdf

Please advise a good and short solution or a method to solve this problem. Thanks

Title: Finding 6 angles for UR5 a 6Dof Robot


问:
How to calculate the covariance and gain in SLAM when only one measurement is available?
I am trying to perform SLAM for cases where only one sensor measurement is available.

For example, suppose I want to track the position of a robot moving in a room with multiple known landmarks (2D scenario). The robot can currently measure both the distance to the landmark and the angle to the landmark via its sensors.

The state of the robot can be represented by its current heading angle and current $x$ and $y$ coordinates. The control input can be represented by the robot's velocity and rate of change of heading angle :
$$
X=
\begin{bmatrix}
x\\
y\\
\psi\\
\end{bmatrix}
\quad U=\begin{bmatrix}
V\\
\dot{\psi}\\
\end{bmatrix}
$$

The robot's state can be updated as follows:
$$
\begin{bmatrix}
x(i+1)\\
y(i+1)\\
\psi(i+1)\\
\end{bmatrix}
=\begin{bmatrix}
x(i)\\
y(i)\\
\psi(i)\end{bmatrix}
+
\begin{bmatrix}
Vcos(\psi(i)+\dot{\psi}\Delta T)\Delta T\\
Vsin(\psi(i)+\dot{\psi}\Delta T)\Delta T\\
\dot{\psi}\Delta T\end{bmatrix}
$$

Now, as indicated in SLAM for Dummies, the measurement model can be represented as follows:
$$
\begin{bmatrix}
range = r\\
bearing = b\\
\end{bmatrix}
=\begin{bmatrix}
\sqrt{(x_L-x)^2+(y_L-y)^2}\\
tan^{-1}(\frac{y_L-y}{x_L-x})\end{bmatrix}
$$
Finally, the Jacobian matrix for the measurement model is as follows:

$$
H=
\begin{bmatrix}
\frac{\partial r}{\partial x_L}&&\frac{\partial r}{\partial y_L} \\
\frac{\partial b}{\partial x_L}&&\frac{\partial b}{\partial y_L} \\
\end{bmatrix}
=
\begin{bmatrix}
\frac{x_L-x}{r}&&\frac{y_L-y}{r} \\
-\frac{y_L-y}{r^2}&&\frac{x_L-x}{r^2}\\
\end{bmatrix}
$$

When only one sensor measurement such as bearing or range is only available, do my Jakobians simply become as follows?
$$
H_{bearing}=
\begin{bmatrix}
\frac{y_L-y}{r^2} \\
\frac{x_L-x}{r^2}\\
\end{bmatrix}
\quad H_{range}=
\begin{bmatrix}
\frac{y_L-y}{r} \\
\frac{x_L-x}{r}\\
\end{bmatrix}
$$

If so, how do I calculate the covariance and gain for single-measurement cases?

My innovation covariance is $S =HPH^{T}+ R$, where $P$ is my state covariance and $R$ is my measurement noise covariance.
Kalman gain is then $K = PH^{T}S^{-1}$.
With the current two measurement setup, $S,H, P, R$ are $2x2$ matrices.

When using one measurement, $H$ is $2x1$, $H^{T}$ is $1x2$, $P$ is $1x1$, $R$ is $1x1$ and $S$ therefore remains as $2x2$. However I am running into the issue where $S$ often becomes an invertible matrix, so I cannot calculate $K$ to predict the new landmark position and update the covariance. I am not sure why this is the case and I do not how to proceed.

Reference: https://dspace.mit.edu/bitstream/handle/1721.1/119149/16-412j-spring-2005/contents/projects/1aslam_blas_repo.pdf


问:
Computation of centrifugal and Coriolis matrix
So I have a seesaw-cart electromechanical system which I want to control. It has 2-DOF (the angle of the seesaw $\varphi$ and the cart position $s$).
I used the Euler-Lagrange method to get my equations of motion, which are as follows:

$$(J+m_Bb^2+m_Cc^2+m_Cs^2)\ddot\varphi - m_Cc\ddot{s} + \kappa\dot\varphi - g(m_Aa + m_Bb)sin(\varphi) + gm_C(s*cos(\varphi) - c*sin(\varphi)) = 0$$

$$m_C\ddot{s} + \frac{K_eK_t}{R_ap^2r^2}\dot{s} - m_Cc\ddot\varphi - m_Cs\dot\varphi^2 + m_Cgsin(\varphi) = \frac{K_t}{R_apr}U_a$$

Obviously, this is a non-linear problem which needs to be linearized so I can rewrite this into state-space form. Before I do that though I want to simplify my equations by writing them in the "infamous" dynamic equation:

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)+\phi\dot{q}=E_MU_a$$

where $M$ is my inertia matrix, $C$ is my centrifugal and Coriolis matrix, $g$ is my gravitational vector, $\phi$ represents the viscous dampng term in my rotational joint and $E_M$ would (in my case) be the unit vector.

I got $M$, $g$, $\phi$ and $E_M$:

$$M=\begin{bmatrix}J+m_Bb^2+m_Cc^2+m_Cs^2&-m_Cc\\-m_Cc&m_C\end{bmatrix},$$
$$g=\begin{bmatrix}-gsin(\varphi)(am_A+bm_B)+gm_C(s*cos(\varphi)-c*sin(\varphi))\\m_Cgsin(\varphi)\end{bmatrix},$$
$$\phi=\begin{bmatrix}\kappa&0\\0&0\end{bmatrix},$$
$$E_M=\begin{bmatrix}0\\\frac{K_t}{R_apr}U_a\end{bmatrix}.$$

All that is left is the $C$ matrix. Based on online searches I found that the following could be my answer:

$$C=\begin{bmatrix}m_Cs\dot{s}&m_Cs\dot{\varphi}\\-m_Cs\dot{\varphi}&\frac{K_eK_t}{R_ap^2r^2}\end{bmatrix}$$

Can anyone check if this is correct? I would greatly appreciate it :)


问:
What inertial information can I get from a SolidWorks object for simulation in MuJoCo?
MuJoCo needs me to enter the position of the inertial frame and the diagonal of the inertial matrix, (if I understand correctly). I'm not sure how to get this information from SolidWorks.

This object is what I'm working with.

I'm not sure if I entered the inertia information correctly and maybe there is another factor to this model to simulate the real life physics. Maybe there is a problem with the units for the inertia. I don't know.

I have an actuator in the hinge of this object. It tends to make the object fly around like a butterfly. I expect the object to fall to the floor and open and close like a normal egg slicer with an actuator with no mass in the hinge.

Here are the docs for inertial in MuJoCo.

I don't know what an "inertial position" is exactly. This is what it says, "pos: real(3), required
Position of the inertial frame. This attribute is required even when the inertial properties can be inferred from geoms. This is because the presence of the inertial element itself disabled the automatic inference mechanism."

This is my code, and below it is a picture of the information from SolidWorks.

```<code><mujoco model="egg_slicer">
    <compiler angle="radian" meshdir="meshes/"/>
    <default class="viz">
        <geom contype="5" conaffinity="1" group="1" type="mesh" rgba=".51 .6 .7 1"/>
    </default>

    <asset>
        <mesh name="base"         file="egg_mash1.stl" />
        <mesh name="swing"         file="egg_mash2.stl"/>
    </asset>
    <asset>
        <texture type="skybox" builtin="gradient" rgb1="0.6 0.6 0.6" rgb2="0 0 0" width="512" height="512"/> -->
        <texture name="texplane" type="2d" builtin="checker" rgb1=".25 .25 .25" rgb2=".3 .3 .3" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
        <material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
    </asset>
    <worldbody>

        <geom name="floor" pos="0 0 -0.5" size="0 0 1" type="plane" material="matplane"/>
        <light directional="true" diffuse=".8 .8 .8" specular=".2 .2 .2" pos="0 0 5" dir="0 0 -1"/>

        <!-- start building our model -->
        <body>
            <joint type="free" limited='false' damping="0" stiffness="1" armature="0"/>

            <geom name="slicer_base" mesh="base" class="viz"/>
            <inertial pos="0 0 0" mass="0.49" diaginertia="0.01 0.02 0.02"/>

            <body pos="0 0 0">
                <joint name="hinge" pos="228.60 120.56 33.58"
                axis="0 0 1"
                limited="true" range="-3.0 0" /> -->
                <geom name="slicer_swing" mesh="swing" class="viz"/>
                <inertial pos="0 0 0.01" mass="0.70" diaginertia="0.02 0.02 0.03"/>
            </body>
        </body>
    </worldbody>

    <actuator>
        <motor name="hinge" gear="100" joint="hinge"/>
    </actuator>
</mujoco>
</code>```


问:
Definition (and differences) between robotic platform and semirobotic platform
I'm looking for the definitions (and the diferences) of robotic platform and semirobotic platform. Online it's quite impossible to find them. Can you share your knowledge and suggest me a good source please?</p>


问:
Custom sensors and materials simulation in Gazebo: how to simulate specific sensors behaviors for heterogenous materials / buildings
We are currently working on a simulation for a drone that would evolve in an urban environment. It would flight in front of facades and thus must avoid collision into walls, balconies, windows, etc. To fulfil this task, we added several sensors (lidars, sonars and cameras) to the drone that will jointly understand the surrounding environment by fusing the sensors information. One interesting case would be to have windows that are invisible to lidars and stereo cameras but visible by sonar. Even if the use case seems simple, we couldn’t find any resources or information to simulate different materials (especially their opacity or their transparency) or model sensors behaviors according to the object they face (apart from the transparency it would be interesting to specify different error noise according to the surface seen by a sensor).

Do you think Gazebo is the correct simulation environment to test this kind of behavior? If yes:

Is there a way to directly specify textures or opacity characteristics that have physical meaning and are not only visual?

Is there a way to program the sensors to behave differently according to the materials they’re detecting or not?

Thank you!


问:
Communication error
My Robot arm and controller battery got ablated it showed H1600 mechanism error.. did ram reset of the controller but controller not seeing robot arm. it is RV-2FB-D1-SM15 Robot arm and CR750 D controller. During the ram reset it is expected the controller should recognize the robot arm but all it shows is user on the teach Pendant.</p>

Title: Communication error


问:
Why would we solve AX=XB instead of just using the Kabsch algorithm?
I have a high precision stereo camera and a robot which are independent units (the camera is not mounted to the robot). I wish to compute a transformation from the camera frame to the robot base frame. The robot kinematics are characterised, so such a calibration would enable me to manipulate world objects using the camera for perception.

I'm assuming the AX=XB approach is familiar to someone reading this question. But with Kabsch I was thinking:

Set the robot end effector to at least 3 positions (not co-linear) with a calibration pattern on the end effector.

Don't even bother getting the pose of the calibration pattern. Just read the center co-ordinate directly off the point cloud from my camera. (so the calibration pattern is merely acting as an easily detectable keypoint)

Apply Kabsch (with no scaling) to get the transformation matrix.

I also realised the Kabsch approach is possible without a stereo camera, as the PnP step with a 2D camera would still give me the center co-ordinate of the calibration pattern (albeit much less precise than my expensive stereo camera). But still, this makes me wonder whether I'm missing something with my Kabsch approach.


问:
What exactly is meant by the "Bias instability over temperature range" specification on IMUs?
I'm trying to become educated on IMU specifications, and my understanding is that when comparing IMUs, the Gyros make the big difference. I am looking at the InertialLabs IMU-Nav-100:

https://inertiallabs.com/wp-content/uploads/2021/10/IMU-NAV-100_Datasheet.rev1_.6_October_2021.pdf

They specify the "bias instability over temperature range" as being 15deg/hr. What exactly does this mean?

I have an understanding of what a regular gyro bias instability looks like: a wandering bias with some standard deviation and time constant. But I don't know what the "over temperature range" looks like or means.

Does this mean that, over the operating range, in the worst case (at a particular temperature) the moving bias has a standard deviation of 15deg/hr, and therefore some temperatures are better than others? Does it matter since the document also states that it is "internally calibrated for temperature?"


问:
How to rotate a handle shown in the below picture using raspberry pi

Need ideas in the direction for to rotate below circled handle to be rotated robotically using raspberry pi etc


Title: this is matlab code for drawing circle by 6dof arm robot but i don't know how to limit the joint's angles

Body:
```<code>clear,clc

R=175;
H= 150;
N=5;
c=[200,200];
theta=linspace(0,2*pi,N);
x= R*cos(theta)+c(1);
z= R*sin(theta)+c(2);
y= ones(1,length(x))*H;
End=[x',y',z'];
% plot3(x,y,z)

L(1)= Link('d', 0 , 'a', 44.51, 'alpha', pi/2);
L(2)= Link('d', -68.41, 'a', 257.4, 'alpha', 0);
L(3)= Link('d', 0 , 'a', 0, 'alpha', -pi/2);
L(4)= Link('d', 331.5 , 'a', 0, 'alpha', pi/2);
L(5)= Link('d', 0 , 'a', 0, 'alpha', -pi/2);
L(6)= Link('d', 0 , 'a', 72, 'alpha', -pi/2);
Arm= SerialLink(L, 'name', 'Inmoov');
t =(0:1:N-1);
time = t';
a=1;

for i =1:N-1
xa1 = End(i,1) ; ya1 = End(i,2) ; za1 = End(i,3);
xf1 = End(i+1,1) ; yf1 = End(i+1,2) ; zf1 = End(i+1,3) ;

stylus=true;

T11 = SE3(xa1,ya1,za1)*SE3.Rz(90);         % initial end effector pose
T12 = SE3(xf1,yf1,zf1)*SE3.Rz(90);         % final end effector pose

Ts1 = T11.ctraj(T12,length(t));

q1 = Arm.ikine(Ts1, [0 0 0 0 0], 'mask', [1 1 1 1 1 1]);

p41=transl(T11);
p42=transl(T12);

xa1=[p41(1),p42(1)];
y1=[p41(2),p42(2)];
z1=[p41(3),p42(3)];

qs1 = Arm.ikine(T11, [0 0 0 0 0], 'mask', [1 1 1 1 1 1]);

% 0 < q1(1) < pi/2;
% q1(5)=0;

% Arm.plot(q1);
hold on
if stylus==true
    plot3(xa1,y1,z1)
end
Arm.plot(q1);
A(a,:)=qs1;
 a=a+1;
end
A=[A;A(1,:)];
th1=A(:,1)*180/pi;
th2=A(:,2)*180/pi;
th3=A(:,3)*180/pi;
th4=A(:,4)*180/pi;
th5=A(:,5);
%  figure
%  plot(th1,time)

<span class="math-container">```</span>
</code>```


问:
Uncontrolled drift of Visual Inertial Odometry
I'm trying to use the VINS-Fusion package to obtain odometry for a quadcopter. The primary sensor is an Intel Realsense D435i. I've calibrated the camera and IMU extrinsics using Kalibr. IMU Calibration using imu_utils gives noise standard deviation values that are too low to work with so I had to use values that are ~10-100x the values obtained from calibration.

The system works fine with this configuration (apart from trajectory errors) in sunny outdoor conditions but lately we've been seeing instances of uncontrolled drift in the Odometry. It works fine during one flight and fails inexplicably in the next. Sudden changes in brightness might be the issue so I've been looking into improving the image quality but apart from this I'm out of ideas.  I've come across issues on the OpenVINS repository where it's suggested that the IMU data of the D435i is faulty. How should I go about making the system more robust? Should I work on improving my calibration results or get a better sensor?


问:
Sample code for L298N H bridge Arduino Motor Driver not working
I am quite new to robotics, arduinos, motors and that sort of thing so there is a very strong chance I am missing something very obvious. However, I have looked up multiple tutorials online, none using the exact same H Bridge as I am using.
I was wondering whether anybody has also uses the L298N H Bridge who could tell me why my code isn't working. (Link to product I am using)

Code:

```<code>
void setup(){
pinMode(13,1);
pinMode(12,1);
pinMode(11,1);
pinMode(8,1);
pinMode(10,1);
pinMode(9,1);
}

void loop(){
digitalWrite(13,1);
digitalWrite(12,0);
digitalWrite(11,1);
digitalWrite(8,0);
analogWrite(10,255);

}
</code>```

Thank you for your time.


问:
learning (embedded) electronics
I am an aerospace engineer (currently in grad school) and I really want to get into (embedded) electronics.
But I have this problem:
I understand the theory fairly well, I took an edX course in circuits and had no problem. I can build projects from the internet. However, I have a very hard hard time connecting the theory with the practical part, understanding why projects are done the way they are done and I have a hard time to design my own projects!

Please help!

I'd appreciate the following:
-General tips: how did you learn it? How is your workflow? What should I do? Which steps should I take?
-Books: which hands on books and websites can you recommend? I am looking for books and website that are practical but also explain the why
-Kits: What kits can you recommend that combine the theory with the practical?
-Anything you think is important

Thank you for your time!

Title: learning (embedded) electronics


问:
Monocular SLAM position estimation
I have been reading about mono SLAM and I have a couple questions which are not so clear:

When doing pose estimation, I have seen people both reference homography transformation and also reference 5\8 point algorithm. Can homography be used? It appears it is only made to work for a planar surface but I get the impression it has been used for SLAM.

How do you maintain scale consistency between pose estimates? It's understandable that absolute scale is not recoverable but how do you make sure you maintain the same scale when estimating pose 2 from image 3 following estimating pose 1 from image 2, starting with pose 0 and image 1? Or in other words, how do you transfer scale information from the first estimate to the second?

Title: Monocular SLAM position estimation


问:
Robot increases in length, trying to keep robotic along previous position trajectory
Sorry for the confusing title, this seems to be a difficult problem to concisely describe. I am working on a 50-DOF (yes, 50) robot that is comprised of "elbow macaroni" links that rotate around their end normals, as shown below

So far, I have implemented inverse kinematics using CCD, which works very well and solves fast even for the large amount of links. However, I am still having a problem trying to get the robot to "stay on" it's previous path. Let me explain:

The robot is continuously "fed-out" by discrete units of 0.1" or so. As it feeds out, the desired behavior is to move the end effector in a certain path, while the links behind the end effector track the previous path in space of the end effector. So, if the end effector moved forward in 4 steps of 0.1" to points A, B, C, and D - the centerline of each of the links would travel along (or as close as possible to) the interpolated path as possible.

Something like fixing the actuator angles wouldn't work since the position of each link change. And if I fix the position of each point to some distance down the path, I have no guarantee that the configuration of the robot would be valid.

Any thoughts on solving this problem?


问:
SMC: Is there a way to control two sliding surfaces with just one control input?
This is a question related to Control Systems.

I've been searching papers all over the internet with very little success. I know that traditionally we want one control per sliding surface but If anyone has any papers in how to do something like this I'd appreciate it.

UPDATE: If anyone wants context, I'm doing a project about guiding a helicopter/drone into its final landing position (Not doing the descent). It goes straight into the final position without making any turns. Pretty straightforward. The problem is that I have two sliding surfaces. This is its trajectory:
https://drive.google.com/file/d/1I0l5hrMvcyCqEIWbN7Ya_Pnfy8dOnqOL/view?usp=sharing


问:
Aligning accelerometer readings with vehicle's frame of reference
I have a device with an accelerometer installed in a vehicle. Its orientation in relation to the vehicle is unknown. So I am looking for a reliable way to align the coordinate system of the accelerometer with that of the vehicle. This will allow me to determine acceleration and braking on the x-axis of the vehicle (the driving direction), as well as to identify right and left turns (acceleration around the z-axis).

I am able to capture the stream of accelerometer readings when the vehicle is stationary on level ground, and average the readings (I am using exponential moving average). Let's say the averaged reading (in mG) is {271:-507:-785}. I am also able to accelerate while driving straight ahead (for say 5 seconds), and collect the corresponding accelerometer readings.

My question:

Is it possible, based on the data obtained as described above, to determine the rotation matrix  with which I can rotate any future accelerometer readings so they as to align them with the vehicle's frame of reference? Can this be done with a single matrix, and if so, how exactly?

I have found this answer to a similar question. I can understand the general explanation (in terms of what needs to be done), but don't know how to execute it (the "how" aspect). Will appreciate any help as I am not an expert in the maths involved (a simple example would be great).


问:
Mapping IMU readings from body frame to navigation frame
I'm trying to combine IMU displacements with the time of flight sensor readings in order to navigate through the indoor environment with a non-linear Kalman filter variant. In the graphic below, I tried to give an example of the setup. S0, S1, S2, and S3 are the time of flight anchors that can measure the distance between them and the tracked device. Their locations are given in the parentheses next to their label as meters.

I've read some papers related to this work. The thing that I could not understand is how can I project my IMU readings from the body frame to the navigation frame? The navigation frame is the arbitrary frame that I've chosen and the body frame changes with the orientation of the device. In the papers, they are using the 3D rotation matrix below to project their IMU vectors onto the navigation frame (c and s are cosine and sine functions, and the symbols are Euler angles). Aren't the Euler angles that I am going to get from a 6-axis IMU relative?


问:
Move a Simulated Quadcopter in a straight line
I'm trying to move a quadcopter in a simulator from (x0, y0) to (x, y) in a straight line at a constant speed. The simulator takes x and y inputs in m/s. Altitude as an input is a constant value in meters.

I found the following formula below which works relatively well but the quadcopter veers off the straight line course slightly. These values are calculated every time step.

Given a constant speed s, initial position (x0,y0), and final position (x,y)

Distance d is calculated as

$$ d = \sqrt{(x-x0)^2 + (y-y0)^2}  $$

Vectors calculated as
$$ Vx = \frac{s}{d}(x - x0)  $$
$$ Vy = \frac{s}{d}(y - y0)  $$

Is there a better way to calculate these inputs or should I implement some kind of PID / Cross track controller to get the quadcopter to stay moving in as straight a line as possible?


问:
Which are the Euler and Runge-Kutta odometry formulas for a four mecanum wheels omnidirectional robot?
I have a omnidirectional robot with four mecanum wheels and I have computed its linear and angular velocities starting from wheel speeds expressed in ticks. Now, to compute odometry, I have to integrate with Euler and Runge-Kutta methods, but I can't find the formulas for this type of robot. Which ones can I use?</p>


问:
How to translate a joint configuration
I'm currently working on a basic implementation of the RRT algorithm to control my 6DOF robotic arm.
But how can I reduce the node to be in the radius of the nearest node of my graph without using the inverse kinematic ? If I use the IK, I'm losing all the informations I had with the joints.

So what is the way to do a "translation" of a joint configuration to have a new one which is in the radius of another joint configuration ?

Title: How to translate a joint configuration


问:
Simple web interface with beaglebone black
This is actually a very simple question, but I'm lost at the moment.

I was using a beaglebone black for a school project. It controls a bunch of motors and actuators,etc. We wrote everything in C++, and made libraries of functions. When a main program calls them, the functions run just fine.

Recently we have been told to demo our progress so far. The main program is nowhere near done, so we were thinking of some sort of web interface that can execute the complied C++ program on command. We were hoping to get the server hosted on the board, and access it via LAN from other PCs. But I've never done this before and have no idea where to start. Does node.js (with the 'bonescript') going to be of any help? Or is there a simpler way with basic HTML?

I only have a few days to figure it out, so I didn't want to waste time looking at the wrong methods.


问:
robot_localization not fusing imu data
According to the documentation in : http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
I was able to transform the imu data header fram from "imu_link" into "base_link" through a tf transformation in a node.

However, somehow my imu data is not being fused into the ekf_localization node.
I mean I was able to see the yaw changes as I move my robot. But when I subscribe to the odometry/filtered, it still shows my yaw to be at 0 degree.

I have double checked with the raw data from the imu and verified it outputs data as shown below:
The data below are my imu message after I move the robot. This which reflects the changes of quaternion in the orientation

```<code>---
header:
  seq: 9520
  stamp:
    secs: 1617336774
    nsecs:  71053028
  frame_id: "base_link"
orientation:
  x: -873.0
  y: -496.0
  z: 0.0
  w: 16353.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
  x: 0.0
  y: 0.01
  z: -0.18
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
</code>```

Below is my configure file for the ekf kalman filter.

```<code><include file="$(find roboteq_driver)/launch/controller.launch"/>
<include file="bno055.launch"/>
<node pkg="robot_localization" type="ekf_localization_node"
    name="robot_localization_ekf_node_continuous"
    clear_params="true">
    <param name="frequency" value="30" />
    <param name="sensor_timeout" value="0.1"/>
    <param name="two_d_mode" value="true" />
    <param name="publish_tf" value="true" />
    <param name="odom_frame" value="odom" /> <!-- publish a new data to odom-->
    <param name="base_link_frame" value="base_link" />
    <param name="world_frame" value="odom" /> <!-- change here if we want to add map frame later-->
    <param name="odom0" value="/drive_odometry" />
    <rosparam param="odom0_config">
        [false,false, false,
        false, false, false,
        true, true,false,
        false, false, false,
        false, false, false]</rosparam>
    <param name="odom0_differential" value="false"/>
   <param name="odom0_relative" value="false"/>
    <param name="odom0_nodelay" value="true"/>

    <param name="imu0" value="transformed_imu"/>
    <rosparam param="imu0_config">
        [false,  false, false,
        false, false,false,
        false, false, false,
        false , false, true,
        true, false, false]  <!-- from IMU message acceleration-->
    </rosparam>
    <param name="imu0_differential" value="false"/>
    <param name="imu0_relative" value="false"/>
    <param name="imu0_nodelay" value="false"/>
    <param name="imu0_remove_gravitational_acceleration" value="true"/>
    <param name="imu0_queue_size" value="10"/>

    <param name="use_control" value="false"/>
    <!--final odom message-->
    <remap from="/odometry/filtered" to="/odometry/filtered_odom"/>

</node>
</code>```

Odometry message from : drive_odometry

```<code>header:
  seq: 1392
  stamp:
    secs: 1617336632
    nsecs: 897039890
  frame_id: "odom"
child_frame_id: "base_link"
pose:
  pose:
    position:
      x: 10.4
      y: 4.43
      z: 0.0
    orientation:
     x: -873.0
     y: -500.0
      z: 0.0
      w: 1240.0
  covariance: [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4]
---
</code>```

So In this case, according to the kalman filter in the library, since I only use the yaw velocity from IMU in the configure file to determine the current yaw position of the robot, I am expecting the yaw value output from odometry/filtered_odom to be about the same as the yaw value in the IMU message.

However, when I subscribed to the odometry/filtered_odom,
I find the orientation to be:
x: 0.0
y: 0.0
z: 0.0
w: 1.0

Note: I only allow the angular velocity of IMU to be fused with the filter in determining the yaw direction.  I did not use the yaw message from the odom, because the Odom message is generated by my calculation based on the feedback from the wheel's encoder.

Title: robot_localization not fusing imu data


问:
orocos ros integration, createStream causes execution to stuck in a loop
I'm integrating orocos with ros, basically i created a component that read data from some input ports and write on output ports that create streams, because i want to public this values on ros topics and then read those values in plotjuggler.

So I created this component without any problem and work perfectly. After that i created used another ssd configured in the same way as the previos ssd in which i build this component that publish data on ROS but on this new ssd when i call the first createStream() it stuck without any error as if it is in an infinite loop. The only thing that i notice different from the other ssd is that i got the following message

```<code>[2022-05-06 12:33:28] [connect] WebSocket Connection 192.168.2.13:8081 v-2 "WebSocket++/0.7.0" /socket.io/?EIO=4&transport=websocket&t=1651833208 101
</code>```

to create the stream I'm using, in the c++ code the following code

```<code>port.setName(<port_name>);
port.doc(<doc>);
port.createStream(rtt_roscomm::topic(<topic_name>));
</code>```

i also tried to create the stream in the ops file, but i get exactly the same result, when it reach that point it stuck.

I repeat again that i have a working version of this component on a similar ssd, so this means that the code inside the component is should be correct as well as the ops file.

I'm on ubuntu 18.04.06 LTS, I'm using ROS melodic.

Is 1 week that I'm working on this issue without success, so i tried a lot of different things but at the end when i create a stream it stuck.

Ros is working because if i create a topic and i publish something from terminal or a create a simple ros package i can send and receive data, also orocos is working because i have different component running without problem.

I know that with so few information is difficult to solve the problem but why when i create a topic, with createStream, i get stuck in this loop?

There is any known case in which it is possible to remain stacked in this way during the creation of a topic in ros?

I perfectly know that it is very difficult to find a solution in this way even because it is a week that I'm struggling with this issue, and i tried a lot of different things.

I'm quite sure that the problem is when I create the ros topic because I have 3 ways to do it, that are

inside the componet class using createStream(rtt_rostopic::topic("topic"))

inside the componet after calling ros::init etc using the  node.advertise<std_msgs::String>("topic", 1000);

in the ops using stream("my_component.port", ros.comm.topic("topic"))

and with all these methods i get always the same problem. But in the first ssd createStream() works perfectly so i don't think the problem is in inside the component's code.

Another thing that I'm sure is that the topic does not exist, I want to create it and after that publish data on it so I'm creating the topic and I'm sure that this topic does not exist because if i run rostopic lists i cannot see the topic instead i can see the node that is running using rosnode lists.

One thing that i notice now is that in the rosout.log i have the following error

ERROR /mynode [....] [topics: here there are some topics till the one i get stuck ] [registerPublisher] Failed to contact master at [:11311] Retrying ...


问:
TORQUE OF ARDUINO CAR
when an obstacle is in front, the Arduino car should turn left or right. But it is not turning properly. So, what can be the problem?</p>

Title: TORQUE OF ARDUINO CAR


问:
The costs of using existing 6 axis kuka/abb robots and existing vision systems in picking and placing tasks
I would like to use a Kuka/abb 6 axis robot and a machine vision system to pick and place a variety of metal drill bits in size ranges from 0.5mm (ascending up 0.5mm per cylinde) to 13mm in metric and then 1/16 of an inch ascending to 9/64 of an inch. The machine would not have to differentiate between the bits, niether drill bit would weight more than 1kg.

Crucially at the beginning and end of the picking and placing I would like to inspect the very tip of the cylinders to be inspected for a 118 degree chamfer on one end of the bit which should be present regardless of drill diameter of length.

I am lead to believe that if the drill bits are placed end up on a conveyor belt and always the the same place its relatively low cost but crucially if the kuka 6 axis robot has to find the drill bits then the cost increases dramatically, is this true?


问:
Multiple Stepper Synchronization with updating position control at specified speed
So I have build a 6-axis manipulator having all the degree of freedom. Currently I have used TeensyStep library for controlling synchronization of 6 stepper axis. Also I have tried AccelStepper library for controlling 6-axis stepper synchronization. Until now everything works great.

But now I have to move the robot (all 6-axis), with the help of numeric control code.
(Basically what happens in 3D printer control with Gcode)

For example :

```<code>N001   A1 150,  A2 32.6, A3 -50, A4 -75.8, A5 50, A6 10.2

N002   A1 120,  A2 42.6, A3 -55.0, A4 88, A5 150, A6 10.8

N003   A1 50,  A2 3.6, A3 -5.50, A4 -5.8, A5 150, A6 1.2

N004   A1 158,  A2 38, A3 80, A4 7.8, A5 44.4, A6 100.2
</code>```

where A1 to A6 represents angle values in degree.

With AccelStepper or TeensyStep library I can move to give code line(N00x) in a synchronous move. So when I feed this code lines it will go from current position accelerating-to maxSpeed- then deaccelerating to target position.

What I want to do is to move from one code line to next code line and to next in One-Continuous move. That is no accelerating from start position to setSpeed/maxSpeed to first point - then to next point - then to next point and deaccelerating finally at the end of code line.
So if there is new code line in the buffer it will not deaccelerate, go to current target with maxSpeed/setSpeed and then to new target with updated speed and so on.

Basically what Computer Numeric Control(CNC) machines do.

I tried searching regarding this in many different platforms but can't find anything closely related to this.

Thank you.


问:
Mounting the ur5e robotic arm on a table
I am working on testing motion planning algorithms on a robotic arm. My arm collides with the ground while moving it from one point to another point. My solution to this problem is to mount the arm on a table. How do I do that? And is there a better solution to the problem?


问:
Setting start and end headings during Pure Pursuit path generation
I've recently been looking into the Pure Pursuit algorithm, and have found this post from FRC Team 1712 which has a great explanation on how to implement it. However, a part that confused me was the Path smoothing algorithm. Here is code for that (which I have written in C++20 based on FRC Team 3256's implementation of the algorithm):

// note that this code needs support for `constexpr std::vector`s, meaning you need
// a compiler that supports that in C++20 (such as GCC v12)

#include <vector>
#include <cmath>

struct Waypoint
{
  double x;
  double y;

  // ... (other data doesn't matter for this algorithm)
}

struct PathBuilder::Util
{
  using PathVector = std::vector<Waypoint>;

  constexpr static PathVector smooth(const PathVector &path,
                                     const double tolerance,
                                     const double a, const double b)
  {
    // algorithm borrowed from FRC Team 2168, cleaned up using
    // FRC Team 3256's implementation
    PathVector newPath(path);
    double change = tolerance;
    while (change >= tolerance)
    {
      change = 0.0;
      for (int i = 1; i < path.size() - 1; ++i)
      {
        const Waypoint &oldPoint = path[i];
        const Waypoint &prevPoint = newPath[i - 1];
        const Waypoint &nextPoint = newPath[i + 1];

        Waypoint &currPoint = newPath[i];
        auto currXCopy = currPoint.x, currYCopy = currPoint.y;

        currPoint.x += a * (oldPoint.x - currPoint.x) +
                       b * (prevPoint.x + nextPoint.x - 2 * currPoint.x);

        currPoint.y += a * (oldPoint.y - currPoint.y) +
                       b * (prevPoint.y + nextPoint.y - 2 * currPoint.y);

        change += std::abs(currXCopy - currPoint.x);
        change += std::abs(currYCopy - currPoint.y);
      }
    }

    return newPath;
  }
}
```

The problem I have with this code is that it never awknowledges or uses the start direction/heading or the desired end direction/heading of the robot, which means that it could generate a path in a different direction than what the robot is facing. I also looked into other sources, such as this by vinesmsuic on a GitHub blog, but often found this same "simpler" algorithm. However, looking at other implementations, such as this one by zhm-real on GitHub, they often use Reed-Shepps paths or Cubic/Quintic Splines (in fact, they mention using Quintic Splines in the paper by FRC Team 1712 instead of this smoothing algorithm).

This led to a few questions:

How could you tell the "simpler method" the desired heading and the start heading? I had two ideas for this:

Create an extra point in the direction of where the robot is pointing whenever you create a path, so it smooths around that point and makes it easier for the robot to follow (I haven't tested this) OR

Just don't do anything to the path and let Pure Pursuit handle it on its own. (Doesn't this defeat the purpose of path smoothing?)

Is this "simpler method" better or worse than using Reed-Shepps or Cubic/Quintic Splines? Which one is the "best", or are they all equally comparable/does it really matter practically which one is used?

Right now, I'm thinking the best method is the first option for the first question (adding an extra point) and that this simpler smoothing algorithm will probably work fine after doing that.

Thanks in advance!


问:
Analytical solution to inverse kinematics of 5 dof robot arm
the followings are the equations I worked out for the inverse kinematics of a 5 dofs robotic arm.
First of all, the kinematic diagram:

For the joint angles $q_{2}, q_{3}$ and $q_{4}$ I resorted to the IK solution of the 3-link planar arm.
px,py,pz are desired position coordinates, $\phi$ should be the angle between the $z_{5}$ axis and the horizontal plane $x_{0}-y_{0}$ which determine the orientation in the plane where the joints lie, see second image.

First compute

$q_{1} = atan2(p_{y}, p_{x})$

Then, see third image

$r = \sqrt{p_{x}^{2}+p_{y}^{2}}$

$r_{w} = r - d_{5}*cos(\phi)$

$r$ and $r_{w}$ are the projections on the $x_{0}-y_{0}$ plane

$p_{wz} = p_{z} - d_{5}*sin(\phi)$

$s_{w} = \sqrt{r_{w}^2+(p_{wz}-d_{1})^2}$

Apply cosine theorem to triangle a2-a3-sw:

$s_{w}^2 = a_{2}^2+a_{3}^2 - 2*a_{2}*a_{3}*cos(\pi-q_{3})$ ->

$cos(q_{3}) = \frac{s_{w}^2 - a_{2}^2 - a_{3}^2}{2*a_{2}*a_{3}}$

Two solutions for $q_{3}$:

$q_{3}= acos(cos(q_{3}))$

$q_{33}= -acos(cos(q_{3}))$

Then

$\alpha = atan2(p_{wz} - d_{1}, r_{w})$

Apply again cosine theorem to the triangle a2-a3-sw

$a_{3}^2 = s_{w}^2 + a_{2}^2 - 2*a_{2}*s_{w}*cos(\beta)$ ->

$cos(\beta) = \frac{s_{w}^2 + a_{2}^2 - a_{3}^2}{2*a_{2}*s_{w}}$

$\beta = acos(cos(\beta))$

Two solutions for $q_{2}$

$q_{2} = \alpha - \beta$

$q_{22} = \alpha + \beta$

Two solutions for $q_{4}$

$q_{4} = \phi - q_{2} - q_{3}$

$q_{44} = \phi - q_{22} - q_{33}$

Now, how to get $q_{5}$??

To apply these equations I need to specify the target pose with px,py,pz, $\phi$. I tried to specify the target pose that I want the robot to reach using the following matrix multiplication to transform from the {0} frame to the {5} frame:
$D(x)*D(y)*D(z)*R_{z}(atan(py/px))*R_{y}(\pi/2-\phi)*R_{z}(\pi+\theta_{5})$

From this matrix I get the position vector px,py,pz and the $\phi$ angle. $\theta_{5}$ should match the $q_{5}$ joint angle, which I'm not sure how to get, but then I thought I could specify directly the desired last joint rotation without any computation.

Are this solution and the way I choose the target pose correct? Give me some feedback please


问:
What is Rotation when pose is looking back?

I've model a relative pose between two cameras. $T_0$ is origin and $T_1$ has the relative pose.

The $Camera_0$ is facing a the map point $X$, and I can get a vector $v_0$ from $Camera_0$ toward $X$.

How to determine the rotation matrix of $Camera_2$, which facing direct back? following a $-v_0$

Thanks.


问:
Lidar data and map misaligned after rotation
I am working with the telepresence robot of the Ohmnilabs (here a picture of the robot). I setup the navigation stack and I tuned the parameters and everything worked fine for the map creation phase.

When I try the autonomous navigation, after few goals, the lidar data showed on rviz is misaligned respect to the map.

I thought the problem was in the odometry and my thoughts were confirmed when I followed the step 1.2 in this guide. In particular, the robot failed the rotation test.

Can you please tell me what can I do to avoid or reduce the misalignment? Are there tutorials to solve this problem?


问:
Sending livefeed wirelessly from a Canon Camera to an Arudino/Raspberry Pi
I have a camera which is WiFi enabled, I was wondering if can do inference and detection wherein I put my model on an Arduino, and do the detection with a high resolution camera instead of something along the lines of an ESP32 Cam Module. Looking for solutions to enable this.</p>


问:
What is the format and required content to define a Joint trajectory using the RoboAnalyzer simulator?
I'm trying to learn how to use the Roboanalyzer simulation tool to model and then display the torques required to pivot a 2 DoF arm. ( [http://www.roboanalyzer.com/][1] )

I see that the tool has a feature that allows me to create an input file ( a CSV file) as the required joint trajectory.
The simulator provides an example file ( named jointinput_2R_trajectory.csv ). That works.

I want to create my own input file,
but the simulator documentation does not explain what format and structure of data is required in an input file.
The above example file has 7 columns, A to G
I can see that the first column is the step increments.

what is the data in the other columns ?

I know this seems like a tool-specific question ... but I'm hoping that someone  out there has used the tool and can help with an answer. I have already tried to contact the tool designers , but got no reply. I am not sure they are in operation these days.

Many Thanks
[1]: http://www.roboanalyzer.com/


问:
Robot-to-robot relative position over time?
I have 3 robots and I know the starts and end coordinate of each of them. For example for Robot 1 I have Xr1, Yr1 and Xr2,Yr2 coordinates pair , same for Robot2 and 3. I have lateral cameras so can obtain the visual information. I would like to get the robot-to-robot relative position over time. So I can record all Sensor data and do the localization offline . The camera update is every 0.5 seconds. And each robot covered 300 degree horizontal field of view. How can I approach this problem and any possible solution? Is the Hidden Markov Model chains would be a good approach? OR semantic Mapping can be helpful?
Thanks</p>