问:
Drone Development using ROS Noetic on Ubuntu 20.04
I installed ROS Noetic on Ubuntu Mate 20.04 in my Raspberry Pi but couldn't find any specific tutorial to follow along for Noetic. Most are available for Melodic, which doesn't work with the latest one.

I tried following with one of JdeRobots guides: https://github.com/JdeRobot/drones

But the following command doesn't seem to work:

```<code>sudo apt-get install ros-noetic-jderobot-drones
</code>```

Output:

```<code>Reading package lists... Done
Building dependency tree
Reading state information... Done
E: Unable to locate package ros-noetic-jderobot-drones
</code>```

I want to do Drone Development using Noetic, but there's not much I could find specific for Noetic or I could convert old commands to. I tried searching Github repos but they aren't specifically for Noetic.

答:
It appears that the package you're looking for is available in noetic: https://index.ros.org/p/jderobot_drones/#noetic

If you're getting that error it's likely that you have not setup the ROS package sources: http://wiki.ros.org/noetic/Installation/Ubuntu#Installation.2FUbuntu.2FSources.Setup_your_sources.list

If you are still having trouble please edit your question to add more information about how you setup your system. To help you most effectively, how to reproduce your problem is important to share.


问:
ROS Laser Scan rotates with the robot in RVIZ
I am currently working on developing a ROS application for autonomous navigation using AMCl. I am first trying to get move_base to work before continuing with AMCL. I came across an issue that shows the LaserScan rotating in a similar fashion to the robot when I use teleop_twist_keyboard to manually rotate the robot. I am using the rplidar_ros package to run the lidar and the create_autonomy package for controlling the robot. I have tried searching for an issue similar to mine and could not find one.

Hardware: - Robot: iRobot Create 2 - Lidar: RPLidar A1M8 - Computer: Jetson NX with Ubuntu 18 running ROS Melodic

Launch File:

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

  <arg name="use_robot" default="true" />
  <arg name="use_gazebo" default="false" />
  <arg name="gui" default="false"/>

  <include if="<span class="math-container">$(arg use_gazebo)" file="$</span>(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="<span class="math-container">$(find example_bot_sim)/worlds/maze.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$</span>(arg gui)"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
    <arg name="verbose" value="true"/>
  </include>

  <!-- Spawn Robot
       This must be the same robot type as the robot application
  -->
  <include if="<span class="math-container">$(arg use_gazebo)" file="$</span>(find example_bot_sim)/launch/spawn_bot.launch">
      <!-- Override arg parameters here e.g,
          <arg name="x_pos" default="10.0"/>
          <arg name="y_pos" default="5.0"/>
     -->
  </include>

  <include if="<span class="math-container">$(arg use_robot)" file="$</span>(find ca_driver)/launch/create_2.launch" />

  <node pkg="tf" type="static_transform_publisher" name="laser_broadcaster" args="0 0 0 0 0 0  base_link laser 100" />

  <include if="<span class="math-container">$(arg use_robot)" file="$</span>(find rplidar_ros)/launch/rplidar.launch" />

  <include file="$(find example_bot_sim)/launch/gmapping.launch"/>

  <include file="$(find example_bot_sim)/launch/move_base.launch"/>

  <node pkg="rviz" type="rviz" name="rviz"/>

</launch>
</code>```

Initial robot pose:

Robot pose after moving:

rosrun tf view_frames output:

答:
Behavior like this suggests that your odometry is not being computed properly when the vehicle turns. It's hard to tell but the model appears to have rotated about 90 degrees but the laser scans appear to have rotated close to 180 degrees comparatively. There may be an issue with your wheel radius or some similar setting. Alternatively there could be a sign error on the directions. And thinking about it for a doubling error like that you may have the laser indicated as mounted upside down such that left is right and right is left for the laser. If that's the case you just need to invert the axes for the static transform publisher.

Note that an animation or video of the robot when it's turning will tell you a lot of information about the errors as you can see the progression. It's also helpful to enable the transform tree to debug where they are in space while things are updating.

ContentLicense: CC BY-SA 4.0


问:
Caculate/Specify the covariance matrix for a omniwheel robot
I have an omnidirectional robot (x, y and rotation around z), which I would like to use the SLAM-algorithm from ROS. But the odometry from the robot has a drift, which makes some error. To improve the odometry error I need to calculate or specify the covariance matrix. How do I do that on a real system?

Regards
Markus

答:
If you want to start from first principals there's a pretty thorough paper here

There's a question here which is related Calculate covariance matrix from x,y,z data

And there's some pretty practical advice here: https://answers.ros.org/question/12642/calculating-covariances-of-robots-odometry/


问:
Servo Starts Oscillating Under Disturbance
I'm using MG996R servos to control a 6-axis desktop robot arm. When no disturbance (like a gentle push to the robot), the servos are fine and can hold their places. But under a gentle disturbance, servos at the top will oscillate a little and come back to the set position, while servos at the bottom will keep oscillating.

The question here is what is causing the oscillation?

I understand inside a servo there is a PID for angular position control, and the oscillation, really is the response to the disturbance (and one can do s-plane analysis, etc.). The bottom servos have heavier loads, so my suspicion is there is not enough stall torque.

I didn't hear buzz, and without load, the servos would work fine (it could go to the set position, being able to hold its place when I use my finger to rotate the plate right on top of the motor shaft). So I don't think it's the PWM that causes the issue.

The PWM is set to 50%, so that should be near a neutral position.

PS:
One follow-up observation is if I change this motor to a different one, the oscillation stops, and the new motor performs quite robustly. Therefore, my current suspicion is manufacturing defects, which make the internal PID loop work less reliably with the real hardware

答:
I had a few ideas about what might cause the jitter action you showed in the video. I suggest you'll need to be more specific in the information provided to enable more specific answers.

Idea 1 - Movement Boundaries Conflict

Is the joint trying to move past a physical boundary? When constructing your robot arm for the first time, the servos need to be in a known position. Often the neutral 1500$\mu$s pulsewidth mid-rotation point. Each joint can then be assembled to a suitable mid-point joint rotation, matching the range of movement available on the servo to the range of movement possible in that joint.

Idea 2 - Insufficient power to the servos

Are you powering the servo from an external supply capable of supplying sufficient current, and not via a microcontroller (MCU) pin? The MG996R datasheet notes a stall current of 2.5A (@6V) per servo. A power supply capable of supplying power for multiple servos in the robot arm is recommended. They won't all be stalled (maximum drain) at the same time, but you should allow leeway. Base load equals number of servos times 900mA plus extra to allow for stalls. Your video shows three servos so 3*0.9+2.5 = 5.2A, so say a 6A 6V supply.

Idea 3 - Incorrect PWM signal (out-of-range)

You mention a PWM of 50% (which is a 10,000$\mu$s pulsewidth at 50Hz - see this explanation). A neutral 1500$\mu$s will be more like 7.5% duty cycle.

The datasheets I located were unclear or conflicting regarding the designed range for pulsewidth. I suggest working out from the neutral point until you establish the maximum travel of your particular servo. Minimum is anticipated in the range 500 - 1000$\mu$s; maximum in the range 2000 - 2500$\mu$s

Idea 4 - Servo Quality

These servos are on the economy end of the hobbyist market and get mixed reviews. You may just have a dud servo.

Idea 5 - PWM signal quality

You didn't mention in your question how you are generating the controlling PWM signal. If you're using an MCU then your code, the libraries you selected, and the other things going on in your program can impact the quality of the generated PWM signal. An oscilloscope is useful for checking the generated PWM.


问:
Peter Corke Robotics Matlab Toolbox Jacobian in Simulink
I am working on getting the Peter Corke Robotics Toolbox for Matlab connected to a Panda robot via Simulink. The problem I am encountering is the fact that the variables created by the Peter Corke Toolbox are not recognized by Simulink. I want to feed a function block with joint angles and end-effector velocities and then have the block output the joint velocities. This is done via the following multiplication:
$\dot{q}=J(q)^{\dagger} \dot{p}$. Executing this multiplication in Matlab is easy and straightfoward using the toolbox. You can get $J$ using robot.jacobe(q) and then take the pseudo-inverse and multiply this with the end-effector velocities $\dot{p}$.

The problem arises as soon as I try to execute this same multiplication in Simulink. The function block along with its inputs and outputs is shown below:

The function block contains the same script as the m-file that correctly outputs the joint velocities in Matlab. When I build the Simulink model, I get a multitude of error messages. Caused by the fact that Simulink does not recognize the SerialLink object (shown below) created by the Peter Corke Robotics Toolbox.

I've tried converting everything to a struct but then the toolbox's Jacobian function no longer works (unsurprisingly). Is there anybody that has experience with using SerialLink object in Simulink or is there a simple way to get the robot data into Simulink?

Thanks a lot in advance, for any help and or tips.

答:
I haven't used the Robotics Toolbox before, but it seems like probably Simulink just doesn't know how to package the panda into a signal it can move over one of the Simulink wires. Simulink can do things with signals in the wire like gains, derivatives, etc., but it has to know something about those signals.

The easy solution, I think, would be to just load the panda data inside your Matlab block, using whatever technique you're already using to load it in the Matlab scripts. Here's a Mathworks post describing loading variables into the Matlab workspace, which might be useful because,

Simulink blocks like the Constant block read variables from the base workspace.

Their post is trying to load data in Matlab then start Simulink, but you're kind of the opposite in that you're running Simulink and trying to load something. However, I think you could probably use the same technique to load the data if it doesn't exist. For example:

```<code>if(exist('variableYouNeed', 'var') == false)
    evalin('base', 'yourMatFile.mat');
