问:
How to design a controller for a robotic arm given two velocity profiles
See image for reference:

Essentially I have a robotic arm with two linear actuators. Lets say, for example, I wanted the endpoint on the arm (where you see the stepper motors) to move purely in the x direction (extending outwards). Both linear actuators would have to move in tandem with each other at the correct velocities to ensure that they cancel out each others y movements and sum up their x movements.

I have equations that relate the required angular velocity of each segment (and therefore velocities of each linear actuator) to get from point A to point B. I am just confused how to implement this now.

My electronics consist of an arduino and H-bridge. Feedback are potentiometers at the joints.

I'm just looking for general guidance on the procedure, as I am just not sure what steps I need to take here. I have taken a number of classes in control systems, but don't quite understand how to relate all the theory to practice.

答:
At a high level, you would need to do something like this:

Create a kinematic chain the helps you map from the 2D configuration space (the position of the linear actuators) to the 3D task space (the position of the end of the robotic arm).

Use inverse kinematics (IK) to figure out how the actuators needs to be positioned for a desired task space position. If you use a library like KDL to represent your kinematics, it is easy to use one of their inverse kinematic solvers.

Write a controller that uses this IK solver to move the arm as needed... This part depends on how you are able to control your linear actuators. If your interface to these actuators is a position, identify a sequence of positions in configuration space that you need to traverse will end in the desired task space position.


问:
Counterweight in arm designs
I wonder why most robotic arms use large J2 (shoulder) motors rather than counterweights. What sort of complexity do they add to the design beside increase in overall weight? In fact, rather than using extra weight as counterweight, the J3 (elbow) motor could be shifted down past J2 and use J3 motor as counterweight, but why isn't it done?</p>

答:
The weight of the motors is less then the weight of the gears and bearings in the joint. So moving the motors only not that advantageous. Moving the gearboxes can be done, but bearings still need to be left at the joint. If motors and gearboxes are moved, but bearings are not, then additional bearings are required for the "moved" gearboxes and some kind of transmission shaft or mechanism will also be needed, which adds further weight.

Many large robots include a hydro-pneumatic "spring" at J2 instead of a mass. This has more flexibility than a mess, since the force of this spring can be more easily adjusted (by increasing/decreasing the pressure). Usually this is set up based on the intended load of the robot.

J3 is sometimes co-axial with J2, but this is more common for older robot models or currently for palatalizing robots.

As for the J3 axis, moving J4-6 behind J3 is currently more popular then moving J3 behind or next to J2. Some manufacturers favour this approach, but even if all 3 motors are placed behind J3 a counterweight might still be needed, as this image shows. The black element behind the 3 motors next to J3 is a counterweight. this counterweight is optional, based on the intended load.  (Please also note the hydro-pneumatic spring next to J2).

ContentLicense: CC BY-SA 4.0

Title: Counterweight in arm designs


问:
Implementation of a particle filter algorithm
How do you calculate the likelihood of making an observation ? </p>

<p>Does someone have a link to a book or article that explains the math that you need to do to implement a PF algorithm?
Conceptually i understand the implementation but I'm having a hard time updating the particles after a measurement update. Any resource that goes over the math in details/ with examples would really help. </p>

答: Here you can find a really interesting video explanation on particle filters.</p>


问:
What is an example of a good motion control API?
We are in the process of implementing a motion control API for a research project. The goal is to implement a somewhat comprehensive API for motion control that provides the most commonly used primitives, while providing a workable programming model.

There doesn't seem to be any standardization in the industry (or is there?), and the space seems fragmented. But a good place to start would be to look at common use cases and existing implementations. Do you have suggestions on how to go about exploring this space? Or suggestions for well implemented APIs?

We intend for this to be good enough to be adopted for commercial use eventually.

答:
You can take a look at PLCOpen Motion. This can be thought of a motion control SDK for programming languages used in the automation world. This is general purpose and well established.

Furthermore, you can take a close look at all Robot Controller programming languages (like Kuka KRL, ABB Rapid, Fanuc Karel, Staubli Val3, etc.) These are programming languages build explicitly for motion control applications involving robots, so less general purpose then PLCOpen. The motion control related things are built in the language and do not have an explicit SDK or API character.

For CNCs there is G-Code. It is a language for motion control, but its syntax is a direct successor of the punch-card system used before textual programming languages, so it is really bad in the syntax sense, but very good in the features which it offers for motion control of machine tools.

If you want to have an example of motion control and a modern programming language, you can look at Kuka's Sunrise platform, which uses Java to program robots. I have never used it, so I do not know if it is any good.

In general purpose motion control PLCOpen is the closest you get to a standard, basically anything else is manufacturer specific (event the standardized G-Code has manufacturer specific syntax and features).


问:
RLException: neither a launch file package nor a launch file name
To get straight to the point I want to know how to fix this error and understand it, what is an RLException? Why am I getting this? How can I avoid this later? Below I will post what I did step by step:

This is the directory where I have my files

```<code>~/Desktop/Research/research_ws
</code>```

This is the error i'm getting:

```<code>RLException: [ddrobot_rviz.launch] is neither a launch file in package [ros_robotics] nor is [ros_robotics] a launch file name
The traceback for the exception was written to the log file
</code>```

The following are the codes for dd_robot.urdf and ddrobot_rviz.launch respectively:

```<robot name="dd_robot">

    <!-- Base Link -->
    <link name ="base_link">
        <visual>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <geometry>
                <box size = ".5 .5 .25"/>
            </geometry>
        </visual>
    </link>
</robot>
```
```    <!-- values passed by command line input -->
    <arg name = "model" />
    <arg name = "gui" default ="False" />

    <!-- Set these parameters on Parameter Server -->
    <param name = "robot_description"
        textfile = "<span class="math-container">$(find ros_robotics)/urdf/$</span>(arg model)" />
    <param name = "use_gui" value = "$(arg gui)" />

    <!-- Start 3 nodes: joint_state_publisher, robot_state_publisher
    rviz -->
    <node name = "joint_state_publisher" pkg = "joint_state_publisher"
            type = "joint_state_publisher" />
    <node name = "robot_state_publisher" pkg = "robot_state_publisher"
            type = "state_publisher" />
    <node name = "rviz" pkg = "rviz" type = "rviz"
        args = "-d $(find ros_robotics)/urdf.rviz"
        required = "true" />
</launch>
```

I do not yet completely understand what is written in the above codes, I'm following the ROS robotics by example book Fairchild.

答:
I just had to re-enter:

```<code>source devel/setup.bash
</code>```

In every new terminal.

Body:
I just had to re-enter:

source devel/setup.bash

In every new terminal.


问:
Applying Rotation & Translation Matrix Obtained from Iterative Closest Point
Suppose I have M lidar contour points from t = k and N lidar contour points from t = k+1.