end
</code>```

You could selectively load different robots by passing a constant that you use inside your Matlab code (enum/switch) to selectively load different files.

Body:
I haven't used the Robotics Toolbox before, but it seems like probably Simulink just doesn't know how to package the panda into a signal it can move over one of the Simulink wires. Simulink can do things with signals in the wire like gains, derivatives, etc., but it has to know something about those signals.

The easy solution, I think, would be to just load the panda data inside your Matlab block, using whatever technique you're already using to load it in the Matlab scripts. Here's a Mathworks post describing loading variables into the Matlab workspace, which might be useful because,

Simulink blocks like the Constant block read variables from the base workspace.

Their post is trying to load data in Matlab then start Simulink, but you're kind of the opposite in that you're running Simulink and trying to load something. However, I think you could probably use the same technique to load the data if it doesn't exist. For example:

if(exist('variableYouNeed', 'var') == false)
    evalin('base', 'yourMatFile.mat');
end

You could selectively load different robots by passing a constant that you use inside your Matlab code (enum/switch) to selectively load different files.


答:
I can add one more point to RowanP's excellent list: the servo's PID is unreliable, not because of a defect, but because it is mistuned for your application.

Recall that tuning a PID control is arcane, if not entirely a black art. The servo manufacturer tuned the control PID coefficients for some use case, perhaps moving RC aircraft control surfaces. That might need to exert considerable torque, but against a minimal moment of inertia.

Your setup, on the other hand, has a considerable moment of inertia. Imagine that the manufacturer's tuning expects a quick response when it gives a certain push, but your setup responds slower and has inertial overshoot. Without refreshing my undergrad s-plane analysis, I think that might explain what you observe. If you have the patience to conduct more experiments, you might try attaching loads with various moments of inertia and see how the behavior changes.

I agree with your surmise that a beefier servo might have a stiffer response. But it might also be slower, depending on how fast the motor actually grinds the gearbox around. Either way it might change the PID-vs-MoI enough to scare away your problem.

Some people have modified servos to supply an error or position signal to the controlling microcontroller. That would let software exert some more control, sort of servoing the servo. Further gutting could effectively disable the servo's internal control entirely, making it just a gearbox motor with a position encoder.

I have seen a sort of hybrid approach, where the $\mu C$ would gradually direct the servo to go from point A to point B. But won't help with your setup , where the oscillation happens when it's merely trying to maintain a position.

Title: Servo Starts Oscillating Under Disturbance

Body:
I'm using MG996R servos to control a 6-axis desktop robot arm. When no disturbance (like a gentle push to the robot), the servos are fine and can hold their places. But under a gentle disturbance, servos at the top will oscillate a little and come back to the set position, while servos at the bottom will keep oscillating.

The question here is what is causing the oscillation?

I understand inside a servo there is a PID for angular position control, and the oscillation, really is the response to the disturbance (and one can do s-plane analysis, etc.). The bottom servos have heavier loads, so my suspicion is there is not enough stall torque.

I didn't hear buzz, and without load, the servos would work fine (it could go to the set position, being able to hold its place when I use my finger to rotate the plate right on top of the motor shaft). So I don't think it's the PWM that causes the issue.

The PWM is set to 50%, so that should be near a neutral position.

PS:
One follow-up observation is if I change this motor to a different one, the oscillation stops, and the new motor performs quite robustly. Therefore, my current suspicion is manufacturing defects, which make the internal PID loop work less reliably with the real hardware


问:
4 axis orientation only robot solving inverse kinematics
I'm working on a 4 axis robot that only changes orientation (every axis is revolute and rotates around a common point). My goal is to be able to provide a look vector (x,y,z) and have the robots tool point in that direction (converting 4 axes into 3 axes). I'm relatively new to robotics and am learning about Jacobian IK but I want to make sure that I'm going down a good path or if anyone has suggestions for other methods of solving this robot.

Here's an illustration of the robot:

In the robots home positive, roll is redundant to train. The pan axis has limited rotation of +/- 15° so that the tool doesn't hit the tilt arms. Tilt also is constrained +/- 240° so that the tool doesn't hit the train arms. Train and Roll have slip rings so they can infinitely spin.

With this, if I wanted to have the robot look directly left (1,0,0) I would spin the train axis 90°, tilt 90° and roll -90°. That's the easy use case, but becomes more challenging if I wanted to have it look at vector (1,1,1). In this case there's a lot of valid combinations of rotations to make it look in that direction and solving it with trig doesn't seem feasible and needs to be an iterative solution.

I've been watching lots of videos on DH tables, homogeneous transforms and now Jacobians but I don't have a big math background so its a lot to take in. Also most examples are for arm like robots and not just orientation. I want to make sure that before I spend a lot more time looking into Jacobian methods that its a good path forward. There also seems to be a lot of options for Jacobian methods such as transpose, pseudoinverse, dampened least squares and I'm not sure which one to look more into.

Alternatively, I'm not tied to a Jacobian method if anyone has any other suggestions on algorithms that would work well for this. I've used FABRIK before but that doesn't seem appropriate here as it seems more suited for solving chains.

答:
Since the 3 axes intersect at a single point I'd recommend thinking about having a gimbal or spherical wrist instead of having a 4 dof robot. This way you don't need to bother with DH params.

With this view, your forward and inverse kinematics (for the first 3 dofs) will involve manipulating the resulting roll-pitch-yaw Euler angle rotation matrix.

I would do a search for "robot spherical wrist inverse kinematics" and go through some of the lecture notes and videos that come up and go through this step-by-step. I think this will simplify your kinematics.

You didn't mention anything about your 4th dof so I'm assuming once you're pointed in the right direction you already know how to find the angle for the 4th dof. Or, if you want to use this redundant dof, I'd refer you to scholar.google.com for papers on control of 4 dof gimbals.


答:
Based on your comment I rewrote the answer. I was under the impression that you want to control a 3D orientation; however, what you seem to aim for is a pointing/ray style target. This is actually a 2D target (the position on a 3D unit sphere) and means you have one more DoF in your null space. The solution, however, is quite similar in either case.

I also added a more detailed derivation to - hopefully - make it clear where things come from.

Hope this helps.

We can express the orientation of the tool frame in the world frame by tracing all the rotations used in the kinematic chain:

$$ ^\mathrm{world}\mathbf{R}_\mathrm{tool} = \mathbf{R}_z(\theta_\textrm{roll})\mathbf{R}_x(-90°)\mathbf{R}_z(90°)\\
\mathbf{R}_z(\theta_\textrm{pan})\mathbf{R}_x(90°)\mathbf{R}_z(-90°)
\mathbf{R}_z(\theta_\textrm{tilt})\mathbf{R}_x(90°)
\mathbf{R}_z(\theta_\textrm{train})$$

In the tool frame, we know the direction of the ray, which is a constant $\mathbf{v}=(0, 0, 1)=\mathbf{e}_\mathrm{z}$ and we can work out the direction of the ray in world coordinates by taking the inverse of the above chain of rotations. This gives us the general forward kinematics:

$$FK(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})= {^\mathrm{world}\mathbf{R}}_\mathrm{tool}^{-1}(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})~\mathbf{v}$$

Getting that inverse rotation is simple, because - as you probably know - $\mathbf{R}^{-1}=\mathbf{R}^T$, $\mathbf{R}^{-1}(\alpha)=\mathbf{R}(-\alpha)$, and $(\mathbf{A}\mathbf{B})^T=\mathbf{B}^T\mathbf{A}^T$.

This gives us a (lengthy) formula for the FK:

$$FK(\theta) =
\mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{tilt})\\
\mathbf{R}_z(90°)\mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{pan})
\mathbf{R}_z(-90°)\mathbf{R}_x(90°)\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{e}_\mathrm{z}$$

At this point, we could use CCD (cyclic coordinate descent) to solve the IK problem and find the joint angles by choosing the current configuration as the initial condition.

We can compute the Jacobian using the product rule $\mathbb{d}(\mathbf{A}\mathbf{x})/\mathbb{d}\theta_i = \delta\mathbf{A}/\delta\theta_i~\mathbf{x} +\mathbf{A}~\mathbb{d}\mathbf{x}/\mathbb{d}\theta_i$ and the derivative of a rotation matrix $\delta\mathbf{R_z(\alpha)}/\delta\alpha = \mathbf{S}((0,0,1))\mathbf{R}_z(\alpha)$, where $\mathbf{S}$ is the skew-symmeric matrix in 3D that associates with the cross product.

Since any $\theta_i$ only occurs once in the chain, the derivative is straightforward. We simply replace the respective $\mathbf{R}_z(-\theta_i)$ in the $FK$ equation with $\mathbf{S}(-\mathbf{e}_z)\mathbf{R}_z(-\theta_i)$ and obtain the respective column of the Jacobian.

Here is one example column:

$$\mathbf{J}_{pan}=\mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{tilt})\\
\mathbf{R}_z(90°)\mathbf{R}_x(-90°)\mathbf{S}(-\mathbf{e}_z)\mathbf{R}_z(-\theta_\textrm{pan})
\mathbf{R}_z(-90°)\mathbf{R}_x(90°)\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{v}$$

Notice that $\mathbf{J}_\mathrm{roll} = 0$ because $\mathbf{S}(-\mathbf{e}_z)\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{v} = \mathbf{S}(-\mathbf{e}_z)\mathbf{v} = 0$.

With this, we get access to all the variants of Jacobian-based IK, e.g., inverse jacobian.

Solving the IK problem analytically would mean that we wish to find all combinations of $(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})$ such that $\mathbf{v}_\mathrm{target} = FK(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})$.

We have 4 unknowns, so we will need 4 (non-redundant) equations. You might think that we can get 3 from the above $\mathbf{v}_\mathrm{target} = FK(\theta)$, but we can actually only get 2. Although we have formulated the problem in 3D, we are moving around on the unit sphere, which is a 2D subspace. This becomes particularly clear if we transform the entire problem into spherical coordinates and then look at the radius $r$ component:

$$r_\textrm{FK} = |FK(\theta)| = 1 = |\mathbf{v}_\mathrm{target}| = r_\textrm{target}$$

The above is independent of $\theta$ and hence a redundant equation. Hence we will have to "add" two equations/constraints while solving the IK.

The first equation shall be $\theta_\textrm{roll} = 0$, because $\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{v} = \mathbf{v}$. We can also see this from the Jacobian of $\theta_\textrm{roll}$, which is always be 0. Hence, a solution of the IK will be a solution regardless of the angle we choose here. This reduces the number of unknowns down to 3:

$$
\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{tilt})
\mathbf{R}_z(90°)\mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{pan})
(-\mathbf{e}_x)
$$

We can then do some reordering (variable terms to the front, constant terms towards the back) via $\mathbf{R}_x(-90°)\mathbf{R}_z(\alpha) = \mathbf{R}_y(-\alpha)\mathbf{R}_x(-90°)$:

$$
\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_y(\theta_\textrm{tilt})\mathbf{R}_x(-90°)
\mathbf{R}_z(90°)\mathbf{R}_y(\theta_\textrm{pan})\mathbf{R}_x(-90°)
(-\mathbf{e}_x)
$$

and $\mathbf{R}_z(90°)\mathbf{R}_y(\alpha) = \mathbf{R}_x(\alpha)\mathbf{R}_z(90°)$:

$$
\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_y(\theta_\textrm{tilt})\mathbf{R}_x(-90°)
\mathbf{R}_x(\theta_\textrm{pan})\mathbf{R}_z(90°)\mathbf{R}_x(-90°)
(-\mathbf{e}_x)
$$

and finally $\mathbf{R}_x(-90°)\mathbf{R}_x(\alpha) = \mathbf{R}_x(\alpha)\mathbf{R}_x(-90°)$:

$$
\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_y(\theta_\textrm{tilt})
\mathbf{R}_x(\theta_\textrm{pan})\mathbf{R}_x(-90°)\mathbf{R}_z(90°)\mathbf{R}_x(-90°)
(-\mathbf{e}_x)
$$

We can then combine all the constant terms:

$$
\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_y(\theta_\textrm{tilt})
\mathbf{R}_x(\theta_\textrm{pan})\mathbf{e}_z
$$

Unsurprisingly so, we can identify the above as the unit vector along the tool's z-axis being rotated by a xyz intrinsic Euler rotation (from the tool frame into the world frame).

At this point we can make a choice for the second equation we need to add and pick $\theta_\mathrm{pan}=0$. This would reduce the problem to

$$\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train})
\mathbf{R}_x(\theta_\textrm{tilt})\mathbf{e}_z$$

which is exactly the two angles measured by your standardspherical coordinate system (mind a minus):

$$\theta_\mathrm{train} = -\arccos(v_{\mathrm{target},3})$$
$$\theta_\mathrm{tilt} =\operatorname{atan2}(v_{\mathrm{target},2}/v_{\mathrm{target},1})$$

This gives us one of the various IK solutions:

$$
\theta_\mathrm{train} = -\arccos(v_{\mathrm{target},3})\\
\theta_\mathrm{tilt} =\operatorname{atan2}(v_{\mathrm{target},2}/v_{\mathrm{target},1})\\
\theta_\mathrm{pan}=0\\
\theta_\mathrm{roll}=0\\
$$

The above works if you just want to steer to a given position. If you want to do some kind of tracking or use cartesian control, however, this will fail once $`\theta_\mathrm{tilt}$ approaches $\pm 180°$. There, the robot/gimbal will do a flip/spin to the other side and lose track or deviate significantly from the desired trajectory.

You can avoid that by involving $\theta_\mathrm{pan}$. Another, easier, way would be to make your tool face along any of roll's axes that isn't the z-axis. That way you get two joints without constraints and will never run into this problem.

Title: 4 axis orientation only robot solving inverse kinematics

Body:
I'm working on a 4 axis robot that only changes orientation (every axis is revolute and rotates around a common point). My goal is to be able to provide a look vector (x,y,z) and have the robots tool point in that direction (converting 4 axes into 3 axes). I'm relatively new to robotics and am learning about Jacobian IK but I want to make sure that I'm going down a good path or if anyone has suggestions for other methods of solving this robot.

Here's an illustration of the robot:

In the robots home positive, roll is redundant to train. The pan axis has limited rotation of +/- 15° so that the tool doesn't hit the tilt arms. Tilt also is constrained +/- 240° so that the tool doesn't hit the train arms. Train and Roll have slip rings so they can infinitely spin.

With this, if I wanted to have the robot look directly left (1,0,0) I would spin the train axis 90°, tilt 90° and roll -90°. That's the easy use case, but becomes more challenging if I wanted to have it look at vector (1,1,1). In this case there's a lot of valid combinations of rotations to make it look in that direction and solving it with trig doesn't seem feasible and needs to be an iterative solution.

I've been watching lots of videos on DH tables, homogeneous transforms and now Jacobians but I don't have a big math background so its a lot to take in. Also most examples are for arm like robots and not just orientation. I want to make sure that before I spend a lot more time looking into Jacobian methods that its a good path forward. There also seems to be a lot of options for Jacobian methods such as transpose, pseudoinverse, dampened least squares and I'm not sure which one to look more into.

Alternatively, I'm not tied to a Jacobian method if anyone has any other suggestions on algorithms that would work well for this. I've used FABRIK before but that doesn't seem appropriate here as it seems more suited for solving chains.


问:
Why do current manipulators tend to use 7 DoF instead of 6 DoF?
In theory, controlling a 6D pose (3D position + 3D orientation) would require 6 joints, and - assuming the robot is carefully designed - the inverse kinematics should be solvable for that case.

Adding a 7th joint, and still targeting a 6D pose, would result in a null space of at least one dimension anywhere in the workspace. Why is this desirable?

My intuition is that it allows controlling the direction of the velocity and that the desired target changes; however, I didn't verify or investigate this so it is at most a best guess.

答:
Adding a 7th joint, and still targeting a 6D pose, would result in a
null space of at least one dimension anywhere in the workspace.

That is basically one the reasons people may wish to use a 7-DOF Robot for a 6-DOF End-Effector. It adds what is called Kinematic Redundancy. Your robot's end effector can reach a certain pose with different joint position sets.

Have a look at the following video for example:

https://www.youtube.com/watch?v=Wm2SdIhd3hU&ab_channel=EnergidTechnologies

As you can see, even though robot moves with changing some of the joint positions, end effector is not moving. You may wish to have multiple joint positions for a single end-effector pose if you have obstacles in the environment, or there is a possibility of your robot to have a self-collision.

Sometimes in industrial environments, these robot arms have limited free-space.  Even though a certain configuration can be achieved with multiple joint positions, one of them may cause robot to collide with its surroundings (another robot arms, a vehicle, and etc.).


问:
Sensor fusion in a metal heavy environment
I am trying to get heading information of a forklift. I've designed a small PCB for this purpose. This PCB has a 3 axis magnetometer and a 6 axis gyroscope-accelerometer (Z- axis looking towards the ground). I've used Madgwick's sensor fusion algorithm to combine 9 axis' informations. There are 3 choices for the sensor fusion implementation;

Choice 1: It uses all 9 axis information and returns roll, pitch, yaw values (Affected by magnetic interference)

Choice 2: It uses only gyroscope and accelerometer readings and returns relative roll, pitch, yaw values (Does not get affected by magnetic interference however value drifts over time)

Choice 3: It uses only magnetometer and accelerometer readings returns tilt compansated heading (Affected by magnetic interference)

Which one would be more suitable in metal heavy environment? Are there any other methods that you can suggest?

Also due to dynamic magnetic interference of the forklift's moving parts, I've mounted the device at the rear top side of the forklift. Should I do hard and soft iron calibrations when the device is mounted on the forklift or should I do calibrations before mounting? If I try to calibrate when the device is mounted, I won't be able to rotate device around the Z axis.

答:
Heading information from an accelerometer can be inherently noisy due to integration errors. Magnetometers and Gyroscopes can provide additional information to help reduce some of these errors, but if you are operating in the presence of external magnetic fields, you won't be able to reliably trust them either. Since you have designed / have access to the pcb, one possible solution would be to use some simple magnetic shields around your IMU.

Another solution would be to use some other sensors, like cameras or LiDAR. This might be more involved, but may provide you with a more accurate heading.


答:
Kinematic redundancy is certainly a good thing to avoid objects in the workplace etc., but we could simply see this as we want an extra degree of freedom for its own sake.

More fundamentally important is that 7DOF avoids gimbal lock situations. That's when some of the degrees of freedom in joint space happen to map to the same movement in object space. Because you still only have the same total number of dimensions in joint space, there's then suddenly one too few to cover all possible movements in object space. If you have one “spare” degree of freedom, this issue can be avoided – the extra DOF can jump in for the one of those that have become identical, and it gives you the choice-freedom to avoid the points where multiple joints might become locked.

Gimbal lock famously was an issue on the Apollo missions, here's a fascinating video about it:

https://www.youtube.com/watch?v=OmCzZ-D8Wdk

Title: Why do current manipulators tend to use 7 DoF instead of 6 DoF?

Body:
In theory, controlling a 6D pose (3D position + 3D orientation) would require 6 joints, and - assuming the robot is carefully designed - the inverse kinematics should be solvable for that case.

Adding a 7th joint, and still targeting a 6D pose, would result in a null space of at least one dimension anywhere in the workspace. Why is this desirable?

My intuition is that it allows controlling the direction of the velocity and that the desired target changes; however, I didn't verify or investigate this so it is at most a best guess.


问:
Workable low-resolution object/target recognition pattern and library?
I've spent quite some time researching this, but most of my Google search results have turned up academic research papers that are interesting but not very practical.

I'm working on a target/pattern recognition project** where a robot with a small camera attached to it will attempt to locate targets using a small wireless camera as it moves around a room. The targets will ideally be as small as possible (something like the size of a business card or smaller), but could be (less ideally) as large as 8x10 inches. The targets will be in the form of something easily printable.

The pattern recognition software needs to be able to recognize if a target (only one at a time) is in the field of vision, and needs to be able to accurately differentiate between at least 12 different target patterns, hopefully from maybe a 50x50 pixel portion of a 640x480 image.

Before playing with the camera, I had envisioned using somewhat small printed barcodes and the excellent zxing library to recognize the barcodes.