(Some of you might question why I have differing number of contour points, and the reason is that these lidar contour points have been filtered from raw point cloud data to only detect road edge, and sometimes these edges aren't detected.)

My goal is to localize the vehicle position using these lidar contour points. Currently, I have generated two plots to make comparison to the result of lidar localization.

- Plot 1. GPS trajectory plot (this will be my reference)

- Plot 2. Vehicle odometry trajectory plot (this has drift compared to GPS trajectory)

I need to prove that my lidar localized trajectory somewhat improved the drift that existed from plot 2.

My current approach: I am using ICP to solve the problem. Since I do not have 'absolute reference' point cloud, I am basically using lidar points at t = k as a reference to lidar points at t = k+1. Therefore, the reference point cloud is constantly being updated as I go. Once I derive the rotation & translation vector between t = k and t = k+1, I'd like to use that to correct the vehicle trajectory from t = k to t = k+1.

Note: Exception case is when lidar contour data at t = k + a is empty, in which I simply use odometry (dead reckoning) to predict the trajectory.

My difficulty lies with the fact that I am lost on how to apply the rotation and translation vector to the previous vehicle pos_x, previous vehicle pos_y, and heading angle theta to make the correction to the trajectory. I suppose the heading angle value doesn't really matter in terms of updating the next position since I am working with 'point' rather than shape. The code below is snippet from my work, where closest_rot_angle, closest_translation_x, and closest_translation_y are obtained from ICP iterations until it converges.

I think the problem may be at the update step for the 'input_arr[0:2]' which contains x & y coordinate for the current position of the vehicle. Please provide me with some spark on a good way to apply ICP results for localization!

```<code>        # transform 'points' (using the calculated rotation and translation)
        c, s = math.cos(closest_rot_angle), math.sin(closest_rot_angle)
        rot = np.array([[c, -s],
                        [s, c]])
        aligned_points = np.dot(points, rot.T)
        aligned_points[:, 0] += closest_translation_x
        aligned_points[:, 1] += closest_translation_y

        aligned_target = np.dot(input_arr[0:2], rot.T)
        aligned_target[0] += closest_translation_x
        aligned_target[1] += closest_translation_y

        # update 'points' for the next iteration
        points = aligned_points
        input_arr[0:2] = aligned_target

</code>```

答:
I don't know if your confusion is with applying the transform to the points or applying it to the pose. So I'll just show you both.

The easiest way is to store your points and transform in the homogenous form. For 2D the transform is a matrix(3x3) that looks like

$$T=\begin{bmatrix}
cos(\theta) & -sin(\theta) & t_x\\
sin(\theta) & cos(\theta) & t_y \\
0 & 0 & 1
\end{bmatrix} $$

Points:

A point in homogenous form just has a 1 appended at the end.

$$p=[x,y,1]$$

You can now multiply a point by this transform matrix to get the new position.

$$p_b=T_{ba}*p_a$$

where $a$ and $b$ are some coordinate frames. If you actually break this down you can see that the multiplication is the same as your code.

$$p_b=R_{ba}*p_a+t_{ba}$$

Poses:

Your vehicle pose of $[t_x,t_y,\theta]$ can be represented as a homogenous matrix. The nice thing about this form is that you can concatenate multiple transforms together with just matrix multiplication.

$$T_a=T_b*T_{ba}$$

In your application ICP solves for the delta transform between two coordinate frames. So at every timestep solve for the delta using ICP and multiply your previous pose.

$$T_{k+1}=T_{k}*T_{k,k+1}$$

Note depending on your ICP algorithm your answer may be the inverse transform so $T_{k+1,k}$. Another nice property of the homogenous matrix is that the matrix inverse also is also the inverse of the transform.

$$T_{k+1,k}^{-1}=T_{k,k+1}$$


问:
In SLAM loop closure, What does a sim3 matrix do?
I am confused between 3D rigid affine(with scale, without shear) transformation and Lie group's sim(3) matrix.

3D affine would be [sR|t; 0 1].
we rotate, scale and then translate a 3D point using 3D affine transform

\begin{matrix}
s\cdot r_{11} & s\cdot r_{12} & s\cdot r_{13} & t_1\\
s\cdot r_{21} & s\cdot r_{22} & s\cdot r_{23} & t_2\\
s\cdot r_{31} & s\cdot r_{32} & s\cdot r_{33} & t_3\\
0 & 0 & 0 & 1
\end{matrix}

sim3, according to [1] is [R|t; 0 1/s]

\begin{matrix}
r_{11} & r_{12} & r_{13} & t_1\\
r_{21} & r_{22} & r_{23} & t_2\\
r_{31} & r_{32} & r_{33} & t_3\\
0 & 0 & 0 & s^{-1}
\end{matrix}

In loop closure context, for a candiate loop closure keyframe, we have  an old pose(SE3) and its calculated sim3. sim3 has a scale, rotation and translation part.

what is this sim3 actually? How do I get the new pose (SE3) using sim3 ?

How do I get the new set of 3D points visible on the old keyframe pose?

In ORB slam code, the authors find the new set of 3D points from old set by first finding camera coordinates of the 3D points (using old pose) and then multiplying it by the inverse of calculated sim3

If sim3 is a matrix that maps the old points to new points, why don't we multiply the 3D points directly with sim3 matrix?

Reference:
[1] http://ethaneade.com/lie_groups.pdf

答:
Sim3 is just the lie group associated a similarity transform which is the exact same thing as your 3D Affine transform. If you work out the multiplication for your two matrices you would find that they end up with the same result as we are dealing with homogenous coordinates. Therefore the following is true

$$x=kx$$

where $k$ is just some constant, and $x$ is a homogenous vector.

Now if we multiply a vector $[x,y,z,1]$ by your matrices. The results for the first row are shown below.

For affine matrix:

$$s(r_{11}*x + r_{12}*y + r_{13}*z) + t_1$$

Sim3 matrix:

$$r_{11}*x + r_{12}*y + r_{13}*z + \frac{t_1}{s^{-1}}$$
Multiply by $s$
$$s(r_{11}*x + r_{12}*y + r_{13}*z) + t_1$$

So the results end up differing by some scaling constant $s$, which means they are the same point.

what is this sim3 actually?

Similarity transform. Almost same thing as SE3, but with a extra degree of freedom for scaling. What you call 3D affine.

How do I get the new pose (SE3) using sim3?

You can't really. It has 1 extra degree of freedom so any conversion will lose information.

How do I get the new set of 3D points visible on the old keyframe pose?

You answered your own question. Multiply the point by the sim3 matrix. In ORB-SLAM they just break up the multiplication into the individual parts, but it is the same as multiplying by the matrix.

If sim3 is a matrix that maps the old points to new points, why don't
we multiply the 3D points directly with sim3 matrix?

You can, and that's what ORB-SLAM even does. The inverse is just used cause the sim3 transform is in the wrong direction(e.g. from keyframe 2 -> keyframe 1) when you want the opposite.

In SLAM loop closure, What does a sim3 matrix do?

It does the same thing as the SE3 or SE2 transform in pose graph optimization. Add a constraint between poses. Sim3 is used instead as monocular based SLAM is special ,because scale/distance is unobservable. So the actual correct method to model this motion is sim3.


问:
Does an S type load cell need to be loaded perpendicular to its screw holes?
I know that a bar type load cell needs to be setup as shown here as to flex the strain gauge

However, most tutorials or guides show how to setup an S type load cell like this (for tension)

and not much info on how to setup for compression. Im guessing the load does not need to be right on top of the screw hole as per the bar type? I would be able to mount a platform directly onto the top and bottom surface of the "S", right?

On another note do most load cells already come wired with a wheatstone bridge? (some dude in a tutorial mentioned this)

I'm a newbie and I'd appreciate any advice.

Edit: This is the load cell Im looking to purchase. Does not mention much other than the material properties in their 'datasheet'. link

答:
Your ability to calibrate a load cell is going to depend on the orientation of the strain gauges inside the load cell, which is determined by however the manufacturer chooses to apply or embed them.

There are all kinds of "tricks" you can do with strain gauges, like being able to remove the effects of bending moments, thermal compensation, etc., but again it all depends on how the strain gauges are applied.

If you are at all unsure of how the load cell is intended to be used then the first place you should go to is the datasheet.

The top image in your question looks like it's setup to torque the load cell, but if you have a strain gauge on top and one on bottom, and they're wired in series, then one strain gauge is in tension when the other is in compression and the net result is (nearly) zero change in total resistance - this is one of the "neat tricks" I mentioned previously. Such an arrangement would give (virtually) no change in reading if you were to put that on a countertop and place items on it like a weighing scale, but WOULD give you a change if you were to mount it on a wall and use it like a coat rack.

Again, it all depends on how the strain gauges are installed and wired. Consult the datasheet!


问:
Does every rotation vector has an one-to-one corresponding rotation matrix?
Given a rotation vector, can we always get the same corresponding rotation matrix computed by the Rodrigue's rotation formula?

答:
Mathematically a rotation vector(or axis angle) representation will always convert to the same rotation matrix.

However, multiple different rotation vectors can lead to the same rotation matrix.

A rotation vector represents a rotation by an axis and an angular rotation around that axis(axis angle).

$$ \begin{bmatrix} e_x \\ e_y \\ e_z \end{bmatrix},\theta$$

Thinking about it for a bit it is pretty easy to see where different axis angle representations will lead to the same rotation.

One is when the angle($\theta$) is the same angle but in the opposite direction. E.g $\frac{\pi}{2}$ and $\frac{-3\pi}{2}$.

The other is when the axis points in the opposite direction, and the angle is also in the opposite direction.

$$ \{\begin{bmatrix} e_x \\ e_y \\ e_z \end{bmatrix},\theta\}=\{\begin{bmatrix} -e_x \\ -e_y \\ -e_z \end{bmatrix},-\theta\}$$

Finally for numerical reason oftentimes the Rodrigues formula will also set some values constant/Identity if the rotation is small enough. So while mathematically they should lead to distinct values, on your computer they might end up being the same thing.


问:
Get Topic values in script
Hi i am doing an apriltag following project. I am currently trying to test how to get distance between the tag and the camera. I have subscribed to the AprilTagDetectionArray msg.

My main question is how do i get the distance between the tag and my camera. Should i use the position or the orientation values? I want the robot to detect and then approach the tag.

I tried getting the values from both position and orientation but i am getting the error below.

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

import rospy
from apriltag_ros.msg import AprilTagDetectionArray

w = 0.0

def tagDetect(msg):
    global w
    w = msg.pose.pose.orientation.w
    w = abs(w)
    rospy.loginfo("w = %s",w)

def listener():
    rospy.init_node("listener",anonymous=True)
    rospy.Subscriber("/tag_detections",AprilTagDetectionArray,tagDetect)
    rospy.spin()

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

but i am getting this error. how do i resolve this??

```<code> [ERROR] [1593270010.489688]: bad callback: <function tagDetect at 0x7f53e4b62a28>
Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/zeus/catkin_ws/src/autonav/src/test.py", line 10, in tagDetect
    w = msg.pose.pose.position[0].x
AttributeError: 'AprilTagDetectionArray' object has no attribute 'pose'
</code>```

Here is the message just in case.

```<code>[apriltag_ros/AprilTagDetectionArray]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
apriltag_ros/AprilTagDetection[] detections
  int32[] id
  float64[] size
  geometry_msgs/PoseWithCovarianceStamped pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/PoseWithCovariance pose
      geometry_msgs/Pose pose
        geometry_msgs/Point position
          float64 x
          float64 y
          float64 z
        geometry_msgs/Quaternion orientation
          float64 x
          float64 y
          float64 z
          float64 w
      float64[36] covariance
</code>```

答:
I can't comment, so writing this as an answer. Did you try calling msg.detections.pose.pose.orientation.w?

From your message definition hierarchy, is seems pose is under object detections.

Title: Get Topic values in script


问:
Custom implementation of robot_localization package
I plan to implement a sensor fusion of IMU + Visual odometry using an EKF. I came across the excellent robot_localization package which does pretty much all that I want. However, I need to use perform my task without using ROS, and work in an offline setting, where I have a dump of my IMU and Visual odometry data timestamped. Is there a way to use the package as a library, by calling functions as APIs? How would I go about it, has it been done?

If not, are there any alternatives that fuse IMU + visual odometry data using an EKF or a UKF? I ideally wish to code in Python, however I am comfortable in C++ as well.

答:
Why would you not like to use ROS? Seems like if you use this package along with custom code for the rest of the solution it'll be good to go.

Is there a way to use the package as a library, by calling functions
as APIs? How would I go about it, has it been done?

It is possible by altering the code to remove ROS dependencies and use the cor filter algorithm in the package. However I won't suggest that. There should probably be other libraries independent of ROS to achieve this. You can try editing the code as a last resort.

I've been using robot_localization package and have been getting satisfying results


问:
Regarding kalman filter implementation
I think I might have originally posted this in the wrong stack exchange forum, link. I think this might be the right place, I am not sure if posting again is considered "bad style", I apologize if this is the case.

My questions is regarding the implementation of a discrete time Kalman filter assuming the time update occurs much more often than measurement update. I'll be specifically looking at the covariance propagation and Kalman gain equations.

Given a D.T. KF with the following state space model:
$$
\hat{x}_{k+1} = F \hat{x}_k + G \omega_k
$$
$$
\hat{y}_k = C \hat{x}_k + \upsilon_k
$$
and assuming $\hat{x}^-(0)$ and $P^-(0)$ are known as well as the process and measurement noise intensities (Q and R respectively) the relevant equations are:

Gain update:
$$
K = P^-C^T (CP^- C^T + R)^{-1}
$$
Measurement update
$$
P^- = F P^+ F^T + Q
$$
Time Update:
$$
P^+ = (I-KC)P^-
$$

The difficulty I am having is with respect to implementation and how to properly initialize. A pseudo code example of what I think should be done is the following:

```<code>% Assume P0 is given
Pp = P0;                                       % Initializing P-
for i=1:N                                      % N = number of measurement updates
    for j=1:m                                  % m = number of time updates in one measurement update
        Pp = F*Pu*F' + Qd;                     % covariance prop
    end
    K = Pp*C'*(R + C*Pp*C')^(-1)        % update gain
    Pu = (eye(nx) - K*C)*Pp;            % measurement update
end
</code>```

But this has the problem that the first iteration Pp cannot compute because there has yet to be a measurement update. This is easily solved by forcing a measurement update before any time updates. Maybe it's just me but it seems kind of incorrect to NEED a measurement update before any time updates.

答:
D.T Kalman filter time update equations (predict) with initial estimates for $\hat{x}_k$ and $P_{k}$

Project the state ahead: $\hat{x}^-_{k+1} = F\hat{x}_{k}+Bu_{k}$

Project the error covariance ahead: $P^-_{k+1} = FP_{k}F^T+Q$

Measurement equations (correct)

Compute the Kalman gain: $K_{k+1} = P^-_{k+1}C^T(CP^-_{k+1}C^T+R)^{-1}$

Update estimate with measurement: $\hat{x}_{k+1} = \hat{x}^-_{k+1}+K_{k+1}(y_{k+1}-C\hat{x}^-_{k+1})$

Update the error covariance: $P_{k+1}= (I-K_{k+1}C)P^-_{k+1}$

Note the super minus sign is for a priori estimate.

Title: Regarding kalman filter implementation


问:
2D Visual-Inertial Extended Kalman Filter
I am trying to implement an Extended Kalman filtering for combining IMU data and visual odometry in a simple 2D case where I have a robot that that can only accelerate in its local forward direction which is dictated by its current heading (theta). I am restricting IMU readings to a single acceleration reading (a) and a single angular velocity reading (omega). Visual odometry will only provide a single angular displacement as well as displacement in the u and v directions (x and y relative to the robot). The equations for the derivation of my state transition matrix are

$$ x_{k+1} = x_k + \dot{x_k}\Delta T + 0.5a \cdot cos(\theta) \Delta T^2 $$
$$ y_{k+1} = y_k + \dot{x_k}\Delta T + 0.5a \cdot sin(\theta) \Delta T^2 $$
$$ \theta_{k+1} = \theta_k + \dot{\theta} \Delta T $$
$$ \dot{x_{k+1}} = \dot{x_{k}} + a \cdot cos(\theta) \Delta $$
$$ \dot{y_{k+1}} = \dot{y_{k}} + a \cdot sin(\theta) \Delta $$
$$ \dot{\theta_{k+1}} = \dot{\theta_{k}} $$
$$ \dot{\dot{x_{k+1}}} = \dot{\dot{x_{k}}}$$
$$ \dot{\dot{y_{k+1}}} = \dot{\dot{y_{k}}}$$

and the equations that I use to obtain the measurements are

$$ \Delta x = \dot{x} \Delta T + 0.5 \dot{\dot{x}} \Delta T^2 $$
$$ \Delta y = \dot{y} \Delta T + 0.5 \dot{\dot{y}} \Delta T^2 $$
$$ \Delta u = \Delta x \cdot cos(\theta) + \Delta y \cdot sin(\theta) $$
$$ \Delta v = -\Delta x \cdot sin(\theta) + \Delta y \cdot cos(\theta) $$
$$ \Delta \theta = \dot{\theta} \cdot \Delta T $$
$$ a = \dot{\dot{x}} \cdot cos(\theta) + \dot{\dot{y}} \cdot sin(\theta) $$
$$ \omega = \dot{\theta} $$

To calculate the Jacobian of the measurement function I used the following MATLAB code

```<code>deltaX = xDot*t + 0.5*xDotDot*(t^2);
deltaY = yDot*t + 0.5*yDotDot*(t^2);
deltaU = deltaX * cos(theta) + deltaY * sin(theta);
deltaV = -deltaX * sin(theta) + deltaY * cos(theta);
deltaTheta = thetaDot*t;
accel = xDotDot*cos(theta) + yDotDot*sin(theta);
omega = thetaDot;

jacobian([accel, omega, deltaU, deltaV, deltaTheta], [x, y, theta, xDot, yDot, thetaDot, xDotDot, yDotDot])
</code>```

To test my implementation I am creating test data from random acceleration and angular velocity values. I am plotting the trajectory calculated from this as well as from the trajectory calculated directly using the odometry values and the IMU values. I am then comparing this with the odometry estimated by my Kalman filter.

The Kalman filter has been implemented without any control values and is combining all the sensor reading into a single measurement vector.

To test if the filter has any hope of working, I first tested it without any added measurement noise but the outcome is fairly crazy as can be seen in

where it can also be seen that using both sensor readings on their own without the filter produces the exact trajectory. This simulation, including my Kalman filter was implemented with the following Python code

```<code>import numpy as np
import matplotlib.pyplot as plt
from random import *

# Sampling period
deltaT = 1

# Array to store the true trajectory
xArr = [0]
yArr = [0]
thetaArr = [0]

# Array to store IMU measurement
imuA = []
imuOmega = []

# Current state variables
x = 0
y = 0
theta = 0
x_dot = 0
y_dot = 0

# Arrays to store odometry measurements
odoU = []
odoV = []
odoTheta = []

# Setup simulated data
for i in range(100):
    # Calculate a random forward (u-axis) acceleration
    a = uniform(-10, 10)
    imuA.append(a)

    # Calculate the change in global coordinates
    deltaX = (x_dot * deltaT) + (0.5 * a * np.cos(theta) * deltaT**2)
    deltaY = (y_dot * deltaT) + (0.5 * a * np.sin(theta) * deltaT**2)

    # Update the velocity at the end of the time step
    x_dot += a * np.cos(theta) * deltaT
    y_dot += a * np.sin(theta) * deltaT

    # Update the current coordinates
    x += deltaX
    y += deltaY

    # Store the coordinates for plotting
    xArr.append(x)
    yArr.append(y)

    # Calculate local coordinate odometry
    odoU.append(deltaX * np.cos(theta) + deltaY * np.sin(theta))
    odoV.append(-deltaX * np.sin(theta) + deltaY * np.cos(theta))

    # Calculate a random new angular velocity
    theta_dot = uniform(-0.2, 0.2)
    imuOmega.append(theta_dot)

    # Calculate the change in angular displacement
    deltaTheta = theta_dot * deltaT
    odoTheta.append(deltaTheta)

    # Update the angular displacement
    theta += theta_dot * deltaT
    thetaArr.append(theta)

# Calculate the trajectory from just the odometery
xArr2 = []
yArr2 = []

x = 0
y = 0
theta = 0

for i in range(100):
    deltaU = odoU[i]
    deltaV = odoV[i]
    deltaTheta = odoTheta[i]

    x += deltaU * np.cos(theta) - deltaV * np.sin(theta)
    y += deltaU * np.sin(theta) + deltaV * np.cos(theta)
    theta += deltaTheta

    xArr2.append(x)
    yArr2.append(y)

# Calculate the trajectory from just the IMU readings
xArr3 = []
yArr3 = []

x = 0
y = 0
theta = 0
x_dot = 0
y_dot = 0
theta_dot = 0

for i in range(100):
    # Calculate the change in global coordinates
    a = imuA[i]
    deltaX = (x_dot * deltaT) + (0.5 * a * np.cos(theta) * deltaT**2)
    deltaY = (y_dot * deltaT) + (0.5 * a * np.sin(theta) * deltaT**2)

    # Update the velocity at the end of the time step
    x_dot += a * np.cos(theta) * deltaT
    y_dot += a * np.sin(theta) * deltaT

    # Update the current coordinates
    x += deltaX
    y += deltaY

    # Store the coordinates for plotting
    xArr3.append(x)
    yArr3.append(y)

    # Calculate the change in angular displacement
    theta_dot = imuOmega[i]
    theta += theta_dot * deltaT

# Estimate the true trajectory with a Kalman filter

# State matrix
X_k_min = np.array([
    [0], # x
    [0], # y
    [0], # theta
    [0], # x_dot
    [0], # y_dot
    [0], # theta_dot
    [0], # x_dot_dot
    [0]  # y_dot_dot
])

# State covariance matrix
P_k_min = np.zeros((8, 8))

# State transition matrix
A = np.array([
    [1, 0, 0, deltaT, 0, 0, 0.5*deltaT**2, 0],
    [0, 1, 0, 0, deltaT, 0, 0, 0.5*deltaT**2],
    [0, 0, 1, 0, 0, deltaT, 0, 0],
    [0, 0, 0, 1, 0, 0, deltaT, 0],
    [0, 0, 0, 0, 1, 0, 0, deltaT],
    [0, 0, 0, 0, 0, 1, 0, 0],
    [0, 0, 0, 0, 0, 0, 1, 0],
    [0, 0, 0, 0, 0, 0, 0, 1]
])

# Process covariance matrix
Q = np.eye(8)

# Measurement vector
## 0: a (forward acceleration)
## 1: omega (angular velocity)
## 2: deltaU (local x displacement)
## 3: deltaV (local y displacement)
## 4: deltaTheta (local angular displacement)

# Measurement covariance matrix
R = np.eye(5)

# Function to calculate the measurement function Jacobian
def CalculateH_k(X, t):
    theta = X[2, 0]
    xDot = X[3, 0]
    yDot = X[4, 0]
    xDotDot = X[6, 0]
    yDotDot = X[7, 0]

    return np.array([
        [0, 0, yDotDot * np.cos(theta) - xDotDot * np.sin(theta), 0, 0, 0, np.cos(theta), np.sin(theta)],
        [0, 0, 0, 0, 0, 1, 0, 0],
        [0, 0, np.cos(theta) * ((yDotDot * t**2) / 2 + yDot * t) - np.sin(theta) * (
                    (xDotDot * t**2) / 2 + xDot * t), t * np.cos(theta), t * np.sin(theta), 0, (t**2 * np.cos(theta)) / 2, (
                     t**2 * np.sin(theta)) / 2],
        [0, 0, - np.cos(theta) * ((xDotDot * t**2) / 2 + xDot * t) - np.sin(theta) * (
                    (yDotDot * t**2) / 2 + yDot * t), -t * np.sin(theta), t * np.cos(theta), 0, -(t**2 * np.sin(theta)) / 2, (
                     t**2 * np.cos(theta)) / 2],
        [0, 0, 0, 0, 0, t, 0, 0]
    ])

# Measurement function
def Measure(X):
    theta = X[2, 0]
    xDot = X[3, 0]
    yDot = X[4, 0]
    thetaDot = X[5, 0]
    xDotDot = X[6, 0]
    yDotDot = X[7, 0]

    deltaX = xDot * deltaT + 0.5 * xDotDot * (deltaT**2)
    deltaY = yDot * deltaT + 0.5 * yDotDot * (deltaT**2)
    deltaU = deltaX * np.cos(theta) + deltaY * np.sin(theta)
    deltaV = -deltaX * np.sin(theta) + deltaY * np.cos(theta)
    deltaTheta = thetaDot * deltaT
    accel = xDotDot * np.cos(theta) + yDotDot * np.sin(theta)
    omega = thetaDot

    return np.array([
        [accel],
        [omega],
        [deltaU],
        [deltaV],
        [deltaTheta]
    ])

xArr4 = []
yArr4 = []

for i in range(100):
    a = imuA[i]
    omega = imuOmega[i]

    # Setup the observation matrix
    Z_k = np.array([
        [imuA[i]],
        [imuOmega[i]],
        [odoU[i]],
        [odoV[i]],
        [odoTheta[i]]
    ])

    # Calculate the estimated new state
    X_k = A.dot(X_k_min)

    # Calculate the estimated new state covariance matrix
    P_k = A.dot(P_k_min).dot(np.transpose(A)) + Q

    # Find the measurement Jacobian at the current time step
    H_k = CalculateH_k(X_k_min, deltaT)

    # Calculate the Kalman gain
    G_k = P_k.dot(np.transpose(H_k)).dot(np.linalg.inv(H_k.dot(P_k).dot(np.transpose(H_k)) + R))

    # Calculate the improved current state estimate
    X_k = X_k + G_k.dot(Z_k - Measure(X_k_min))

    # Calculate the improved current state covariance
    P_k = (np.eye(8) - G_k.dot(H_k)).dot(P_k)

    xArr4.append(X_k[0, 0])
    yArr4.append(X_k[1, 0])

    # Make the current state the previous
    X_k_min = X_k
    P_k_min = P_k

plt.plot(xArr, yArr, linewidth=3)
plt.plot(xArr2, yArr2)
plt.plot(xArr3, yArr3)
plt.plot(xArr4, yArr4)
plt.legend(['Ground truth', 'VO', 'IMU', 'Filtered'])
plt.grid()
plt.show()
</code>```

I have double checked everything and just can't figure out what I am doing wrong even though it must be something obvious. Any ideas?

答:
Your noise term for the KF needs to reflect how you expect the true propagation of the state will differ from your model of the propagation. For example, the acceleration uncertainty is 1 while your true uncertainty is drawn from a uniform distribution of [-10,10].

I altered your code so the KF is now using the IMU information within the propagation step. It still needs to incorporate the IMU uncertainty correctly within the process noise. I also simplified the measurements to be the position and orientation of the state. You should probably rewrite the numerical Jacobians that I've used in favor of analytic Jacobians.

```<code>import numpy as np
import matplotlib.pyplot as plt
from random import *

# The state vector is
#  pos_x, pos_y, theta, vel_x, vel_y

def getVehRate(state, imu):
    # position rate is equal to velocity
    dxdy = state[3:5]

    # theta rate is euqal to the gyro measurement
    dtheta = imu[1]

    # velocity rate is equal to the accel broken into the xy basis
    dvelx = imu[0] * np.cos(state[2])
    dvely = imu[0] * np.sin(state[2])

    dstate = 0. * state
    dstate[0:2] = dxdy
    dstate[2] = dtheta
    dstate[3] = dvelx
    dstate[4] = dvely
    return dstate

def rk4(state, imu, func, dt):
    # runs a rk4 numerical integration
    k1 = dt * func(state, imu)
    k2 = dt * func(state + .5*k1, imu)
    k3 = dt * func(state + .5*k2, imu)
    k4 = dt * func(state + k3, imu)

    return state + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4)

def numericalDifference(x, func, data, ep = .001):
    # calculates the numeical jacobian

    y = func(x, data)

    A = np.zeros([y.shape[0], x.shape[0]])

    for i in range(x.shape[0]):
        x[i] += ep
        y_i = func(x, data)
        A[i] = (y_i - y)/ep
        x[i] -= ep

    return A

def numericalJacobianOfStatePropagationInterface(state, data):
    # data contains both the imu and dt, it needs to be broken up for the rk4
    return rk4(state, data[0], getVehRate, data[1])

# Sampling period
dt = .1
t_steps = 500
state = np.zeros(5)

state_hist = np.zeros([t_steps, 5])
imu_hist = np.zeros([t_steps, 2])

# Setup simulated data
for i in range(t_steps):
    # generate a rate to propagate states with
    accel = uniform(-10, 10)
    theta_dot = uniform(-0.2, 0.2)
    imu = np.array([accel, theta_dot])

    # propagating the state with the IMU measurement
    state = rk4(state, imu, getVehRate, dt)

    # saving off the current state
    state_hist[i] = state *1.
    imu_hist[i] = imu*1.

# kf stuff
state = np.zeros([5])
cov = np.eye(5) * .001

kf_state_hist = np.zeros([t_steps, 5])
kf_cov_hist = np.zeros([t_steps, 5,5])
kf_meas_hist = np.zeros([t_steps, 3])
kf_imu_hist = np.zeros([t_steps, 2])

# imu accel and gyro noise
accel_cov = .0001
gyro_cov  = .0001
Q_imu = np.array([[.1, 0],[0, .01]])

r_meas = .001

#  running the data through the KF with noised measurements
for i in range(t_steps):

    # propagating the state
    imu_meas = imu_hist[i]
    imu_meas[0] += np.random.randn(1) * accel_cov**.5
    imu_meas[1] += np.random.randn(1) * gyro_cov**.5

    A = numericalDifference(state, numericalJacobianOfStatePropagationInterface, [imu_meas, dt])
    cov = A.dot(cov.dot(A.T))

    ###
    # TODO : calculate how the accel and gyro noise turn into the process noise for the system
    ###
    # A_state_wrt_imu = jacobianOfPropagationWrtIMU
    # Q = A_state_wrt_imu * Q_imu * A_state_wrt_imu.T
    # cov += Q
    # sloppy placeholder
    cov += np.eye(5) * .1

    state = rk4(state, imu_meas, getVehRate, dt)

    # measurement update
    zt = state[:3]  + np.random.randn(1) *r_meas**.5
    zt_hat = state[:3]

    H = np.zeros([3,5])
    H[:3,:3] = np.eye(3)

    S = np.linalg.inv(H.dot(cov.dot(H.T)) + r_meas * np.eye(3))
    K = cov.dot(H.T).dot( S )

    #state = state + K.dot(zt - zt_hat)
    cov = (np.eye(5) - K.dot(H)).dot(cov)

    kf_state_hist[i] = state
    kf_cov_hist[i] = cov
    kf_meas_hist[i] = zt_hat
    kf_imu_hist[i] = imu_meas

plt.plot(state_hist[:,0], state_hist[:,1], linewidth=3)
plt.plot(kf_state_hist[:,0], kf_state_hist[:,1], linewidth=3)
plt.legend(['Ground truth', 'kf est'])
plt.grid()
plt.show()
</code>```

Body:
Your noise term for the KF needs to reflect how you expect the true propagation of the state will differ from your model of the propagation. For example, the acceleration uncertainty is 1 while your true uncertainty is drawn from a uniform distribution of [-10,10].

I altered your code so the KF is now using the IMU information within the propagation step. It still needs to incorporate the IMU uncertainty correctly within the process noise. I also simplified the measurements to be the position and orientation of the state. You should probably rewrite the numerical Jacobians that I've used in favor of analytic Jacobians.

import numpy as np
import matplotlib.pyplot as plt
from random import *

# The state vector is
#  pos_x, pos_y, theta, vel_x, vel_y

def getVehRate(state, imu):
    # position rate is equal to velocity
    dxdy = state[3:5]

    # theta rate is euqal to the gyro measurement
    dtheta = imu[1]

    # velocity rate is equal to the accel broken into the xy basis
    dvelx = imu[0] * np.cos(state[2])
    dvely = imu[0] * np.sin(state[2])

    dstate = 0. * state
    dstate[0:2] = dxdy
    dstate[2] = dtheta
    dstate[3] = dvelx
    dstate[4] = dvely
    return dstate

def rk4(state, imu, func, dt):
    # runs a rk4 numerical integration
    k1 = dt * func(state, imu)
    k2 = dt * func(state + .5*k1, imu)
    k3 = dt * func(state + .5*k2, imu)
    k4 = dt * func(state + k3, imu)

    return state + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4)