As it turns out, the camera's resolution is terrible - 640x480, and grainy and not well-focused. Here is an example still image. It's not very well-suited for capturing barcodes, especially while moving. I think it could work with 8x10 barcodes, but that's really larger than I'm looking for. (I'm using this particular camera because it is tiny, light, cheap, and includes a battery and wi-fi.)

I'm looking for two things: a suggestion or pointer to an optimal pattern that I could use for my targets, and a software library and/or algorithm that can help me identify these patterns from images. I have NO idea where to start with the right type of pattern so suggestions there would really help, especially if there is a project out there that does something resembling this. I've found OpenCV and OpenSIFT which both seem like potential candidates for software libraries, but neither seemed to have examples of doing the type of recognition I'm talking about. I'm thinking picking the right type of pattern is the big hurdle to overcome here, so any pointers to the optimal type of pattern would be great. Being able to recognize the pattern from all different angles is a must.

So far, my idea is to use patterns that perhaps look something like this, where the three concentric color rings are simply either red, green, or blue - allowing for up to 27 unique targets, or 81 if I use 4 rings. From about 2 feet, the capture of a 3x3 inch target (from my computer screen) looks like this which seems like it would be suitable for analysis but I feel like there should be a better type of pattern that would be more compact and easier to recognize - maybe just a plain black and white pattern of some sort with shapes on it?

Pointers to an optimal approach for this are greatly appreciated.

答:
Well, one of the things is that computer vision starts at light, lenses and camera, you need good ones. Its not only a question of resolution, it's also a question of sensor size, signal/noise ratio, and good lenses. There's no need for high resolution if your camera has poor optics.

So your environment is pretty challenging and there's no certainly that it will works in this way.

The 640x480 is not to low resolution. I have read a paper that proposed something like your circle, but without the spaces between the rings, so more light reflected and less wasted space, the circles are divided to represent a code of the place.

A simple circle divided into 4 segments with 4 colors would give you 256 distinct codes.

More or less this:

The lines are just for exemplification. Lets suppose you use 4 divisions with 4 colors: Red, Green, Blue and White. This would give you $ 4^4=256 $ distinct marks.
You could detect the circle with edge detect algorithms, and then you have the coordinates to get the color marks.

This needs that the orientation of the robot and the mark is always the same, if the robot or camera tilts, or you want to put the mark in any position, just add a first marker.

Adding a redundancy check to it is good to, as this will help removing false-positive marks.

ContentLicense: CC BY-SA 3.0


问:
C++ (ROS node) for Euler Integration to estimate Velocity knowing the acceleration
Hi I have IMU reading and would like to estimate the linear Velocity knowing the linear  acceleration from the IMU using the Euler method. Im aware of the accumulated error. So need a C++(python) code ( possible a ROS node) that takes the IMU data reading from the IMU sensor and gives the estimated Linear velocity using the Euler Integration.

I know that acceleration a is

a=dv/dt

and that will be the function.

dv/dt = f(t) . i.e. v(n+1) = v(n) + f(n) * dt

So how are the steps calculated? I have the time stamps from the ros topic.. So any help about the C++ (ROS node) code?

Thanks

答:
I find it easiest to make a class for stuff like this, where you have data you want to use between callbacks, or your want to publish a message inside of another callback. For example, your class might look something like (in the header:)

```<code>class IMUIntegrator
{
    public:
        void start();
    private:
        void imu_callback(const your_imu::Message::ConstPtr &msg);
        void send_velocity();
        ros::Publisher  m_velocity_pub;///< Publisher for the stopping estimate marker
        ros::Subscriber m_imu_sub; ///< Velocity subscriber
        float m_velocity = 0.0f;
        float m_prev_timestamp = 0.0f;
}
</code>```

then your cpp file does something like:

```<code>void IMUIntegrator::start()
{
    ROS_INFO("Starting...");
    // Setup the publisher first so it's ready to go before we trigger the first sub callback.
    ros::NodeHandle velocity_node;
    ros::NodeHandle imu_node;
    m_velocity_pub = velocity_node.advertise<your_velocity::Message>("/integrated_velocity", 1);
    m_imu_sub = imu_node.subscribe("/imu", 10, &IMUIntegrator::imu_callback, this);
    ros::spin();
}

void IMUIntegrator::imu_callback(const your_imu::Message::ConstPtr &msg)
{
    float timestamp = msg.header.timestamp;
    if(m_prev_timestamp==0.0f)
    {
        m_prev_timestamp = timestamp;
        return;
    }
    float dt = timestamp - m_prev_timestamp;
    m_prev_timestamp = timestamp;
    m_velocity += msg.acceleration[0]*dt;
    send_velocity();
}
void IMUIntegrator::send_velocity()
{
    your_velocity::Message message;
    message.header.frame_id = "your_frame";
    message.header.stamp = ros::Time();
    message.velocity = m_velocity;
    m_velocity_pub.publish(message);
}
</code>```

and finally you start it from your main cpp file:

```<code>int main(int argc, char **argv)
{
  ros::init(argc, argv, "velocity_integrator");
  IMUIntegrator integrator;
  integrator.start();
  return 0;
}
</code>```

I used placeholder names in there for your messages, etc., but that's generally the way it goes. This is all generally taken from the ros tutorials, which are actually pretty excellent, with the only exception that they're mashed together into a class and then used as a class. The class is nice because it lets you use member variables to pass information from the callback to the publisher. Member variables are kind of like global variables, but they're exclusive to the particular class, so you're not polluting your whole project with the information they contain (and other things aren't allowed to touch your class' data without permission!)

Body:
I find it easiest to make a class for stuff like this, where you have data you want to use between callbacks, or your want to publish a message inside of another callback. For example, your class might look something like (in the header:)

class IMUIntegrator
{
    public:
        void start();
    private:
        void imu_callback(const your_imu::Message::ConstPtr &msg);
        void send_velocity();
        ros::Publisher  m_velocity_pub;///< Publisher for the stopping estimate marker
        ros::Subscriber m_imu_sub; ///< Velocity subscriber
        float m_velocity = 0.0f;
        float m_prev_timestamp = 0.0f;
}

then your cpp file does something like:

void IMUIntegrator::start()
{
    ROS_INFO("Starting...");
    // Setup the publisher first so it's ready to go before we trigger the first sub callback.
    ros::NodeHandle velocity_node;
    ros::NodeHandle imu_node;
    m_velocity_pub = velocity_node.advertise<your_velocity::Message>("/integrated_velocity", 1);
    m_imu_sub = imu_node.subscribe("/imu", 10, &IMUIntegrator::imu_callback, this);
    ros::spin();
}

void IMUIntegrator::imu_callback(const your_imu::Message::ConstPtr &msg)
{
    float timestamp = msg.header.timestamp;
    if(m_prev_timestamp==0.0f)
    {
        m_prev_timestamp = timestamp;
        return;
    }
    float dt = timestamp - m_prev_timestamp;
    m_prev_timestamp = timestamp;
    m_velocity += msg.acceleration[0]*dt;
    send_velocity();
}
void IMUIntegrator::send_velocity()
{
    your_velocity::Message message;
    message.header.frame_id = "your_frame";
    message.header.stamp = ros::Time();
    message.velocity = m_velocity;
    m_velocity_pub.publish(message);
}

and finally you start it from your main cpp file:

int main(int argc, char **argv)
{
  ros::init(argc, argv, "velocity_integrator");
  IMUIntegrator integrator;
  integrator.start();
  return 0;
}

I used placeholder names in there for your messages, etc., but that's generally the way it goes. This is all generally taken from the ros tutorials, which are actually pretty excellent, with the only exception that they're mashed together into a class and then used as a class. The class is nice because it lets you use member variables to pass information from the callback to the publisher. Member variables are kind of like global variables, but they're exclusive to the particular class, so you're not polluting your whole project with the information they contain (and other things aren't allowed to touch your class' data without permission!)


问:
A* as a planning algorithm
Researchers refer to A* as the planning method but: how does A* work as a path planning algorithm, instead of a graph search method?
And how does it compare to RRT?</p>

答:
A* is a graph search method, so there is no real difference from the general family. It is different from the standard depth-first search and breadth-first search in that the search order differs.

How it works and how it differs from DFS and BFS becomes quite clear when looking at pseudo-code:

root_node = ...
cost = 0  # distance

target_node = ...

# a function that estimates the remaining
# distance to target_node
distance_heuristic = lambda x: ...

@dataclass(order=True)
class QueueItem:
    expected_total_cost:float
    cost_so_far:float=field(compare=False)
    node:Node=field(comare=False)
    path:List[Node]=field(compare=False)

# if we do pruning
visited:List[Node] = []

queue = PriorityQueue()
queue.put(QueueItem(0, cost, root_node, []))

while not queue.empty():
    item = queue.get()

    if item.node is target_node:
        break

    cost:float  # the cost of moving from node to child
    for cost, child in item.node.children:
        # sometimes we skip/prune nodes we have already seen
        if child in visited:
            continue

        child_cost_so_far = item.cost_so_far + cost
        cost_to_go = distance_heuristic(child)

        expected_total_cost = child_cost_so_far + cost_to_go

        queue.put(QueueItem(
            expected_total_cost,
            child_cost_so_far,
            child,
            path + [node])
        )

return item.cost_so_far, item.path
```

As you can see, this looks rather similar to a standard tree-search / graph search method. The main difference is that we introduce a distance_heuristic that gives us a hint on how far the target is. If the metric is good (e.g., if it is always exact) then we will only visit the nodes that are along a shortest path (there can be multiple) between the root and the target node (can you see why?). If the metric is inexact, i.e., it always overestimates, then we may visit more nodes than we have to, but - assuming the metric is half-decent - we will still visit fewer nodes than BFS does; especially when the graph/tree has high branching factor.

Another noteworthy aspect of the above is that your choice of cost metric + heuristic will influence the behavior of the search. In particular:

cost(parent,child)=1 + distance_heuristic(x) = 0 --> BFS

cost(parent,child)=0 + distance_heuristic(x) = 1/len(path) --> DFS

cost(parent,child)=distance + distance_heuristic(x) = 0 --> Djikstra

cost(parent,child)=distance + distance_heuristic(x) = something smart --> A*

RRT and A* are different algorithms in that RRT is a method of discretizing a continuous planning space into a tree-like structure, whereas A* is a method of searching within a graph (or tree-like structure). You could actually use A* to find a path after you have discretized your space using RRT; however, this is overkill as your structure is a tree (a special graph) and hence simpler methods to finding a shortest path exist.

Title: A* as a planning algorithm

Body:
A* is a graph search method, so there is no real difference from the general family. It is different from the standard depth-first search and breadth-first search in that the search order differs.

How it works and how it differs from DFS and BFS becomes quite clear when looking at pseudo-code:

root_node = ...
cost = 0  # distance

target_node = ...

# a function that estimates the remaining
# distance to target_node
distance_heuristic = lambda x: ...

@dataclass(order=True)
class QueueItem:
    expected_total_cost:float
    cost_so_far:float=field(compare=False)
    node:Node=field(comare=False)
    path:List[Node]=field(compare=False)

# if we do pruning
visited:List[Node] = []

queue = PriorityQueue()
queue.put(QueueItem(0, cost, root_node, []))

while not queue.empty():
    item = queue.get()

    if item.node is target_node:
        break

    cost:float  # the cost of moving from node to child
    for cost, child in item.node.children:
        # sometimes we skip/prune nodes we have already seen
        if child in visited:
            continue

        child_cost_so_far = item.cost_so_far + cost
        cost_to_go = distance_heuristic(child)

        expected_total_cost = child_cost_so_far + cost_to_go

        queue.put(QueueItem(
            expected_total_cost,
            child_cost_so_far,
            child,
            path + [node])
        )

return item.cost_so_far, item.path

As you can see, this looks rather similar to a standard tree-search / graph search method. The main difference is that we introduce a distance_heuristic that gives us a hint on how far the target is. If the metric is good (e.g., if it is always exact) then we will only visit the nodes that are along a shortest path (there can be multiple) between the root and the target node (can you see why?). If the metric is inexact, i.e., it always overestimates, then we may visit more nodes than we have to, but - assuming the metric is half-decent - we will still visit fewer nodes than BFS does; especially when the graph/tree has high branching factor.

Another noteworthy aspect of the above is that your choice of cost metric + heuristic will influence the behavior of the search. In particular:

cost(parent,child)=1 + distance_heuristic(x) = 0 --> BFS

cost(parent,child)=0 + distance_heuristic(x) = 1/len(path) --> DFS

cost(parent,child)=distance + distance_heuristic(x) = 0 --> Djikstra

cost(parent,child)=distance + distance_heuristic(x) = something smart --> A*

RRT and A* are different algorithms in that RRT is a method of discretizing a continuous planning space into a tree-like structure, whereas A* is a method of searching within a graph (or tree-like structure). You could actually use A* to find a path after you have discretized your space using RRT; however, this is overkill as your structure is a tree (a special graph) and hence simpler methods to finding a shortest path exist.


问:
What would be a way to estimate IMU noise covariance matrix?
Weirdly enough, my robot platform which has an official ROS package supported by a manufacturer doesn't provide any covariance matrices of its sensors. So, I'm basically trying to estimate these values for odometery and IMU using collected raw sensor readings to use the Kalman filter. For odometer, I feel like putting the robot into a static position (i.e., initial position) and collecting sensor readings will be enough to estimate its covariance.

What I'm a bit confused about is the way to calculate the covariance for IMU. So far, sensor readings collected in a static position show some random oscillation only in angular velocity and linear acceleration. Orientation never shows any noise in this situation. Would it be enough to follow the exactly same way I did for the odometer or should I do something else?

答: Usually, IMU manufacturers implement some kind of filter to remove the noise these days, therefore the probability is your IMU is not throwing raw values. Nevertheless, you can initiate the sensor covariance matrix by giving some small values on the diagonal let's say 0.1 or 0.05. Also, if you really would like to go one step further, you can do some specific rotation a few times (5 times) and collect the data. Then calculate the standard deviation of this data. Now initiate the covariance matrix with the variance (square of the standard deviation) of the data on the diagonal. It's always trial and error, and in my personal experience, you don't have to worry about the covariance matrix that much, initiate with some small values and the Kalman filter will converge anyway.</p>


问:
How to gyroscopically measure number of rotations on one axis when there is concurrent random motion on another axis
Can a gyroscopic sensor (comparable to the type that are typically used in smartphones) that is embedded in this black object

that is rotating around the X axis measure the number of rotations around the X axis if the object may or may not also be rotating at the same time in random ways (number of partial or full rotations, speeds, and directions) around the Z axis?

If so, is the Z axis rotation irrelevant, or is there special mathematics involved in filtering out the affects of the Z rotation on the measurement of X axis rotation?  Or does another measurement such as acceleration or magnetism need to be used to solve the problem?

Is there any impact in using a 2-axis vs. a 3-axis gyroscopic sensor for this measurement scenario?

答:
I believe the term you are looking for is 'Cross sensitivity'. This term describes the effect that motion in one axis has on the measured motion of other orthogonal axes in accelerometers or gyroscopes

Cross Sensitivity is normally stated by the manufacturer of the sensor and (depending on the quality/type of the sensor) is typically 1-5%. So, for example, a 10 rad/s rotation around the x axis might also give an output of 0.1 rad/s around the y axis and -0.2rad/s around the z axis which will lead to drift in the calculated position/angle of the device
Also, the cross-sensitivity may be different between different pairs of axes so there may be 3 values Sxy, Syz, Sxz.

In a perfect sensor the motion around one axis would not affect the others but in a real device there is always some effect.


答:
Your question doesn't say if this is in an industrial setting or more general humanoid robot.

In an industrial setting, I think you can get by with as few degrees of freedom as possible.  For the most part, the environment, task, and end-effector are all custom designed and singular purpose.  So if you can get by with only 3 or 4 DoF then go for it.  Extra DoF don't help at all and only increase manipulator cost, weight, and moving parts to fix.

However things get more interesting with more general purpose arms.  When I was designing a new humanoid robot I did a study on the best arrangement of joints.  It became clear that the usable workspace of the arm was not very large.  More joints significantly increases the arm's workspace.  This restriction was severe enough that despite having a 7 DoF arm and a holonomic mobile base, we still added a DoF to the torso of the robot.

Also note that just because an arm can reach a point in space, doesn't mean that it will be very usable there.  @leftaroundabout brought up a good point about gimbal lock, and @kucar had good points about joint limits and avoiding obstacles in the workspace.  But there is also manipulability.  For a given end-effector pose, depending on the arm configuration, the arm may have vastly different speed and strength capabilities.  More DoF give you more flexibility in choosing the best arm configuration for the job.


问:
Self balancing bike using a flywheel: ROS implementation
I have a bike with two stands and a flywheel like this:

The stands can be adjusted so that they don't touch the ground. Now, the task is to implement a self balancing mechanism: when I run the script, (with the stands not touching the ground), the bike should simply stay at one place without tilting appreciably.

Approach: I suppose the tilt of the bike (i.e. roll angle) can be used an "error" term, (the tilt is zero when the bike is upright) to control the torque on the flywheel. If the bike tilts towards the right (i.e clockwise), then the torque on the flywheel should be clockwise, which exerts a reaction torque on the bike in the anti-clockwise direction, which counters the original torque.

The bike comes with an IMU, so we can use it to extract the position quaternion, which can be converted to Euler angles using the tf library. Then, we simply need to use PID to control the angular velocity of the flywheel.

Another point worth mentioning is that I don't think something like $\omega= P+I+D$ is appropriate for this problem. What is important for balancing is the torque on the flywheel, which can be better expressed using $\Delta \omega$ rather than $\omega$, hence something like $\omega =\omega +P +I +D$ seems to be better for controlling the torque on the flywheel. With that in mind, I wrote this script:

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

from std_msgs.msg import Float32
from sensor_msgs.msg import LaserScan, Imu
from geometry_msgs.msg import Quaternion,Twist
import rospy
import tf

class sbb():
    def _init_(self):

        rospy.init_node('controller')

        self.sbb_orientation_quaternion = [0.0, 0.0, 0.0, 0.0]
        self.sbb_orientation_euler = [0.0, 0.0, 0.0]

        self.sample_rate = 50.0

        self.curr_angle = 0.0
        self.prev_angle = 0.0
        self.w=0.0
        self.I=0

        self.data_cmd = Float32()
        self.data_cmd.data = 0.0
        self.data_pub = rospy.Publisher('/flywheel/command', Float32, queue_size=10)

        rospy.Subscriber('/sbb/imu', Imu, self.imu_callback)

    def imu_callback(self, msg):

        self.sbb_orientation_quaternion[0] = msg.orientation.x
        self.sbb_orientation_quaternion[1] = msg.orientation.y
        self.sbb_orientation_quaternion[2] = msg.orientation.z
        self.sbb_orientation_quaternion[3] = msg.orientation.w

    def pid(self):

       (self.sbb_orientation_euler[1], self.sbb_orientation_euler[0], self.sbb_orientation_euler[2]) = tf.transformations.euler_from_quaternion([self.sbb_orientation_quaternion[0], self.sbb_orientation_quaternion[1], self.sbb_orientation_quaternion[2], self.sbb_orientation_quaternion[3]])
           #self.sbb_orientation_euler=[pitch,roll,yaw]
           self.curr_angle = self.sbb_orientation_euler[1]
           rospy.loginfo("angle: " + str(self.curr_angle))

           kp=3.5
           kd=0.02
           dt=0.02
           ki=0.001
           self.I=self.I + self.curr_angle*dt

           self.w=self.w - (kp*self.curr_angle)-(kd*(self.curr_angle-self.prev_angle)/dt)-(ki*self.I)

           self.data_cmd.data= self.w
           rospy.loginfo("flywheel w: "+str(self.w))
           self.data_pub.publish(self.data_cmd.data)

           self.prev_angle=self.curr_angle

if _name_ == '_main_':

    sbb = sbb()
    r = rospy.Rate(sbb.sample_rate)
    while not rospy.is_shutdown():

        try:
            sbb.pid()
            r.sleep()
</code>```

The - signs in the PID function have directional significance. A negative value publishes a clockwise angular velocity to the flywheel. So if the error (curr_angle) is positive (i.e. bike tilts CW), then the angular velocity of the flywheel is negative and increasing, i.e. a clockwise angular acceleration, i.e. a clockwise torque, i.e. an anti-CW reaction torque.

This script somewhat manages to balance the bike for the first couple of seconds (it oscillates a bit about the vertical), but then the angular velocity of the flywheel starts to increase/decrease rapidly and eventually the bike spirals out of control.

What changes can I make to the script/my approach to prevent this from happening?

The obvious answer is to tune the PID values properly, but is there a systematic way to do this? I have tried changing around the kp values (since that is largely responsible for  the oscillation), but at one value, the oscillations are too violent, and when I decrease it slightly, the reaction torque becomes insufficient to counter the original torque..

edit: Video demonstration: https://www.youtube.com/watch?v=lBRUsTbNaj0

答:
I came back to this question and thought a bit more about it because of your bounty - typically the bounties are offered from a point of desperation, and I hate that feeling myself.

I think probably your code is fine, in looking at it. There are things that I'd do differently, like scaling your PID output by the time step, but you could distribute that timestep across your PID gains and a method like Ziegler-Nichols would still work because that method sets the gains relative to each other.

I think the real issue you're having is that you're controlling position. If you consider the bike to be a kind of inverted pendulum, then you'll wind up with basically

$$
\alpha = -\frac{g}{L}\theta \\
$$

which is the pendulum equation. The thing you're missing here are the dynamics of the system - saying your angular acceleration is zero (which would be true if $\theta = 0$) is not enough - you also need the angular velocity to be zero!

I think what's happening is that your controller is "snapping" to the zero-angle position, and you get to that zero-position with some angular velocity. The angular velocity means you have an angular momentum that is carrying you beyond the zero position, and you wind up with these oscillations.

What I would suggest is trying two PID controllers. Have the first PID controller setup to drive angular velocity to zero, with the same mechanic that you have now - PID output drives speed by $\omega = \omega + P + I + D$. In tuning it you'll probably notice that you have a scenario where your bike starts to lay down and the flywheel slows that rate by continually accelerating, up to some infinite speed.

Once you have your "angular rate" PID loop tuned to where it's slowing you down well, add a second PID controller to drive your angular position to zero. You would want the gains on this controller to be "much lower" than the gains on the angular rate controller. When you do this, you ensure that the angular rate controller is the one dominating the response, which is what you want - you actually want the bike to be stationary; the exact angle isn't as important.

For "much lower," I would ballpark the gains at 1/10th the values for the angular rate controller as a starting point and then try adjusting from there.

Conceptually, the angular rate controller should act quickly to stabilize the bike while the angular position controller acts slowly to bring it back to the zero position. If you set the gains too high on the angular position controller then you'll get a scenario where the angular position controller perturbs the angular rate and you get the two controllers fighting each other.

As with your current implementation, you'd just have

$$
\omega_k = \omega_{k-1} + (PID)_{rate} + (PID)_{position} \\
$$


问:
How to calculate DH parameters for Higher Pair?
I am trying to build a robot which is having Bevel Gears connecting two links. I want to calculate DH parameters, but i am stuck with this bevel gear as when the joint rotates both angle and displacement are being changed.</p>

答:
When you think about the mechanism the gears are offset, but the joint itself has an axis. You should use that axis which may not be overlapping/coincident  with the link attached to the bevel gear.

I found a few tutorials that you may find useful:

https://blog.robotiq.com/how-to-calculate-a-robots-forward-kinematics-in-5-easy-steps

https://automaticaddison.com/how-to-find-denavit-hartenberg-parameter-tables/

https://users.cs.duke.edu/~brd/Teaching/Bio/asmb/current/Papers/chap3-forward-kinematics.pdf


问:
Control both Velocity and Position (Linear actuator)
I am trying to control the velocity + position of a linear actuator.

At this moment I am able to control the position or the velocity. But I'm trying to control both. What the control has to do: Let the linear actuator drive to a position i.e. 0 to 100 cm with a constant velocity of 1cm/s.

I control the actuator using a PWM signal. And I measure the velocity and position using a position sensor on the shaft.

What kind of control is preferred, PID in cascade?
If so, what would the code look like?
Any other kind of control would function better?

EDIT:
A more describing picture.

I want a velocity controlled position controller.
Hopefully this will make it clear

EDIT

My first try is with a trapezoid wave. Maybe there is an easy way without too much calculation power to change it to an s-curve. Then the acceleration/jerk will be a lot smoother.

I let the microcontroller calculate 3 different formulas, afterwards it will calculate it using loop iteration. This way I can use one PID for the position. The parameters in the following code will fictional:

```<code>AccelerationLoops: 5                     //[Loops]
Velocity:          100                   //[mm/s]
DeltaPosition:     7.5                   //[mm]
Looptime:          5                     //[ms]
Loopfactor:        1000 / Looptime       //[-]
VelocityLoop:      Velocity  /Loopfactor //[mm/loop]
VelocityFactor:    VelocityLoop * .5 / AccelerationLoops
                                         //[mm/loop]  (.5 found by integration)
Loops:             DeltaPosition / VelocityLoop / AccelerationLoops
                                         //[Loops]
</code>```

Formula

```<code>Formula1:      VelocityFactor * x^2
LastF1:        Last value of Formula1 Formula1(5)

Formula2:      VelocityLoop * x - LastF1

Formula3:      VelocityFactor * (Loops - x)^2 + DeltaPosition)
</code>```

Using the parameters of above it will generate the following setpoint:

```<code>0   0,00
1   0,05
2   0,20
3   0,45
4   0,80
5   1,25
6   1,75
7   2,25
8   2,75
9   3,25
10  3,75
11  4,25
12  4,75
13  5,25
14  5,75
15  6,25
16  6,70
17  7,05
18  7,30
19  7,45
20  7,50
</code>```

A big problem with the code above is that the amount of acceleration loops is a constant. It cannot be changed except when you already know the amount of loops it will take.

I will be using two separate Arduinos, they will be connected using a CAN-bus connection. Anyway, they won't communicate through it unless the load becomes too high. This will make master/slave impossible. Also the system has to be modular: adding another actuator to the circuit won't be a problem. The actuator is speed controlled by using a PWM signal. The linear sensor will deliver a 0-10v signal which i will reduce to 0-5v by a simple voltage divider. The loop will be around 5 to 10 ms, will depend on the maximum looptime.

Arduino has a 10-bit (0-1023) ADC but use of oversampling I will probably try to increase it to 12-bit. To not decrease the reading speed I will decrease the prescaler of the ADC.

The PWM output is 8-bit (0-255), I am trying to find a way to further increase. Because I think 255 steps are too low for my application.

Because the Arduino has limit internal memory, precalculating all the positions is impossible.

Thank you all for the help so far!

答:
Code for S_Curve generator in MATLAB

Base on Ugo Pattacini answer:

$$
x(t)=x0+(xd−x0)⋅(10(t\div T)^3−15(t\div T)^4+6(t\div T)^5)
$$
code:

```<code>x0=0;           %% First Position  (Start)
xd=100;         %% Second Position  (End)
T=100;          %% Time

for t=0:1:T
x(t+1)=x0+(xd-x0)*(10*((t/T)^3)-15*((t/T)^4)+6*((t/T)^5));
end

plot(x);
title('S-Curve for Motion');
xlabel('Time');
ylabel('Position');
</code>```

Body:
Code for S_Curve generator in MATLAB

Base on Ugo Pattacini answer:

$$
x(t)=x0+(xd−x0)⋅(10(t\div T)^3−15(t\div T)^4+6(t\div T)^5)
$$
code:

x0=0;           %% First Position  (Start)
xd=100;         %% Second Position  (End)
T=100;          %% Time

for t=0:1:T
x(t+1)=x0+(xd-x0)*(10*((t/T)^3)-15*((t/T)^4)+6*((t/T)^5));
end

plot(x);
title('S-Curve for Motion');
xlabel('Time');
ylabel('Position');


问:
What is meant by a speed profile?
When researching robots, micro mouses etc I've often come across people taking about generating "speed profiles" and how to calculate them. Also profiles for acceleration , deceleration , turning etc. Also trapezoidal profile?

But I can't seem to find exactly what is meant by this. The how or why also.

So what is a "profile" in this sense and why you would need one?

Title: What is meant by a speed profile?

Body:
You can use this code in MATLAB:

x = 0:0.1:20;
y = dsigmf(x,[1 4 1 16]);
plot(x,y)

Or

clc
clear all;

speed=600;
step=300;

for i=0:1:step
  y1(i+1)=1/(1+exp(-(0.05)*(i-100)));
  y2(i+1)=1+(-1/(1+exp(-(0.05)*(i-200))));
end

y=[y1,y2];
plot(y)
xlabel('time(Step)')
ylabel('Speed')
xlim([-0.05 600])


问:
How to use Quaternion from IMU to get the transformation to inertial reference frame?
Im trying to get the Linear Velocity integrating the acceleration from IMu. I know there will be accumulated error due to integration bit before the integration I have to do couple of other steps.
First step would be transformation to the inertial reference frame. So basically using Quaternion transformation .
Second, using that transformation to get the acceleration on the IMU regarding that inertial reference frame.
And third final will be to integrate that transformed acceleration.

So here the steps before the integration part
Quaternion-> T(r); a(inertial-reference-frame) = T(r)*a(imu);

So how would be ROS node (C++) that handle these two steps before the integration?

I have part of the code that think can do that but Im not sure

```<code>geometry_msgs::QuaternionStamped imu_quat = geometry_msgs::QuaternionStamped();
    tf::StampedTransform transform;
    tf::TransformListener listener;
    listener.waitForTransform("/base_link",
                                    msg->header.frame_id,
                                    msg->header.stamp,
                                    ros::Duration(3.0));

    listener.lookupTransform("/base_link",
                                  msg->header.frame_id,
                                  msg->header.stamp,
                                  transform);

    imu_quat.header = msg->header;
    imu_quat.quaternion = msg->orientation;
    geometry_msgs::QuaternionStamped q_robot;

    listener.transformQuaternion("base_link",msg->header.stamp,imu_quat, imu_quat.header.frame_id, q_robot);

    tf::Quaternion quatquat;
    tf::quaternionMsgToTF(q_robot.quaternion,quatquat);
    tf::Matrix3x3(quatquat).getEulerYPR(new_yaw,new_pitch,new_roll);
   acceleration_x = (msg->linear_acceleration.x);
   acceleration_x = transform.getOrigin().x();

    if (count2 == 0){
        time_prev = ros::Time::now().toSec();
        count2=1;
    }

    float time_now = ros::Time::now().toSec();
    float time_dif = time_now - time_prev;
    time_prev = time_now;
    acceleration_x = (msg->linear_acceleration.x );
    m_velocity += acceleration_x*time_dif;
    send_velocity();
</code>```

Is this the correct way?Thanks

答: You should use ROS' inbuilt transforms. Assuming that your IMU data is being read as a ROS message, and you know all the frame id's, this example template explains the basics of using tf's.</p>


问:
can anyone explain detailly this video(title: sensorless active complaince)
https://www.youtube.com/watch?v=g8Je93soOZE

i am interseted in how they achieve compilance and detect collision in servo without the use of sensors. so any one please explain briefly

答:
It's explained on the video description:

In this research, a methodology for sensor-less full body active
compliance was used on a 6-DOF RSS (Rotary-Spherical-Spherical)
parallel manipulator. The manipulator can detect and comply with the
external forces on any part of its body without using any explicit
force/torque sensor at the joint or the end-effector. This was done by
utilizing the estimated joint torque based actuator current feedback
only. A three-layer cascaded impedance controller for active
compliance and reaction to various human interactions was used. The
proposed design and unique methodology for compliance exhibits an
effective and inexpensive yet reliable alternative to be used in safe
human-robot interactions and force controlled manufacturing
applications.

Collision / Compliant motion is detected by the current draw increase of the motor.

This was done by utilizing the estimated joint torque based actuator
current feedback only


问:
rospack can't find installed package
I installed ros-melodic-navigation with:

```<code>$ sudo apt-get install ros-melodic-navigation
[sudo] password for user:
Reading package lists... Done
Building dependency tree
Reading state information... Done
ros-melodic-navigation is already the newest version (1.16.6-1bionic.20200530.114248).
The following packages were automatically installed and are no longer required:
  linux-headers-5.3.0-51 linux-headers-5.3.0-51-generic
  linux-image-5.3.0-51-generic linux-modules-5.3.0-51-generic
  linux-modules-extra-5.3.0-51-generic
Use 'sudo apt autoremove' to remove them.
0 upgraded, 0 newly installed, 0 to remove and 300 not upgraded.

</code>```

I can find the navigation package  with:

```<code>$ apt list | grep navigation
...
ros-melodic-navigation/bionic,now 1.16.6-1bionic.20200530.114248 amd64 [installed]
...
</code>```

But when I try it with rospack, I get an error

```<code>rospack find navigation
[rospack] Error: package 'navigation' not found
</code>```

What is the issue here?

答:
Its probably because its a metapackage.  they might leave that out of the rospack find.
but roscd works even though i get the same error you are getting on a rospack find</p>

Title: rospack can't find installed package


问:
What does RR stand for in RR-manipulator?
I have memorized RR-manipulator as having 2 arms from Google images but "arm" isn't spelled with "R". I also noticed that RRR-manipulator has 3 links. Does R stand for revolute?</p>

答:
The RR manipulator is commonly used as shorthand for a two revolute joint configuration in a single plane. This provides a reduced configuration space which is helpful for research and education use.

RRR is as you guessed a 3 joint system but usually still remaining in the plane.

I don't believe that there's an official definition for this but it's more of a common shorthand. Other abbreviations you may see are P for prismatic. And sometimes analyses leave the single plane, but use the same or similar shorthand for configuration space. The simplest versions have the plane oriented horizontally, but as you get into dynamic systems turning it vertical will increase the complexity adding weight/gravity.


问:
Multiplication of rotation matrix help
I'm Watching Angela Sodemann video on 6DOF robots and am following her example, but I'm getting a different answer for my R3_6 matrix and I'm not sure where I'm going wrong. She uses this as her example robot:

She doesn't give her DH Table so I came up with this for the last three joints:

Using the homogenous transformation matrix, I came up with the following rotation matrices for the last three joints:

I believe both of those are correct. I think my issue is just in multiplying the matrices. My understanding is to multiply two matrices you multiply every column in each row by every row in each column and sum them:

```<code>R_35[0][0] = (R_34[0][0] * R_45[0][0]) + (R_34[0][1] * R_45[1][0]) + (R_34[0][2] * R_45[2][0])
R_35[0][1] = (R_34[0][0] * R_45[0][1]) + (R_34[0][1] * R_45[1][1]) + (R_34[0][2] * R_45[2][1])
R_35[0][2] = (R_34[0][0] * R_45[0][2]) + (R_34[0][1] * R_45[1][2]) + (R_34[0][2] * R_45[2][2])
... and so on
</code>```

From this I get the following matrices:

My end result is only correct in a few places. Can anyone spot where I'm going wrong?

Note: Several People In the comments have said R1 should be S5S6. Not sure who is correct on that.

Here is the link to the youtube video:

https://www.youtube.com/watch?v=ZM9GOENJcuo&t=634s

答:
after spending 3-4 hours i successfully found the mistake

first i multiplied the R3_6 by hand and checked the matrix. it came as same as yours, varying from the angela sodemann's R3_6.

after several attempts i decided to define rotation matrices by myself ,and later found that R3_4 is provided here is wrong!

then i calculated all rotation matrices R3_4, R4_5 AND R5_6 and then multiplied to get R3_6 and got the same matrix as angela sodemann   { R3_6 (1) (2) is s5s6 as others mentioned  }

I also checked my R3_6 matrix. written in python and compared it to the rotation matrix part of the homegeneous transformation eqn and both are same

the rotation matric R3_4(That i mentioned wrong) is not for the kinematic diagram that you mentioned in the question, if you compare your image in question and below attached image you will find the difference on 3rd frame, where she used this diagram to teach n another video.

link for that video where she calculates R3_4

link

you can see on 13:25

so the R3_6 you had calculated is applicable for this kinematic diagram of the robot only.

Hope this answer helps you :)