def numericalDifference(x, func, data, ep = .001):
    # calculates the numeical jacobian

    y = func(x, data)

    A = np.zeros([y.shape[0], x.shape[0]])

    for i in range(x.shape[0]):
        x[i] += ep
        y_i = func(x, data)
        A[i] = (y_i - y)/ep
        x[i] -= ep

    return A

def numericalJacobianOfStatePropagationInterface(state, data):
    # data contains both the imu and dt, it needs to be broken up for the rk4
    return rk4(state, data[0], getVehRate, data[1])

# Sampling period
dt = .1
t_steps = 500
state = np.zeros(5)

state_hist = np.zeros([t_steps, 5])
imu_hist = np.zeros([t_steps, 2])

# Setup simulated data
for i in range(t_steps):
    # generate a rate to propagate states with
    accel = uniform(-10, 10)
    theta_dot = uniform(-0.2, 0.2)
    imu = np.array([accel, theta_dot])

    # propagating the state with the IMU measurement
    state = rk4(state, imu, getVehRate, dt)

    # saving off the current state
    state_hist[i] = state *1.
    imu_hist[i] = imu*1.

# kf stuff
state = np.zeros([5])
cov = np.eye(5) * .001

kf_state_hist = np.zeros([t_steps, 5])
kf_cov_hist = np.zeros([t_steps, 5,5])
kf_meas_hist = np.zeros([t_steps, 3])
kf_imu_hist = np.zeros([t_steps, 2])

# imu accel and gyro noise
accel_cov = .0001
gyro_cov  = .0001
Q_imu = np.array([[.1, 0],[0, .01]])

r_meas = .001

#  running the data through the KF with noised measurements
for i in range(t_steps):

    # propagating the state
    imu_meas = imu_hist[i]
    imu_meas[0] += np.random.randn(1) * accel_cov**.5
    imu_meas[1] += np.random.randn(1) * gyro_cov**.5

    A = numericalDifference(state, numericalJacobianOfStatePropagationInterface, [imu_meas, dt])
    cov = A.dot(cov.dot(A.T))

    ###
    # TODO : calculate how the accel and gyro noise turn into the process noise for the system
    ###
    # A_state_wrt_imu = jacobianOfPropagationWrtIMU
    # Q = A_state_wrt_imu * Q_imu * A_state_wrt_imu.T
    # cov += Q
    # sloppy placeholder
    cov += np.eye(5) * .1

    state = rk4(state, imu_meas, getVehRate, dt)

    # measurement update
    zt = state[:3]  + np.random.randn(1) *r_meas**.5
    zt_hat = state[:3]

    H = np.zeros([3,5])
    H[:3,:3] = np.eye(3)

    S = np.linalg.inv(H.dot(cov.dot(H.T)) + r_meas * np.eye(3))
    K = cov.dot(H.T).dot( S )

    #state = state + K.dot(zt - zt_hat)
    cov = (np.eye(5) - K.dot(H)).dot(cov)

    kf_state_hist[i] = state
    kf_cov_hist[i] = cov
    kf_meas_hist[i] = zt_hat
    kf_imu_hist[i] = imu_meas

plt.plot(state_hist[:,0], state_hist[:,1], linewidth=3)
plt.plot(kf_state_hist[:,0], kf_state_hist[:,1], linewidth=3)
plt.legend(['Ground truth', 'kf est'])
plt.grid()
plt.show()


问:
How do I decide the minimum number of link lengths and joints my manipulator must have from the configuration space?
I have a configuration space in 3D where my manipulator must reach certain points. Taking into account all the factors how do I decide the mininum number of links and joints my arm must have?</p>

答:
If you should consider not just points, but poses (position and orientations). If you can write these as a table, where the columns are the coordinates

```<code>+-------+--------+--------+-------+---------+--------+--------+
| Point | X [mm] | Y [mm] | Z [mm]| A [deg] | B[deg] | C[deg] |
+-------+--------+--------+-------+---------+--------+--------+
| P1    |  100.0 |  200.0 | 400.0 |    90.0 |   20.0 |    0.0 |
| P2    | 1500.0 |  200.0 | 800.0 |    90.0 |   45.0 |    0.0 |
| P3    |   42.0 |  200.0 | 400.0 |     0.0 |   90.0 |    0.0 |
+-------+--------+--------+-------+---------+--------+--------+
</code>```

In order to asses how many degree of freedom do you need, you have to see how many coordinates in the table are constant. e.g. the Y column is constant and the C column is constant so you might be able to use a four degrees of freedom robot. However, systems with limited amount of degrees of freedom might have coupled motions, meaning that one or more of the coordinates which are not controlled are not constant, but change in a way which can be calculated, but not controlled. These simply result form the specifics of the mechanism used.

In the example form the table, two transnational axes and two rotational axes and two rotational axes can be used, but four rotational axes would not be well suited.

As for dimensioning the linkages maximum distances between the points can be considered. The angular difference between the orientation can be used to dimension the angular range of the orientation axes. Dimensioning the linkages is heavily dependent on the chosen axis types and configuration, there is no generic answer if the axis configuration is not known.

Body:
If you should consider not just points, but poses (position and orientations). If you can write these as a table, where the columns are the coordinates