Title: Multiplication of rotation matrix help


问:
Dynamically-stable Motion Planning for Humanoid Robots
I was reading the classical robotics paper Dynamically-stable Motion Planning for Humanoid Robots and I don't understand why the two trees are swapped in line 7 of figure 5 (page 11). Seems like an otherwise very straightforward algorithm, just this part is tripping me up.

Any help will be much appreciated, thanks!

答:
I didn't read the paper fully,  so I don't know if this was stated already, but the mechanism of swapping points would grow the path from both ends, which I think would lead to more graceful obstacle avoidance than just growing the path from A to B. Make a segment from A in the direction of B, then make a segment from B in the direction of A. Keep alternating this way and hope to meet in the middle.(again just my best guess)

Try contacting the authors! I did it once and was thrilled to get any response, and it was even helpful!


问:
What is the noise in this PWM signal?
I'm in the process of calibrating servos while constructing a new hobbyist robot arm. When testing the base rotation servo I noticed some unexpected noise in the PWM signal while the servo is in motion. There is no noise in the PWM signal when the servo is stationary. What causes the PWM noise? Is it likely to be significant? If so how would I filter it? The motion behaviour of the servo is exactly as expected, so the likely answer I'm expecting is to just ignore it, but I wanted to learn more about what I am seeing.