+-------+--------+--------+-------+---------+--------+--------+
| Point | X [mm] | Y [mm] | Z [mm]| A [deg] | B[deg] | C[deg] |
+-------+--------+--------+-------+---------+--------+--------+
| P1    |  100.0 |  200.0 | 400.0 |    90.0 |   20.0 |    0.0 |
| P2    | 1500.0 |  200.0 | 800.0 |    90.0 |   45.0 |    0.0 |
| P3    |   42.0 |  200.0 | 400.0 |     0.0 |   90.0 |    0.0 |
+-------+--------+--------+-------+---------+--------+--------+

In order to asses how many degree of freedom do you need, you have to see how many coordinates in the table are constant. e.g. the Y column is constant and the C column is constant so you might be able to use a four degrees of freedom robot. However, systems with limited amount of degrees of freedom might have coupled motions, meaning that one or more of the coordinates which are not controlled are not constant, but change in a way which can be calculated, but not controlled. These simply result form the specifics of the mechanism used.

In the example form the table, two transnational axes and two rotational axes and two rotational axes can be used, but four rotational axes would not be well suited.

As for dimensioning the linkages maximum distances between the points can be considered. The angular difference between the orientation can be used to dimension the angular range of the orientation axes. Dimensioning the linkages is heavily dependent on the chosen axis types and configuration, there is no generic answer if the axis configuration is not known.


问:
Why aren't my 4x 612 RPM motors driving with only 12 lb load?
I have purchased x4 612 RPM actobotics motors. Every time I try to drive the motors on ground, the motors stall, as if the torque was too low to drive.

The motor model: servocity.com model #: 638284 Planetary Gear Motor 612 RPM @ 12v (1/14 Ratio)

Here is a picture of how i assembled the motors to the chassis:

See: Original Agent 390 chassis motor assembly

I have looked at the motor torque tables on this specific model. I have calculated that if my robot weighs 15lbs, that with x4 612 RPM motors, they should be able to run at about 500 RPM:

I weighed my robot and it only weighs 12 lbs, but the motors are too weak to drive!

What I want to know, is: what should the RPM of four 612 RPM actobotics motors with 12 lbs on them collectively, being powered by an 11.5v, 3000mA battery pack be?

And... should the motors be able to hold a 12 lb load?

答: Without the exact circuit its difficult to be sure. If connections are properly done, most probably the power source(battery pack in this case) is unable to provide sufficient current to power the motors. A good start would be to connect just 1 motor and check if the battery pack is able to run your motor. You can also try a different power source to confirm.</p>


问:
State propagation from uncertain control input
Consider a nonlinear system $x(k+1)=f(x(k),u(k))$, where $x(k)\in\mathbb{R}^{n}$ is the state, $u(k)\in\mathbb{R}^m$ is the control input. Here $u(k)$ is normally distributed RV with mean $\mu_u(k)$ and variance $\Sigma_u(k)$. I want to find the distribution of $x(k)$ starting from some deterministic $x(0)$ for some horizon $K$.
Then can the ordinary EKF update equation be used for the prediction in the below way?

\begin{align*}
\mu_x(k+1)&=f(\mu_x(k),\mu_u(k)),\\
\Sigma_x(k+1)&=\nabla_x f(\mu_x(k),\mu_u(k))\Sigma_x(k)\nabla_x f(\mu_x(k),\mu_u(k))^\top+\nabla_u f(\mu_x(k),\mu_u(k))\Sigma_u(k)\nabla_u f(\mu_x(k),\mu_u(k))^\top
\end{align*}

答: Yes. This approach is commonly used when IMUs are present because they measure the rate of state change with some uncertainty.</p>


答:
The no load current draw, per motor, is 0.52 amps. You have 4 motors, so you're looking at over 2 amps for no load.

The stall current is TWENTY amps, per motor.

You also appear to not be using an actual battery, but rather some portable power device that has an internal battery. The electronics in it could easily be performing a short circuit detection and killing output to safeguard the battery.

You should check current draw with your multimeter:

Make sure the multimeter probes are plugged into the current ("A") connectors, then

Make sure the multimeter is set to current, then

Attach the positive lead from your battery device to the positive port/probe on the multimeter, then

Attach the negative lead from the multimeter to the positive connector on your robot.

You measure current with a multimeter by putting it in series with the load. Putting it between the battery and load will let you measure the total current output of the battery.

I would guess you'll see one of two things:

Some small power draw for your electronics, then a rise in current when you try to run the motors, but to an amount not sufficient to run them, like 1 or 1.5 amps, for example, or

Some small power draw for your electronics, then suddenly no current at all when you try to run the motors, which would be a short circuit protection disconnecting the power output.

You'll either need more battery packs or you'll need to ditch the portable electronics and use the batteries directly (or both!)

Lastly, your four motors have the potential to draw a combined EIGHTY AMPS of current. I would strongly suggest you check the current limits of the wire gauge you're using, pick a value LESS than that, and add a fuse so you don't start a fire.

Title: Why aren't my 4x 612 RPM motors driving with only 12 lb load?

Body:
I have purchased x4 612 RPM actobotics motors. Every time I try to drive the motors on ground, the motors stall, as if the torque was too low to drive.

The motor model: servocity.com model #: 638284 Planetary Gear Motor 612 RPM @ 12v (1/14 Ratio)

Here is a picture of how i assembled the motors to the chassis:

See: Original Agent 390 chassis motor assembly

I have looked at the motor torque tables on this specific model. I have calculated that if my robot weighs 15lbs, that with x4 612 RPM motors, they should be able to run at about 500 RPM:

I weighed my robot and it only weighs 12 lbs, but the motors are too weak to drive!

What I want to know, is: what should the RPM of four 612 RPM actobotics motors with 12 lbs on them collectively, being powered by an 11.5v, 3000mA battery pack be?

And... should the motors be able to hold a 12 lb load?


问:
Inverse kinematics for 4-DOF Articualted manipluator
I am trying to find out the math formulas for Inverse Kinematics for a 4-DOF articulated manipulator.

By Googling I found information about inverse kinematics for 3-DOF, 4-DOF and 6-DOF articulated manipulator, but very few information for 4-DOF robot arm.

But what I found is a paper explaining how to calculate FK and IK for a 4-DOF robot arm:

Kinematics Modeling of a 4-DOF Robotic Arm

I tried to implement the math formula in C code but the calculation doesn't seem to work.

I believe that the formulas are wrong.

The math formula algebraically is the as follow:

dx, dy and dz are the global en effector coordinates.

Angle phi is the end effector orientation.

The C-code looks like this:

```<code>#include <stdio.h>
#include <math.h>

#define M_PI   3.14159265358979323846264338327950288

#define deg2Rad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define rad2Deg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)

int main()
{

    float l1, l2, l3, l4;   // Links mm
    float dx, dy, dz;       // EE position
    float phi;              // EE orientation
    float theta1, theta2, theta3, theta4;   // Joint variables that will be calculated
    float A, B, C;

    l1 = 170.0;
    l2 = 45.0;
    l3 = 85.0;
    l4 = 130.0;
    phi = 0.4;

    dx = 30.0;
    dy = 15.0;
    dz = 20.0;

    theta1 = atan(dy/dx);

    A = (dx - l4 * cos(theta1) * cos(phi));
    B = (dy - l4 * sin(theta1) * cos(phi));
    C = (dz - l1 - l4 * sin(phi));

   theta3 = acos(((A*A+B*B+C*C)-(l2*l2)-(l3*l3))/(2*l2*l3));

   printf("theta1: %f\n", theta1);
   printf("A: %f\n", A);
   printf("B: %f\n", B);
   printf("C: %f\n", C);

   printf("theta3: %f\n", theta3);

   return 0;
}
</code>```

When I calculate theta3 I get nand because what is inside acos is greater than 1 and acos can't have a value greater than 1. The conclusion is that something is wrong with the formulas.

Any suggestion?

答:
If your image is correct, it appears that you have a "planar 3 link arm" mounted on a turn-table.  If so, I would suggest that you decompose the problem into yaw (joint 1), and X/Y in the plane (joints 2-4).  It will probably be easiest to use polar coordinates.

You should be able to find many solutions online for a planar 3 link arm.


问:
How to find inertia parameter of payload automatically
I know of several robot manufacturers who can perform automatic inertial measurement for 6 axis robots. Fanuc, for example, moves with axes 5 and 6 each from +180 to -180 °, once with and once without load.
My question is how I can calculate the inertia parameter from e.g. the torques and the positions (thus also from speeds and accelerations) for each individual axis.
There are some papers about this, but no practical examples (e.g. https://hal.archives-ouvertes.fr/hal-00362677/document).
Can anyone help me with this?

My goal would be to run a trajectory once with and once without load and calculate mass and inertia of load, I found some topics here but none of them directly regarding to my question (in my opinion).

Thanks for your effort.

答:
The parallel axis theorem gives:

$$
I = I_{\textrm{cm}} + md^2 \\
$$

where $I_{\textrm{cm}}$ is the moment of inertia about the object's center of mass, $m$ is the mass of the object, and $d$ is the distance from the center of mass of the object to the axis of rotation.

You can also look at load acceleration and torque, where:

$$
\tau = I\alpha \\
$$

Or, torque is moment of inertia times angular acceleration. Then, you could estimate

$$
I = \frac{\tau}{\alpha} \\
$$

Apply a torque, measure the angular acceleration, and divide the two to get load inertia. You could assume the center of mass is located at the tip of the end effector, which isn't true, but as long as the load is fixed it doesn't really matter because the effective moment of inertia is what the arm's actually controlling. Consider it like:

$$
I_{\textrm{apparent}} = I_{\textrm{effective}} + md^2\\
$$

where $d$ is the distance from the joint to the tip of the end effector, and $I_{\textrm{effective}}$ is

$$
I_{\textrm{effective}} = I_{\textrm{cm}} + md_{\textrm{offset}}^2 \\
$$

where $d_{\textrm{offset}}$ is the difference between the object's center of mass and the object's connection to the end effector. Again, the arm actually moves the effective moment of inertia of the load.

You could also determine the mass by the holding torque required to keep the arm stationary, where

$$
\tau = mgd\sin{\theta}\\
$$


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

答:
generally, I think posting ROS related questions would be better on ROS answers

Regarding your question - did you source /opt/ros/melodic/setup.bash ?

You can check the package path by echo $ROS_PACKAGE_PATH , which is the path where rospack will search

Title: rospack can't find installed package


问:
Is it possible to use a LiPo charger as a lab bench power supply?
I recently thought about building a lab bench power supply, it comes in cheaper and I love to build things...

But then I also have a LiPo charger an iMax B6AC, that I had bought for my quadcopter, then came the idea of whether I can use the charger as a lab bench power supply...

My questions is, could this work and how could I make it work?

答: I strongly recomend you to use the iCharger X6, it has the "Power Output" function, this function allow you to adjust the Output Voltage from 2.0V to 26.5V in 0.1V increments, you can limit the output current from 1.0A to 30.0A ("""YES 30A""") in 0.1A incrmentes. Honestly I found this functionality lovely. Most Bench power supplies are very limited in output current typically 2 to 5 Amps. Just be carefull this function is only available on X6 and X8 modeles only, the other iCharger models don´t include this functionalitiy</p>

ContentLicense: CC BY-SA 3.0


问:
How is the depth estimation done in DTAM / LSD-SLAM (direct VSLAM)?
I know that DTAM and LSD-SLAM both employ the photometric error between two keyframes to estimate the image depth. But I don't know what the photometric error describes precisely.
In the DTAM paper, I found the following figure describing the principle:

However, if I only have images I_r and I_m and not the pose translation, how is the cost volume estimated? And how do I know in which row in I_m I have to look for my original Pixel I_r(u,v)?

答:
I can only describe the general idea. I can not tell you exactly how they solve it mathematically.

if I only have images I_r and I_m and not the pose translation, how is the cost volume estimated?

You know that the cost volume lies in a space between depth e_max, e_min, and width u and height v.
Now you take every small "cube" in this cuboid of size (e_max-e_min) x u x v and project it into your two images l_m and l_r. Then you have one pixel in image A (pixelA) and one pixel in image B (pixelB).

Then you calculate the photometric error. I am not sure how they do it here. You could go and sum the red, green and blue values of pixelA and sum the red,green and blue values of pixelB. Then you subtract these values.
If the difference is small then the color or intensity might be similar. This might tell you that the cube is at the correct place.

And then you do this for every single of these little cubes. And calculate the average.

Now you say "what? but how do I know how to project the little cube to the second image?". The first image is easy. That is just 3d-backpropagation. But for the second image, you need the pose transformation. And here a minimization procedure comes into place.

Your transformation can be described by 6+ variables (depending on whether you use Euler angles, rotation matrices etc.). You want to estimate these variables. If there was a perfect solution then you could do this using gauss jordan elimination.

You have some initial guess of your transformation.
This is of course not a good guess. Therefore the photometric error described above is really high. If you guess multiple times then you might see that the error increases or decreases.

This procedure is described in the paper in detail. It does the same as gauss jordan but for many variables. There is no perfect solution so gauss jordan can not work here and it would be far too slow.

Maybe this helps.


问:
Arduino turning off when using bipolar stepper motor (L298N Driver)
Sorry, this might be a bit lengthy, but I really need some help (I am aware that there is a question similar to mine that has already been asked here. However, I couldn't come up with a solution to my problem based on the answers to that question as I am using a separate power source as stated in that question's answers.).

This is my first project with an Arduino and a stepper motor. I did some research and found that an L298N motor driver can both drive the motor and power the Arduino if using an external 12V power supply, but it could also handle up to 35V to the motors if the jumper cable was removed and the Arduino externally powered. I wired the stepper motor to the L298N according to the diagram on this website. I removed the jumper on the L298N and hooked up the circuit with a cheap 24V 2A power brick and a 24V to 5V 3A Tobsun EA15-5V DC to DC converter (I wired the 24V positive to the Tobsun and the L298N +12V positive, and I wired the 5V output of the Tobsun to the +5V of the L298N and to the 5V of the Arduino. The ground of the Arduino and the L298N were both wired to the negative of the 24V).

I uploaded the example StepperOneRevolution example to the Arduino, and it seemed to work fine with the motor spinning as expected when the Arduino was plugged into my computer (the 24V power brick was plugged in as well). As soon as I removed the link to my computer, the Arduino and L298N seemed to turn off and on about twice a second (ALL the lights of the Arduino and L298N would flash on and off). The stepper motor would move just a fraction of a rotation when the lights of the Arduino and L298N were on and would stop when they were off. When I unplugged the wires to the motor, the Arduino and L298N would stop flashing and stay on.

After using a multimeter to measure various voltages across the circuit, I thought that maybe the cheap power brick and/or the Tobsun or my wiring were to blame, so I found a 12V 1A power brick around the home that I know works. I replaced the jumper on the L298N and re-wired the circuit to look exactly like the circuit from the same website as before (12V directly to L298N +12V,L298N +5V to the 5V of the Arduino, GND of both to the negative of the 12V brick).
This time, once I plugged in the 12V brick, the motor spun for about a second (twice as long as before), but then the Arduino and L298N turned off and the motor cut out again. This is consistent with it resetting (as stated in the answers to the similar question I mentioned at the beginning).

I have attempted to look for a solution to this, rewire the circuit again, use a different L298N controller, but all to no avail; however, one interesting thing I noticed was that if I get rid of the delay(500), then the motor spins continuously one revolution back and forth (as the StepperOneRevolution example dictates, minus the delay) and Arduino doesn't turn off. I'm pretty sure it's not the delay function that is causing the Arduino and L298N to turn off and on because the time between the motor turning off and on remains consistent no matter the amount I put in the delay. If I leave the delay, the motor also doesn't spin in the opposite direction like it is supposed to after the first delay.

I know that was quite long, but I wanted to get all of the backstory there in detail so that there was no confusion as to what exactly the problem might be. Thank you very much f you read through or even skimmed those paragraphs, and I would truly appreciate if anyone could answer this very specific question! (Please let me know if I missed a detail; I tried not to, but I might have :) )

答:
You replaced a 2A power supply with a 1A power supply, and then you're having what sounds like a brownout condition when you try to run a motor that is rated to consumer 2A. One of the reviews on the Amazon link you posted says,

In order to drive this motor you either need a current limited power supply <2A or you need to drive it at about 2V. If you supply it with 12V as stated in the spec sheet, it will draw about 11A, which is plenty to smoke-check both the motor and your H-Bridge. If you're going to use this with the L298N, you must use PWM, or wire 4-8ohm@5W resistors. If you don't know what any of this means, don't use it with the L298N modules you will end up smoke checking the motor or the bridge.

The part itself says in the specifications:

As a stepper motor, it should connect to a constant current or chopper driver. Connecting the motor directly to a power supply will destroy the motor.

What is your power supply voltage when you run it? I would guess you're basically shorting it and you'd see very low voltage if you measured it.


问:
Bearing landmark localization
I am trying to initialize the position of a set of landmarks using only bearing observations. Each robot pose, described by position (x,y) and orientation (theta), observes a small set of landmarks. Each observation, bearing angle, is expressed in the robot frame.
My idea was to consider pair of consecutive robot poses, retrieve the equation of the lines passing though them and make the intersection to obtain the position of the landmark observed by both poses. Using the robot position and the bearing observation, I can retrieve the equation of a line as  y-y1=m(x-x1) where the slope m can be computed as m=tan(bearing).

To check if this is the right way to proceed I tested the method on the data from the ground truth dataset. For instance, two consecutives poses  R1=(1.0384, 0.037015, 0.0057897) and R2=(2.0171, 0.019962, 0.0017791) both observe the landmark L1 in position (2.8116, -4.2695), respectively with bearing values of B1=-1.1860 and B2=-1.3894. Evaluating the lines passing through R1-L1 and R2-L1 their equation is y1=-2.4287x + 2.5589 and y2=-5.3989x+10.9102. If now I try to retrieve the slope of the two lines using the method mentioned before I obtain different values m1=-2.4692 and m2=-5.4522.

Some final notes:

I'm almost completely sure that the bearing observation provided is the actual "mathematical" angle measurement, hence no transformation of it is required.

I think that I should use also the theta angle of the robot pose in some way, but I'm not sure how.

What am I missing?

答:
So the method to figure of something with only bearing measurements is called triangulation. While I think your method works you can much more easily solve it using sin rule.

Given at least 1 side and 2 angles you can figure out all the properties of a triangle using the sin rule.

$$\frac{a}{sin(a)}=\frac{c}{sin(C)}=\frac{b}{sin(B)}$$

In your problem you have a length(distance between poses), and 2 angles(bearing measurements). So you have all the information you need to figure out the position of the landmark.

Picture is from https://www.cimt.org.uk/projects/mepres/step-up/sect4/index.htm

I think that I should use also the theta angle of the robot pose in
some way, but I'm not sure how.

Correct. It is pretty simple. You use it to convert the local bearing measurement into the global frame.

$$\theta_{bG}=\theta_R+\theta_{bmeas}$$

So the global angle bearing measurement($\theta_{bG}$) is the heading($\theta_R$) of your robot plus your bearing measurement($\theta_{bmeas}$).

The global bearing measurement is what you plug into the sin rule equations.

Title: Bearing landmark localization


问:
Cartesian Velocity Control between Two 3D Poses
I am really struggling to understand how to do Cartesian velocity control when 3D rotations are involved. So, below is a very simple example which I am hoping somebody can help me with.

Let's say the base frame of my robot is labelled $B$, the end-effector frame is labelled $E$, and the goal frame is labelled $G$. So, the Cartesian pose of the end-effector relative to the base can be expressed as a matrix $T_{BE}$, and similarly, $T_{BG}$ for the goal. The Jacobian, which describes the rate of change of the end-effector about the base frame, is $J$.

I want to move the end-effector from $E$ to $G$, in time $T$. To do this, I can create a loop which continually calculates the required Cartesian velocity of the end-effector, about the base frame. This is a vector length 6, containing translational velocity about the base's x-y-z axes, and rotational velocity about the base's x-y-z axes. We call this velocity vector $v$. Then, I will move the end-effector at this velocity, using the inverse Jacobian to calculate the required velocities of the joints, $q$. This equation is $q = J^{-1} v$.

However, what I don't understand, is how to calculate $v$, which is the required Cartesian velocity of the end-effector about the base frame. I know how to do this with 1D rotation: I would just take the difference between the current angle and the goal angle, and divide by $T$. I also know how to do this in 3D if the motion only involves translation, and no rotation: again, I can find the difference in the current and goal position. But when 3D rotations are involved, I don't know how to do this.

I have tried converting matrices $T_{BE}$ and $T_{BG}$ into Euler representations (with X-Y-Z rotations about $B$), and then finding the difference in the three components of the Euler vectors, between $T_{BE}$ and $T_{BG}$. My intuition was that I could then divide these three components by $T$, and this would give me the rotational velocity components of vector $v$. However, I have implemented this, and I do not get the desired behaviour. I think that this is because the three rotational components are dependent on each other, and so I cannot simply treat each one independently like this.

So, can anybody help me please? How can I create the rotational components of vector $v$, given the current pose $T_{BE}$ and the target pose $T_{BG}$?

The below image summarises the problem:

答: You could run the solution with some guess of $v$ then command a velocity normalized by how long your guess velocity took.


问:
Quadcopter motors vibration problem
I built an Arduino Due based quadcopter, with MPU-6050 for auto levelling and BMP-085 for Altitude.

I am using Kalman filter code from here for the MPU-6050 to take the Roll and Pitch angels from the sensor.

The Roll and Pitch angels are correctly read while the quadcopter motors are off, but when the motor speed increased the angels start to be more inaccurate.

As shown in the following photos (where I took different speed motors to show the effects of the motor vibration on the MPU readings). These tests were taken while the quadcopter on the ground.

Motor speeds vary from 0 to 180 so I choose (50,100, and 150).

My sensors are fixed on a breadboard over a plastic surface with anti-vibrator rubber balls as shown in my quadcopter pictures.

My problem is that the controller I designed is very sensitive to these vibrations, is there any way to reduce these vibrations as minimum as possible.

Thank you

答: Firstly inspect propellers for damage, followed by motor assembly, and finally to reduce vibration on electronics use double sided tape. The other problem I see in the picture is the breadboard. By soldering your components onto a PCB you'll reduce vibration slightly.  I had a similar problem with my rocket, so I soldered my wires and sensors to a PCB.</p>