The equipment in use is:

Arduino Uno as test MCU, powered by USB connection.

PCA9685 I2C PWM controller with a 470$\mu$F 16V electrolytic filter capacitor

an inline breakout in the servo connection for multimeter and oscilloscope connection

external bench power supply 6V limited to 2A

only one servo is connected to PCA9685 port 0

the servo is a GoBilda 2000-0025-0002

Update: Link to video of oscilloscope trace (as per the question description - no bypass capacitor between PWM signal and ground).

答:
After a bit more reading and thinking, I think I’ll answer my own question. I’d like feedback on my logic if you have it.

My logic is this:

the PWM signal from the MCU is clean (evidenced by the trace when the servo is unpowered)

the noise is back EMF from the servo

no other device other than the servo needs to receive or read the signal

if the signal was to be filtered at some point the noise, having been generated at the servo, will always be present on the servo side of the filter

therefore the noise can be ignored.

Title: What is the noise in this PWM signal?


问:
Does a stable library exist for docking robots using images?
I built a mobile base from scratch, and now I am focused on getting the robot to charge.

The robot is using an on-board webcam (and PC) to detect a QR barcode on the charger unit. Using OpenCV I find various barcode attributes such as center point of the barcode, edge lengths, degrees of the edges, area, and so on. The specifics of the way the barcode is captured by the webcam has enough data to locate the robot relative to the charger, but, is it ever a huge pain in the rear. It is leading me to ask questions like this: https://math.stackexchange.com/questions/4281368/how-to-combine-equations-that-are-describing-related-physical-phenomena

So here is my question: Does anyone know of a library (preferably in c++) that uses a webcam and a QR code that can provide accurate information of where the camera is relative to the QR barcode? I am looking for the following data form the lib:

distance of camera from QR code (how far are the two apart?)

rotation of camera relative to QR code (if camera drives straight will it hit the QR code in the center?)

camera center to QR center error reporting (by how much is the center of the barcode and the center of the camera off; assuming the camera is square to QR code)

If I am re-engineering the wheel here, I am ready to throw in the towel and use a precanned solution... Please let there be a docking library out there for my hardware...

Thanks.

答:
There's a lot of solutions out there already for this. I'll start by suggesting AR tags not QR tags. They're better optimized for localizing.

The Alvar library is commonly used for tracking AR tags.

http://virtual.vtt.fi/virtual/proj2/multimedia/alvar/

And there's a ROS package which wraps that and publishes the results as ROS messages.

https://index.ros.org/p/ar_track_alvar/github-ros-perception-ar_track_alvar/

API defined here: http://wiki.ros.org/ar_track_alvar

There's also ArUco: http://www.uco.es/investiga/grupos/ava/node/26
With a ROS wrapper aruco_detect

If you're looking for a higher level of intergration there's an autodock package which is available here: https://github.com/osrf/autodock with some discussion of it here


问:
What is the difference between a CNC router versus a CNC mill?
People at the RepRap 3d-printing project often mention CNC routers or CNC mills.

Both kinds of machines almost always have a motorized spindle with stepper motors to move the spindle in the X, Y, and Z directions.

What is the difference between a CNC router versus a CNC mill?

(Is there a better place for this sort of question -- perhaps the Woodworking Stack Exchange?)

答:
CNC machining is popular in the manufacturing sector with the benefits it offers. You can see different types of CNC machines get developed, each having its advantages. The two most popular ones are the CNC router and CNC mill. They seem similar but are different. Here are the differences:

CNC router

CNC routers are designed to operate at high speeds compared to CNC mills, using the rotational speeds as the driving force to make the cuts. While routers have three axes, there are variations with 4 or 6 axes ideal for complex projects.

CNC router highlights:

CNC router operates at high speeds.

Features a spindle that moves on X, Y and Z axes.

Make 3D cutting easier.

CNC mill

CNC mills can perform nearly all the functions of the CNC router, but there are some differences. CNC routers are for softer materials, and mills get used to cut hard materials. Instead of a stationary table, the CNC mills move in X and Y configuration, with the spindle moving on the linear axis above the workpiece.

CNC mill highlights

Used to cut metals and complex materials.

Make cuts using high torque.

Able to perform thicker cuts.


问:
Derive the system's dynamics function and the Jacobian Matrix G of Extended Kalman Filter of a differential drive robot on a 2D plane
I have found the following answer on the system's dynamics function and what the Jacobian matrix G of the Extended Kalman Filter look like for a 2D robot with a differential drive. Taken from slides and source.

Can someone please show me the full derivation? Or maybe you can show me a link that I can use for generating Jacobians for a given function? Any guide would be appreciated.

Odometry model:

The answers:

答: Here is the closest I could get to my answer. I hope it helps someone in the future. Please do notice that the result matrix is mirrored.


问:
The Jacobian resulted from Screw method is different from analytical one (Example Inside)
I am currently solving a kinematics example that asks for the Jacobian. However, when I solve it using the Screw method I get different results from the analytical method, The example is kinda hard to write here but I will demonstrate on a 2D RR robot which shows the same behavior.

Here is the simple problem:

Now lets find the Jacobian when $θ_1=pi/4 $ while  $θ_2=0$, $L1=L2=1$

Here is the Jacobian using Screw Method:

$J =  \begin{bmatrix}
0 & 0\\
0 & 0 \\
1&1 \\
0& 0.7\\
0& -0.7\\
0&0
\end{bmatrix}
$

Here is the Jacobian using Analytical Method:

$J =  \begin{bmatrix}
-L_1 sin(θ_1) - L_2 sin(θ_1+θ_2)  & -L_2 sin(θ_1+θ_2)\\
L_1 cos(θ_1) + L_2 cos(θ_1+θ_2) & L_2 cos(θ_1+θ_2) \\
\end{bmatrix},
$

$J =  \begin{bmatrix}
-\sqrt{{2}} & -\sqrt{{2}} / 2 \\
\sqrt{{2}} & \sqrt{{2}} / 2 \\
\end{bmatrix}
$

Notice that even singularity of both methods do not match. Where did I make the mistake?

Also, why the screw method does not take into account the length of last link? I mean if the length of last link is 0 or 1000, the linear velocity W x R should change.

Edit1: Here is the method I used to calculate the first jacobian.

答:
You're computing the spatial Jacobian, which relates joint velocities to spatial velocities at the origin. You instead want to compute the body Jacobian, which relates joint velocities to end-effector velocities expressed in the end-effector frame. So in your 2D RR example what you want to do is to compute the body Jacobian then pre-multiply the matrix by $R_z(-(\theta_1 + \theta_2))$ so that the end-effector linear velocities are aligned to the fixed frame. You essentially premultiply the body Jacobian by the current orientation of the end-effector to align it with the world frame.