Title: Quadcopter motors vibration problem


答:
I found what I was looking for:

https://www.robotshop.com/media/files/PDF/robotshop-multi-purpose-robotic-arm-guide.pdf

Title: Inverse kinematics for 4-DOF Articualted manipluator

Body:
I am trying to find out the math formulas for Inverse Kinematics for a 4-DOF articulated manipulator.

By Googling I found information about inverse kinematics for 3-DOF, 4-DOF and 6-DOF articulated manipulator, but very few information for 4-DOF robot arm.

But what I found is a paper explaining how to calculate FK and IK for a 4-DOF robot arm:

Kinematics Modeling of a 4-DOF Robotic Arm

I tried to implement the math formula in C code but the calculation doesn't seem to work.

I believe that the formulas are wrong.

The math formula algebraically is the as follow:

dx, dy and dz are the global en effector coordinates.

Angle phi is the end effector orientation.

The C-code looks like this:

```<code>#include <stdio.h>
#include <math.h>

#define M_PI   3.14159265358979323846264338327950288

#define deg2Rad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define rad2Deg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)

int main()
{

    float l1, l2, l3, l4;   // Links mm
    float dx, dy, dz;       // EE position
    float phi;              // EE orientation
    float theta1, theta2, theta3, theta4;   // Joint variables that will be calculated
    float A, B, C;

    l1 = 170.0;
    l2 = 45.0;
    l3 = 85.0;
    l4 = 130.0;
    phi = 0.4;

    dx = 30.0;
    dy = 15.0;
    dz = 20.0;

    theta1 = atan(dy/dx);

    A = (dx - l4 * cos(theta1) * cos(phi));
    B = (dy - l4 * sin(theta1) * cos(phi));
    C = (dz - l1 - l4 * sin(phi));

   theta3 = acos(((A*A+B*B+C*C)-(l2*l2)-(l3*l3))/(2*l2*l3));

   printf("theta1: %f\n", theta1);
   printf("A: %f\n", A);
   printf("B: %f\n", B);
   printf("C: %f\n", C);

   printf("theta3: %f\n", theta3);

   return 0;
}
</code>```

When I calculate theta3 I get nand because what is inside acos is greater than 1 and acos can't have a value greater than 1. The conclusion is that something is wrong with the formulas.

Any suggestion?


问:
Power to Raspberry Pi and DC motors from a single USB power bank
I'm running a simple robot involving a Raspberry Pi 3 and 2 JGA25-370 motors.

The system is powered from a single Powerbank (EasyAcc 10000MAh) which has 2 2A ports.
DIY USB cable has data pins shortened so the powerbank would give it more power.

When I'm running my robot on smaller speeds everything works fine, but as soon as I increase the speed Raspberry PI restarts when I press "GO" :)

My suspicion is that current draw from the motors when they start causes a power drop.

I can see the red LED going dark on RPi even on lower speeds when I start the movement, but it doesn't really cause the restart, probably because it's so brief.

Could this be solved by putting a capacitor in front of motor controller (TB6612FNG)?
If so, what the value of it should be?

Any other ideas? I really don't want to have 2 separate batteries, especially since it's only a problem when the robot starts from a still position.

Cheers!

答:
The initial motor current required to pull away from standstill is likely the cause of the pi being reset, you could try to accelerate slowly rather than hitting the gas too quickly.

A capacitor is used to stop the voltage spikes caused by the motor
starting and stopping and reduce RF interference. The spikes could also be a cause of the pi reset. I use a 100uF capacitor across the power lines to minimise spikes and noise.

Depending on the motors size I don't think your power-bank will be suitable to provide enough current for the pi and the motors together, check the stall current on the motors and give this overhead at least, if you have usb devices plugged into the pi, desktop ui, wifi and blue-tooth enabled the pi could be using nearly 1 amp.

I use a bigger battery and have 2 dc-dc buck converters to bring the voltage down - one for the pi and one for the motors, separately fused. the battery lasts longer and is under less stress, a decent rc car battery can be inexpensive compared to a usb power bank.


问:
Wifi to pass through aluminium
I am about to make an rc car which uses a wifi connection. The body for the car would be made from aluminium and the wifi receiver will be placed inside this aluminium casing.

How do I make sure that this will work?

Would I be forced to change my material or can I just make an extension for the receiver and make sure it is out of the casing?

If so , would that really help me?

答:
You can use an external onmidirectional wifi antenna, like these on ebay.

If your wifi chip does not have a U-FL connector, you can use a SMA to U-FL adapter or directly a SMA Antenna.

Title: Wifi to pass through aluminium


问:
Rapid code pick and place
Considering the scenario represented in the following image, and the function open_gripper(), close_gripper() and Offs(), present the necessary instructions for the robot to perform the path 1 - 2 - 3, represented by the red line, to transport the block on the table. Consider that the only known point is the point P1. The length of each route is shown in the image. The directions of the robot's movements must be in accordance with the direction of the arrows.

This is my version of code, but i am not sure that this is right. What i am doing wrong? How the code should be?

答:
There is a discrepancy in notation. P1 is the current point at the TCP on the image but 1 denoted the position of the box. By this logic you would need a P4 corresponding to the point on the table marked as 3.

P1 does not seem to be defined in the code and P2, defined relative to P1 should have -700 as z coordinate and P3 should have -200 as x coordinated, relative to P1. I assume the axes of P1 are not rotated relative to the global CS.

You seem to be closing the gripper before arriving to P2. If P2 is a pickup point you should close the gripper after moving to P2.

Title: Rapid code pick and place


答: You essentially want to find the time derivative of a linear interpolation between two rotations. The easiest way to obtain this would probably to convert the rotation matrix between the two orientations to a axis-angle representation and the angular velocity would simply be the axis times the angle divided by $T$.

Title: Cartesian Velocity Control between Two 3D Poses

Body:
I am really struggling to understand how to do Cartesian velocity control when 3D rotations are involved. So, below is a very simple example which I am hoping somebody can help me with.

Let's say the base frame of my robot is labelled $B$, the end-effector frame is labelled $E$, and the goal frame is labelled $G$. So, the Cartesian pose of the end-effector relative to the base can be expressed as a matrix $T_{BE}$, and similarly, $T_{BG}$ for the goal. The Jacobian, which describes the rate of change of the end-effector about the base frame, is $J$.

I want to move the end-effector from $E$ to $G$, in time $T$. To do this, I can create a loop which continually calculates the required Cartesian velocity of the end-effector, about the base frame. This is a vector length 6, containing translational velocity about the base's x-y-z axes, and rotational velocity about the base's x-y-z axes. We call this velocity vector $v$. Then, I will move the end-effector at this velocity, using the inverse Jacobian to calculate the required velocities of the joints, $q$. This equation is $q = J^{-1} v$.

However, what I don't understand, is how to calculate $v$, which is the required Cartesian velocity of the end-effector about the base frame. I know how to do this with 1D rotation: I would just take the difference between the current angle and the goal angle, and divide by $T$. I also know how to do this in 3D if the motion only involves translation, and no rotation: again, I can find the difference in the current and goal position. But when 3D rotations are involved, I don't know how to do this.

I have tried converting matrices $T_{BE}$ and $T_{BG}$ into Euler representations (with X-Y-Z rotations about $B$), and then finding the difference in the three components of the Euler vectors, between $T_{BE}$ and $T_{BG}$. My intuition was that I could then divide these three components by $T$, and this would give me the rotational velocity components of vector $v$. However, I have implemented this, and I do not get the desired behaviour. I think that this is because the three rotational components are dependent on each other, and so I cannot simply treat each one independently like this.

So, can anybody help me please? How can I create the rotational components of vector $v$, given the current pose $T_{BE}$ and the target pose $T_{BG}$?

The below image summarises the problem:


问:
EKF SLAM : Missing older landmarks in new observation
I am a beginner to SLAM and robotics in general and I have been trying to implement SLAM on my GoPiGo3 robot car kit using primarily Chapter 10 from Probabilistic Robotics by Sebastian Thrun as reference.

I am using a laser distance sensor attached to a servo that turns only 180°. The sensor itself has a range of 2m. I am able to get the odometry data from the wheel encoders.

I am planning to use the RANSAC algorithm for line extraction and landmark detection.

Suppose I detect a landmark and add the global coordinates of the landmark into the state vector. Then, I move some distance and the newly added landmark into the state vector is no longer observable.

Is this a problem that is accounted for in the EKF SLAM algorithm from the book? Specifically I wish to know how missing landmarks in the new observation affect the transformation function that maps the predicted state to the predicted observation and thus the Jacobian. How will this affect the computation of the Kalman Gain and the measurement update of the state?

If it is not feasible to use this algorithm in such a situation, I would appreciate it if someone could point me to the right resources for my implementation.

Thank you!

答:
Is this a problem that is accounted for in the EKF SLAM algorithm from the book?

It is. If you look at the pseudo code implementation you posted you will notice that the update only works for the observed features.

For all your matrices,jacobian,$H$ matrix, Kalman gain. The parts related to the unobserved landmark is 0. Therefore the update will be 0, and the landmark will not be changed. If you want to see an implementation of this in code then I recommend looking at this EKF implementation. I specifically linked to the part where the matrices are formed with a bunch of zeros in the necessary places.


问:
Handling Inverse Kinematics error on a UR Robot
We're working on a bigger system that'll use two UR robots as well (although we only have one at the moment for development). We're communicating with the robot through its TCP/IP based RTDE protocol to send parameters like command and desired position. This works perfectly. We're using UR Script's get_inverse_kin() function to convert position (TCP) into joint angles near known ones (e.g. along a prerecorded trajectory or the previous joint values). This also works most of the time.

Sometimes however, we get an error, as the desired position is too far for the robot arm to reach, or the orientation is wrong or the robot doesn't like something else. This wouldn't be a problem, but now a get_inverse_kin() call raises a popup, pauses the robot program, and everything stops.

What is the most civilized way to handle an error like that? We can close the popup and start the robot program again through the dashboard protocol, but we need to recognize it, and it looks like a hack. Also, get_inverse_kin doesn't send the robot to that wrong place, just calculates new joint angles, so stopping everything is wrong in the first place (we could use a try-catch block before the movej if the robot program had one).

Any suggestions?

答: URScript offers a function to check if a pose is within the currently set safety limits. If there are no safety limits set, it should take into account the default joint limits (i.e. it checks reachability of a pose). The function is is_within_safety_limits(pose). It can be used in an if-condition to check reachability, before calling the IK function. This way no try-catch is needed.


问:
How to use Arduino for ESC control?
I am using an Arduino Uno to control an ESC for my (in progress) quadrocopter.  I am currently using the servo library to control the ESC, which works great.

Except..

A count of 100 is max speed, meaning I only have 10 speeds between 90 (stopped) and 100 (motor at full power) to correctly run my quadrocopter, I would like to have many more speed options.  Any ideas?  I'm having a hard time using a PWM signal, I might not be doing it right though.

My current code is here:

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

Servo myservo; // create servo object to control a servo
                // a maximum of eight servo objects can be created

int pos = 0; // variable to store the servo position

void setup()
{
  myservo.attach(8); // attaches the servo on pin 8 to the servo object
}