The next point is to clarify the difference between the analytical and geometric Jacobians. Both the body Jacobian and spatial Jacobian are the analytic Jacobian since they both relate joint velocities to operational linear and angular velocities at some frame (fixed or body frame). If instead the Jacobian for the rotational components relate joint-velocities to some description of orientation such as ZYX Euler angles or unit quaternions, then it would be called the geometric Jacobian. So the linear components are the same for both analytic and geometric Jacobian. Finally To convert between analytical and geometric Jacobian, there is a mapping between angular velocities and the derivative of the orientation representation. For ZYX Euler angle representations for example, there exists a mapping between angular velocities and Euler rates. For unit quaternions, there also exists a mapping between the quaternion derivative and angular velocities.

I hope this helps!


问:
Floor Segmentation to Determine Navigable Paths
In my application, my robot has the following physical setup:

Differential drive mechanics with feedback (wheel encoders)

Commercially available webcam mounted with a known transform to the base of the robot (RGB, no depth)

The robot will be navigating through a structured, indoor type environment (think office, home, or university), and I would like to be able to determine the navigable paths through the environment using my vision sensor.

What is the best way to approach the problem of finding safe paths to travel when given a single vision sensor?

Edit:  I think that I am more interested in the vision processing techniques than the actual path-planning mechanics.

答: You could have a look at visual servoing techniques with occlusion. Namely: try to reach "waypoints" on the floor or walls (e.g. doors, ...) using visual servoing.</p>


问:
2D path following robot, converting XY axis path to input on wheels
at the moment I am creating an android program, that will steer my simple, 3 wheel (2 motors, 1 for balance) robot to move online following the path drawn by user on his screen. The robot is operated through WiFi and has 2 motors that will react on any input signals.

Imagine user drawing a path for this robot on smartphone screen. It has aquired all the points on XY axis, every time beginning with (0,0). Still I have no idea, how to somehow "convert" just points, into voltage input to both motors. Signals will be sent in approx. 60Hz connection, so quite fast. Maybe not every single axis point will be taken into consideration, there will be surely some skips, but that is irrelevant, since this path does not have to be done perfectly by the robot, just in reasonable error scale.

Do you have any idea on how to make the robot follow defined axis points that overall create a path?

Edit 10.01:

The voltage will be computed by the robot, so input on both is between -255 and 255 and the velocity should increase or decrease lineary in those borders.
Additionaly, I would like to solve it as if there were perfect conditions, I don't need any feedback crazy models. Let's assume that all the data is true, no sensors and additional devices. Just XY axis path and required input (ommit wheel slide too).

答:
for the hardware

You don't say what type of motor you use. If it's a DC-brushed type, putting more or less voltage will not necessary linearly alter the speed, in fact if you increase the voltage the relative amount the load on the motor increase the speed won't change. To do a good control you need a feed-back of the motor or wheel speed, or an angle sensor that you can derivate the speed.
If you use this information, and correct it according to the speed you want.

for the software

What you presumable want to navigation is Dead reckoning, but if one wheel slips, the position information will drift and accumulates to the point it turns unusable. So, don't expect the best results with only this.

One more simple solution is to use an electronic compass so you know to what direction relative to north (deviations should not be a problem), your robot is pointing. This should help at the turns and more straight path, even if some wheel slip occurs, it will close the loop by that. Wheel slip will still affect the distance traveled.


问:
What is the most cost effective yet still reliable outdoor boundary system?
I am working on a robot that will operate outside in people's lawns in a predefined space. The issue that I am having right now is figuring out what kind of boundary system I should implement in order to ensure that the robot stays within someone's lawn, assuming that each lawn the robot is being used in does not necessarily have a fence or the same layout but that the operating area is a rectangle or other 4 sided polygon.

I have come up with a few ideas so far. I am trying to keep the cost of the boundary system within 150 - 200 dollars USD. In addition, the idea is that setting up this boundary system should be as easy as possible for the end user. I hope that this system is easy to implement, but I am open to learning new skills to make it happen.

If one of these ideas is not feasible, please let me know. I am working on this project for school (high school, 12th standard). I have some experience with robotics but am a little bit out of my league for some of these boundary solutions.

Guide Wires Buried Underground (expensive, but reliable (from what I've heard), also a lot of work for the boundary system).

4 Poles at each corner of the rectangular operating area. The robot will either have a camera, or some kind of ultrasonic sensor or LiDAR sensor to detect distance from the robot to each pole. Then, it can triangulate its position in the field and move accordingly. (I am guessing this will require some kind of image recognition to first detect the poles, and then figure out the distances to the pole).

Have the user control the robot manually once from one specific starting location around the entire boundary of the field. Measure the distances and whenever the user wants to use the robot, they can put it at the predefined starting location and it will cover the boundary path as well as any parts of the lawn inside. (This is the dumbest solution, and I am worried that the robot will slowly veer off course as it traverses the lawn up and down multiple times).

Some kind of AR app that a user can use, in which the app generates an environment surrounding the lawn and the user places 4 virtual markers at each corner of the lawn. This environment is uploaded to the software running on the robot, and it uses a camera (maybe spinning?) that allows it to figure out where in the environment it is. Then, it moves accordingly. (Honestly, I have no clue how this would work in the very slightest. Is it even feasible?)

Four poles at each corner of the operating area with infrared sensors creating some kind of invisible fence. As the robot passes through the invisible fence, and infrared receiver will pick up the signals and tell the robot to turn around. (I've heard infrared works only up to 30 feet, but this is for lawns, so I'm not sure if this would work, also how would the infrared sensors be powered (that's something I will need to check for myself)).

Red or blue rope about 6 inches off the ground with spikes holding it there. A sensor on the robot will detect this rope, and that forms the boundary. I like this idea a little bit but let's be honest, it would look kind of ugly in someone's front (or back) yard.

GPS (isn't nearly accurate or detailed enough)

As you can see, there's many potential solutions, but I'm trying to figure out which one will work 100% and is still cost effective. Thank you in advance! Also, if you have any ideas, please do let me know!

答:
Having worked on robotic lawn mowers in the past.  I can tell you from first hand experience that this is no easy task.  Outdoor robots are hard.  Much harder than indoor robots.  Especially if you want them to operate over a long period of time (days). Some reasons for the difficulty:

Everything gets dirty.  So cameras and wheels will get covered in dirt, leaves, grass, bugs, etc.

Extensive wheel slip.  Your wheels will slip A LOT.

Uneven terrain.  No matter how flat the yard looks to you, it is actually very uneven.  Not to mention if you go over a small rock or tree root, that can alter your heading in a drastic and unpredictable way.

Harsh lighting.  Contending with full sun (and full shade) can be quite difficult.

Scale.  Typically outdoor robots deal with much greater distances than indoor robots.

Things change.  The look of a place can change dramatically over the course of a day, let alone a season due to the movement of the sun, falling leaves, growing bushes, etc.

Here are some notes on your proposed solutions:

Buried Guide Wires.  This is your best bet for keeping the robot contained.  It won't localize the robot, but is the quickest and cheapest way to prevent the robot from running off.  There is a reason every robotic lawn mower on the market uses this technology.  The sensors on the robot are cheap.  The expense comes from the spool of copper wire you need.  I'd actually recommend that you just buy a robotic lawn mower and hack it to your needs.  It will save you a lot of work.

LiDAR to pole markers.  Note that you would probably be doing trilateration instead of triangulation.  This can get tricky if the ground is very uneven and distances are large.  If the robot is tilted, the LiDAR on the robot might go off the top of the pole, or hit the ground instead.  And you will also have to deal with picking out the poles in the LiDAR data which might be difficult if there are other trees around.  But I think I've seen a beach drawing robot that uses this technique.

Dead Reckoning.  This won't work due to wheel slip and large distances.

AR tags.  Besides the issues with having cameras outdoors and keeping the lens clean.  This can suffer because of the distances involved.  Seeing an AR tag from across the yard with enough resolution to localize to it means a VERY big tag.  Then when you are close to the tag, it will be too large to use...

Infrared fence.  I doubt this will work very well in full sun.  The sun puts out a lot of infrared and will blind and washout your IR receivers.

Rope fence. Actually, this is probably the cheapest.  You can't go wrong with a physical boundary.  In general, I think the more you can engineer the environment, the easier it will be to localize the robot and keep it contained.

GPS.  Right, GPS alone isn't all that accurate.  Especially next to a house or under trees where you have limited satellites in view.  There is differential GPS which uses a fixed GPS base station, and GPS on the robot.  It will give you better performance.  But you still have to deal with satellite visibility (and seeing the same satellites between your two receivers).  But it's hard for me to say if this will be accurate enough for your application.  Actually, there is one robot lawn mower that uses differential GPS.


问:
How to calculate holding and operating torque at the base?
Could anyone please help me with the formula to calculate the operating torque and holding torque at the base to rotate the structure on top of it as shown in image with white arrows?

Thank you in advance for your help.
Have a nice weekend.

答:
Without measuring friction coefficients and weights and such, there is no way to compute a number. But you asked instead about a formula.

The operating torque will be the sum of

static friction (getting it unstuck so it will start moving)

rolling/sliding friction (joints and bearings)

accelerating the moment of inertia

overcoming whatever externally might be trying to move it or constrain it (who knows, wind blowing on the operator's cage? bashing into the building?)

For holding torque (no motion) we see no rolling friction and no inertia. Furthermore, the static friction helps it stay in one position.  So holding torque would be the "overcoming" torque (as above) minus static friction. Of course that can't really go negative -- like if there's no wind and the brake is on.


问:
How much weight can this DC motor carry?
I'm trying to make a RC Car with a total weight around 1kg. It will be 4-wheel drive with differential drive algorithm. The count of motors will be 4 DC motors simply. I have chosen the following motor.

The wheel diameter is 4 cm for now.

Motor Link. Following are its parameters as described in the link:

RPM - 100

Torque - 2kg-cm

Supply Voltage- 3.7-6V

Weight - 20g

How can I estimate with these things how much weight my RC Car can carry?

答:
It seems to me that a credible estimate could come only from someone with a lot of experience with RC cars similar to your design. It depends on many things that aren't all that easy to measure or even quantify. Here are just a few areas of uncertainty that the estimate might depend on:

friction in your axles

friction due to your tires flexing at the floor

texture of the floor surface (wood, carpet, or how short did you mow the grass)

bumps and dips along the way

relative static friction vs rolling friction

how well does your speed controller react to varying load (like, if it strains to get unstuck and then suddenly darts away)

I would suggest that you just build one and test it with different weights. Then build another, and another. This endeavor has no ceiling. You can add sensors, add more power, add onboard computation, make it autonomous. Give it one or more arms, and end effectors. Make it recognize what it sees. Make it run races, dodge obstacles, fold laundry, fetch you a beverage from the fridge. And use your imagination, not mine!