void loop()
{

 int maxspeed=100;
 int minspeed=0;
 int delaytime=5;
 int count;

 for(count=0; count <1; count+=1) {
  for(pos = minspeed; pos < maxspeed; pos += 1) // goes from 0 degrees to 180 degrees
  { // in steps of 1 degree
    myservo.write(pos); // tell servo to go to position in variable 'pos'
    delay(delaytime); // waits 15ms for the servo to reach the position
  }
  for(pos = maxspeed; pos>=minspeed; pos-=1) // goes from 180 degrees to 0 degrees
  {
    myservo.write(pos); // tell servo to go to position in variable 'pos'
    delay(delaytime); // waits 15ms for the servo to reach the position
  }
  if(count>1){
    break;
  }
 }

 myservo.write(92);
 delay(100000);
 myservo.write(90);
 delay(10000000);
}
</code>```

答: Your code uses the typical servo.attach(pin) where you can use the overload of servo.attach(pin, min, max) to set the min and max microseconds of the pulse width to match the desired ranges for you ESC. Additionally to make it a bit more clear where myservo.write(90); is used to set the angle, you can use myservo.writeMicroseconds(1500); to set the duration of the pulse directly in microseconds.

Title: How to use Arduino for ESC control?


问:
How can I get the equations of motion used in webots?
I am trying to use the webots platform for developing controllers. I need the differential equations governing the motion of the robots. How can I get the equations of motion used in webots?</p>

答:
EDIT: the below mention function does not export equations directly, as you requested, but exports the dynamic system in the DIF - Dynamics Interchange Format

Webots uses the Open Dynamics Engine (ODE) to simulate physics. ODE includes an export function for exporting the ODE called dWorldExportDIF().

Webots seems to support this export function and it also includes a test, which uses the function mentioned above to export the equations. The test, which exports the equations can be found here:
https://github.com/cyberbotics/webots/blob/master/tests/physics/plugins/physics/ode_dif_exporter/ode_dif_exporter.c


问:
Motion model and homogeneous transformation issue on a drone
I am trying to develop a EKF-SLAM algorithm for a drone, and I want to estimate its position in the inertial/world reference frame. I have as control variables the linear and angular velocities in the drone-reference frame. To do so, I am following this paper: http://ingmec.ual.es/~jlblanco/papers/RangeBearingSLAM6D.pdf in which it states that the position update is:

```<code>x_ = x_ + delta_t * (vx * T(0,0) + vy * T(0,1) + vz * T(0,2));
y_ = y_ + delta_t * (vx * T(1,0) + vy * T(1,1) + vz * T(1,2));
z_ = z_ + delta_t * (vx * T(2,0) + vy * T(2,1) + vz * T(2,2));
yaw_ = yaw_ + delta_t * omega_z;
</code>```

Where T is the following transformation matrix:
$$
\begin{bmatrix} cos(yaw) * cos(pitch) & cos(yaw) * sin(pitch) * sin(roll) - sin(yaw) * cos(roll) & cos(yaw) * sin(pitch) * cos(roll) + sin(yaw) * sin(roll) & 0 \\
sin(yaw) * cos(pitch) & sin(yaw) * sin(pitch) * sin(roll) + cos(yaw) * cos(roll) & sin(yaw) * sin(pitch) * cos(roll) - cos(yaw) * sin(roll) & 0 \\
-sin(pitch) & cos(pitch) * sin(roll) & cos(pitch) * cos(roll) & 0 \\
 0 & 0 & 0 & 1 \end{bmatrix}
$$

I have triple-check the transformation matrix and it seems to be fine, also the position updates make sense (or I think it does).

The problem comes when I plot the position to compare ground truth with my estimation, and I see the following:

As you can see between t=40 and 60, the ground truth (red) goes down, and my update (blue) goes to the opposite direction.. after this divergence it continues more or less fine, considering that it drifts (which is the expected). That moment in which one line goes up and the other down, is when the drone starts rotating 180 degrees (as you can see in the second plot - green line), that is why I think the problem is in the transformation matrix.

I've also tried to invert the yaw value or transposing T, but the problem gets bigger.

Something that worth mentioning is that this is pure prediction, so no correction is done at this moment.

Can you spot any error in my reasoning? Is there something that I didn't mention that may worth quadruple-check? Or do you think this is more or less expected and I am creating a problem from nothing?

Hope you can help me! Thanks in advance!

答:
Ok, just to answer this and close the question. I asked the lab people to test my node with prediction only, and it turned out that it is ok.. So my conclusion is that for some reason, the simulation in my computer is giving me wrong velocities (I don't know why exactly).

My solution to this, was to record a bag in the lab's computer, and use that to continue the development.


问:
BLDC Stuttering when attached to Arduino Nano
I am working on building a quadcopter with an Arduino nano. I am currently only working with 2 motors. However, I am experiencing an odd issue. When I try to run my motors, one of them runs fine, while the other stutters - it turns on and off extremely quickly to get this stuttering effect, all the while emitting a quick beeping sound. I know the motor and the ESC are both perfectly fine because when I isolate the problematic motor and run it without the Arduino using a Servo consistency master, it works perfectly normally. It also cannot be the signal pin pf the Arduino which the motor is attached to because I switched the signal pins of the two motors, but the issue still persists with the same motor. I'd really appreciate some help, thanks!</p>

答:
If you ran the setup with servo tester, connection between motor and ESC is fine. Therefore, check with oscilloscope if your Arduino generates proper signal. Typically, ESCs proper input is 50Hz PWM with 1000-2000us duty. Ensure, that you have these values correctly. Moreover, ensure that high and low voltage levels are correct - 5V/3.3V (depending on Arduino version) for the high state and 0V for the low state.

If you have your PWM generated by the software you created, the signal will not be consistent and may cause some problems. I'd rather use hardware timer set to generate PWM signal. Also, if you use 8-bit timer, resolution of the output PWM signal will not be good: 20ms period divided by 256 is 0.07ms. The signal you want to generate has a duty between 1 and 2ms, therefore this 1 millisecond used for control is divided in 13 possible positions. Use 16-bit timer instead, to get better results.

The problem with one particular motor can also be caused by tolerance between ESCs; ie. you generate with your Arduino signal, that has slightly low frequency (like, 49Hz) and one of the ESCs gets it and works correctly, the other however does not "understand" the signal you generate. That is why you should precisely check PWM's frequency, duty cycle and voltages. Also mind the timers I mentioned above.

Lastly, your question is more related to the Arduino, than robotics. There is an Arduino Stack Exchange with far superior number of visitors and Arduino programmers/designers.


问:
Differential Drive Tricycle WMR with active caster wheel
I'm doing some research for my university project and in particular it concerns a functional design project. I thought it would have been possible to design a 3-wheel mobile robot with an active caster wheel in the front and two passive standard wheels in the back using differental drive. But all papers i'm reading states exactly the opposite: differential drive robots have two active wheels in the back and a passive caster wheel in the front for balance. Does any of you know if it is possible to realize my idea?</p>

答:
A caster is non driven but I think I get what you're asking. It is possible. But with one driving wheel, you would have to be able to rotate the swivel joint of the wheel as well in order to make the robot turn. (Two motors - one driving the wheel and other rotating the wheel around the axis perpendicular to the floor)

This wheel configuration is called "tricycle drive". Google and you can find more material on this. Here's a random paper related


答:
The servo.write(angle) function is designed to accept angles from 0 to 180.
(The value 180 is significantly larger than 100).
Could you tell me where in the Servo documentation you read "100 (motor at full power)", so we can fix that typo?

Please change the line

```<code>int maxspeed=100; /* wrong */
</code>```

to

```<code>int maxspeed=180;
</code>```

Also, please run servo.refresh() periodically to keep the servos updated -- perhaps something like:

```<code>for(pos = maxspeed; pos>=minspeed; pos-=1) // goes from 180 degrees to 0 degrees
{
    myservo.write(pos); // set the servo position
    myservo.refresh(); // tell servo to go to the set position
    delay(delaytime); // waits 15ms for the servo to reach the position
};
// ...
myservo.write(92);
for( long int delaytime = 0; delaytime < 100000; delaytime+=10 ){
    myservo.refresh(); // tell servo to go to the set position
    delay(10);
};
</code>```

As mpflaga pointed out, you can get more precision by setting the pulse width directly in microseconds with

```<code>myservo.writeMicroseconds(1000); // typical servo minimum; some go down to 600 or less
myservo.writeMicroseconds(2000); // typical servo maximum; some go up to 2500 or more
</code>```

p.s.:
Have you seen "blink without delay" ?
That technique makes it much easier to read sensors and update servo positions with consistent heartbeat timing.

Title: How to use Arduino for ESC control?

Body:
I am using an Arduino Uno to control an ESC for my (in progress) quadrocopter.  I am currently using the servo library to control the ESC, which works great.

Except..

A count of 100 is max speed, meaning I only have 10 speeds between 90 (stopped) and 100 (motor at full power) to correctly run my quadrocopter, I would like to have many more speed options.  Any ideas?  I'm having a hard time using a PWM signal, I might not be doing it right though.

My current code is here:

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

Servo myservo; // create servo object to control a servo
                // a maximum of eight servo objects can be created

int pos = 0; // variable to store the servo position

void setup()
{
  myservo.attach(8); // attaches the servo on pin 8 to the servo object
}

void loop()
{

 int maxspeed=100;
 int minspeed=0;
 int delaytime=5;
 int count;

 for(count=0; count <1; count+=1) {
  for(pos = minspeed; pos < maxspeed; pos += 1) // goes from 0 degrees to 180 degrees
  { // in steps of 1 degree
    myservo.write(pos); // tell servo to go to position in variable 'pos'
    delay(delaytime); // waits 15ms for the servo to reach the position
  }
  for(pos = maxspeed; pos>=minspeed; pos-=1) // goes from 180 degrees to 0 degrees
  {
    myservo.write(pos); // tell servo to go to position in variable 'pos'
    delay(delaytime); // waits 15ms for the servo to reach the position
  }
  if(count>1){
    break;
  }
 }

 myservo.write(92);
 delay(100000);
 myservo.write(90);
 delay(10000000);
}
</code>```

Body:
The servo.write(angle) function is designed to accept angles from 0 to 180.
(The value 180 is significantly larger than 100).
Could you tell me where in the Servo documentation you read "100 (motor at full power)", so we can fix that typo?

Please change the line

int maxspeed=100; /* wrong */

to

int maxspeed=180;

Also, please run servo.refresh() periodically to keep the servos updated -- perhaps something like:

for(pos = maxspeed; pos>=minspeed; pos-=1) // goes from 180 degrees to 0 degrees
{
    myservo.write(pos); // set the servo position
    myservo.refresh(); // tell servo to go to the set position
    delay(delaytime); // waits 15ms for the servo to reach the position
};
// ...
myservo.write(92);
for( long int delaytime = 0; delaytime < 100000; delaytime+=10 ){
    myservo.refresh(); // tell servo to go to the set position
    delay(10);
};

As mpflaga pointed out, you can get more precision by setting the pulse width directly in microseconds with

myservo.writeMicroseconds(1000); // typical servo minimum; some go down to 600 or less
myservo.writeMicroseconds(2000); // typical servo maximum; some go up to 2500 or more

p.s.:
Have you seen "blink without delay" ?
That technique makes it much easier to read sensors and update servo positions with consistent heartbeat timing.