问:
EIF slam algorithm implementation as per Probabilistic Robotics
I want to implement EIF slam from Probabilistic Robotics Table 11.2. This is the algorithm for EIF slam.

To construct $\Omega and \xi$ matrix I write down a Java code. Here is my code

```<code>public class Part1Jama {
public static void main(String args[]) throws IOException {
    Matrix Bigomega = new Matrix(11116,11116);//last 15 row and column define landmarks x and y axis
    Matrix omega = new Matrix(11116,11116);
    Matrix BigXi = new Matrix(11116, 1);
    Matrix Xi = new Matrix(11116, 1);

    int i = 0, i1 = 0, k1 = 0, k2 = 0, l = 0, l1 = 0,k3=0,j=0;
    double[] timex = new double[11101];//11101 odometer data denoting by u_1;
    double[] xcoa = new double[11101];
    double[] ycoa = new double[11101];
    double[]thetacoa=new double[11101];

    double[] landx = new double[3095];//3095 measurment data z
    double[] landy = new double[3095];
    double[] timeya = new double[3095];
    double[] codea = new double[3095];
    double[] rangea = new double[3095];
    double[] bearinga = new double[3095];
    Scanner x = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/measurmentF.txt"));
    Scanner y = new Scanner(new File("/home/froboticscse/IdeaProjects/UJMPtest/src/main/java/uniquemeasure.txt"));

    while (x.hasNext()) {
//Data from odometry data file which consist of time,forward velocity angular velocity
        double time = x.nextDouble();
        double xco = x.nextDouble();
        double yco = x.nextDouble();
        double theta=x.nextDouble();
        timex[i] = time;
        xcoa[i] = xco;
        ycoa[i] = yco;
        thetacoa[i]=theta;
        i++;
    }

    while (y.hasNext()) {
//Data from measurement file consist of time,correspondence Number(landmark number),range,bearing
        double timey = y.nextDouble();
        double code = y.nextDouble();
        double range = y.nextDouble();
        double bearing = y.nextDouble();
        timeya[i1] = timey;
        codea[i1] = code;
        rangea[i1] = range;
        bearinga[i1] = bearing;

        i1++;

    }

    while(k1<timex.length-1) {
        if (timex[k1] < timeya[k2]) {

            omega.set( k1, k1,1);
            omega.set(k1, k1 + 1,-1);
            omega.set( k1 + 1, k1,-1);
            omega.set( k1 + 1, k1 + 1,1);

            Bigomega = Bigomega.plus(omega);
            omega.set( k1, k1,0);// clear value store in omega
            omega.set(k1, k1 + 1,0);
            omega.set( k1 + 1, k1,0);
            omega.set( k1 + 1, k1 + 1,0);

            Xi.set( k1, 0,-(ycoa[k1]));
            Xi.set(k1 + 1, 0,ycoa[k1]);
            BigXi = BigXi.plus(Xi);
            Xi.set( k1, 0,0); //clear value store in Xi
            Xi.set(k1 + 1, 0,0);

        }
// //For line number 10 to 20 as per algorithm
else if (timex[k1] == timeya[k2]) {
            landy[l1] = ycoa[k1] + rangea[k2] * (Math.cos(bearinga[k2] + thetacoa[k1]));
            omega.set( k1, k1,1);
            omega.set(k1, k1 + 1,-1);
            omega.set( k1 + 1, k1,-1);
            omega.set( k1 + 1, k1 + 1,1);
            Bigomega = Bigomega.plus(omega);
            omega.set( k1, k1,0);
            omega.set(k1, k1 + 1,0);
            omega.set( k1 + 1, k1,0);
            omega.set( k1 + 1, k1 + 1,0);

            Xi.set( k1, 0,-(ycoa[k1]));
            Xi.set(k1 + 1, 0,ycoa[k1]);
            BigXi = BigXi.plus(Xi);
            Xi.set( k1, 0,0);
            Xi.set(k1 + 1, 0,0);
            if (codea[k2] == 90.0) {

                omega.set(k1, k1,1);
                omega.set( 11101, k1,-1);
                omega.set(11101, 11101,1);
                omega.set(k1, 11101,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11101, k1,0);
                omega.set(11101, 11101,0);
                omega.set(k1, 11101,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 18.0) {
                omega.set(k1, k1,1);
                omega.set( 11102, k1,-1);
                omega.set(11102, 11102,1);
                omega.set(k1, 11102,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11102, k1,0);
                omega.set(11102, 11102,0);
                omega.set(k1, 11102,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 81.0) {

                omega.set(k1, k1,1);
                omega.set( 11103, k1,-1);
                omega.set(11103, 11103,1);
                omega.set(k1, 11103,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11103, k1,0);
                omega.set(11103, 11103,0);
                omega.set(k1, 11103,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 9.0) {

                omega.set(k1, k1,1);
                omega.set( 11104, k1,-1);
                omega.set(11104, 11104,1);
                omega.set(k1, 11104,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11104, k1,0);
                omega.set(11104, 11104,0);
                omega.set(k1, 11104,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 25.0) {

                omega.set(k1, k1,1);
                omega.set( 11105, k1,-1);
                omega.set(11105, 11105,1);
                omega.set(k1, 11105,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11105, k1,0);
                omega.set(11105, 11105,0);
                omega.set(k1, 11105,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 61.0) {

                omega.set(k1, k1,1);
                omega.set( 11106, k1,-1);
                omega.set(11106, 11106,1);
                omega.set(k1, 11106,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11106, k1,0);
                omega.set(11106, 11106,0);
                omega.set(k1, 11106,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 72.0) {

                omega.set(k1, k1,1);
                omega.set( 11107, k1,-1);
                omega.set(11107, 11107,1);
                omega.set(k1, 11107,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11107, k1,0);
                omega.set(11107, 11107,0);
                omega.set(k1, 11107,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 36.0) {

                omega.set(k1, k1,1);
                omega.set( 11108, k1,-1);
                omega.set(11108, 11108,1);
                omega.set(k1, 11108,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11108, k1,0);
                omega.set(11108, 11108,0);
                omega.set(k1, 11108,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 70.0) {

                omega.set(k1, k1,1);
                omega.set( 11109, k1,-1);
                omega.set(11109, 11109,1);
                omega.set(k1, 11109,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11109, k1,0);
                omega.set(11109, 11109,0);
                omega.set(k1, 11109,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 27.0) {

                omega.set(k1, k1,1);
                omega.set( 11110, k1,-1);
                omega.set(11110, 11110,1);
                omega.set(k1, 11110,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11110, k1,0);
                omega.set(11110, 11110,0);
                omega.set(k1, 11110,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 54.0) {

                omega.set(k1, k1,1);
                omega.set( 11111, k1,-1);
                omega.set(11111, 11111,1);
                omega.set(k1, 11111,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11111, k1,0);
                omega.set(11111, 11111,0);
                omega.set(k1, 11111,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 16.0) {

                omega.set(k1, k1,1);
                omega.set( 11112, k1,-1);
                omega.set(11112, 11112,1);
                omega.set(k1, 11112,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11112, k1,0);
                omega.set(11112, 11112,0);
                omega.set(k1, 11112,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 45.0) {

                omega.set(k1, k1,1);
                omega.set( 11113, k1,-1);
                omega.set(11113, 11113,1);
                omega.set(k1, 11113,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11113, k1,0);
                omega.set(11113, 11113,0);
                omega.set(k1, 11113,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 7.0) {

                omega.set(k1, k1,1);
                omega.set( 11114, k1,-1);
                omega.set(11114, 11114,1);
                omega.set(k1, 11114,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11114, k1,0);
                omega.set(11114, 11114,0);
                omega.set(k1, 11114,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }
            else if (codea[k2] == 63.0) {

                omega.set(k1, k1,1);
                omega.set( 11115, k1,-1);
                omega.set(11115, 11115,1);
                omega.set(k1, 11115,-1);
                Bigomega = Bigomega.plus(omega);
                omega.set(k1, k1,0);
                omega.set( 11115, k1,0);
                omega.set(11115, 11115,0);
                omega.set(k1, 11115,0);

                Xi.set( k1, 0,((-landy[l1]) + ycoa[k1]));
                Xi.set(k1 + 1, 0,(landy[l1] + ycoa[k1]));
                BigXi = BigXi.plus(Xi);
                Xi.set( k1, 0,0);
                Xi.set(k1 + 1, 0,0);

            }

            if(k2<timeya.length-1) {
                k2++;

            }
            l++;
        } else  if(timex[k1] > timeya[k2]) {
            omega.set(k1, k1,1);
            omega.set(k1, k1 + 1,-1);
            omega.set(k1 + 1, k1,-1);
            omega.set( k1 + 1, k1 + 1,1);
            Bigomega = Bigomega.plus(omega);
            omega.set(k1, k1,0);
            omega.set(k1, k1 + 1,0);
            omega.set(k1 + 1, k1,0);
            omega.set( k1 + 1, k1 + 1,0);

            Xi.set( k1, 0,-(ycoa[k1]));
            Xi.set(k1 + 1, 0,ycoa[k1]);
            BigXi = BigXi.plus(Xi);
            Xi.set( k1, 0,0);
            Xi.set(k1 + 1, 0,0);

        }
        k1++;
    }
    Bigomega.set(0,0,2);

    Matrix mu=Bigomega.inverse().times(BigXi);
    mu.print(3,6);

}
</code>```

}

In the java code xcoa,ycoa,thetacoa get from line no.5(Algorithm EIF-construct). I create another java file to save the values that I get from line no: 5.Then I use it here. I am not very sure how to construct line no:6 and 7 so I assume that our world is noise free and use 1 and -1 for to represent it between two pose. Say , Robot go from $x_t to x_{t+1}$ so the cell between them the diagonal cell represent with 1 and off diagonal -1.

```<code>while(k1<timex.length-1) {
        if (timex[k1] < timeya[k2]) {

            omega.set( k1, k1,1);
            omega.set(k1, k1 + 1,-1);
            omega.set( k1 + 1, k1,-1);
            omega.set( k1 + 1, k1 + 1,1);

            Bigomega = Bigomega.plus(omega);
            omega.set( k1, k1,0); //clear value store in omega
            omega.set(k1, k1 + 1,0);
            omega.set( k1 + 1, k1,0);
            omega.set( k1 + 1, k1 + 1,0);

            Xi.set( k1, 0,-(ycoa[k1]));// construct <span class="math-container">$\xi$</span> vector for y co - ordinate. Alternation of line 19. As fail to understand how to create <span class="math-container">$\mu_{tx},\mu_{ty},\mu_{t\theta},\mu_{jx}\mu_{jy}$</span>
            Xi.set(k1 + 1, 0,ycoa[k1]);
            BigXi = BigXi.plus(Xi);
            Xi.set( k1, 0,0); //clear value store is xi
            Xi.set(k1 + 1, 0,0);

        }
</code>```

90.0,54.0,27.0,81.0.... they are the landmark numbers. As per my code same code run thrice for x,y,theta and then plot all of them to get a map and path. The code I write here is run for y  co because $\xi$ is set as * Xi.set( k1, 0,-(ycoa[k1]));
                Xi.set(k1 + 1, 0,ycoa[k1]);  If I write  down  Xi.set( k1, 0,-(xcoa[k1])*);
                Xi.set(k1 + 1, 0,xcoa[k1]); it will give me x coordinates. So I modify this code in this way.

The problem with this code is Matrix inversion is difficult.

Looking for the suggestion how to implement the code as describe in algorithm?

What is the need of Algorithm EIF_reduce as per table 11.3? I have only 15 landmark. So I calculate $\mu $ at the end of this code block. Am I wrong?


问:
EGM setup. Not possible to open the UdpUc device
I am trying to set up EGM (Externally Guided Motion from ABB) for Position Guidance using C#. But when the code reaches EGMSetupUC it will always give the error:

41824: Not possible to open the UdpUc device

Description

It was not possible to open the external device EGMsensor: that was specified in the RAPID instruction EGMSetupUC.

Consequences

The program execution is stopped immediately.

Actions

Check if

1) The device name is spelled correctly.

2) The device is connected.

3) The device is up and running.

The robot code is the following:

```<code>PROC mainEGM()
    ! Move to start position. Fine point is a demand
    MoveJ p10,v300,fine,tMarkerAuto;
    Test_EGM;
ENDPROC

PROC Test_EGM()
    EGMReset egmID1;
    egmSt1:=EGMGetState(egmID1);
    TPWrite "EGM state: "\Num:=egmSt1;

    EGMGetId egmID1;
    egmSt1:=EGMGetState(egmID1);
    TPWrite "EGM state: "\Num:=egmSt1;

    IF egmSt1 <= EGM_STATE_CONNECTED THEN
        ! Set up the EGM data source: UdpUc server using device "EGMsensor:" and configruation "default"
        EGMSetupUC ROB_1, egmID1, "default", "EGMsensor", \pose, \CommTimeout:=5;
    ENDIF
    TPWrite "EGM state: "\Num:=egmSt1;
    Stop;
    ...
ENDPROC
</code>```

In C#

```<code>private void EGMProcess(object caller, DoWorkEventArgs e)
{
    UdpClient mUdpServer = new UdpClient(mPort);
    IPEndPoint sender = new IPEndPoint(IPAddress.Any, mPort);

    Console.WriteLine("Waiting for EGMRobot message...");
    var protomessage = mUdpServer.Receive(ref sender);
    ....
}
</code>```

Robot program won't go past EGMSetupUC and C# program won't get out of Receive, then I am deducing they can't connect with each other.
Both are on the same network, in other projects we have confirmed that UDP protocol works fine, and we have used RRI and socket communication with this robot.

It is most likely a parameter configuration issue.
Can someone help us with this??

ContentLicense: CC BY-SA 4.0


问:
What is rotor torque?
On my stepper's datasheet, it has the category "rotor torque" (labeled in N-CM). What does that mean? Is this the torque it has can supply when turning? (Hopefully)

ContentLicense: CC BY-SA 3.0

Title: What is rotor torque?


问:
Why differential flatness is called "flat"?
A system is differentially flat if the state and control input can be written as functions of the flat outputs and their time derivatives.

In other words, if a system is differentially flat, we can represent the state of the system and control signal as:
$$x = x(y, \dot y, \ddot y, ... , y^{(p)})$$
$$u = u(y, \dot y, \ddot y, ... , y^{(q)})$$

It's really a nice property of the system. But I don't quite get the name of this property. Why it is called differential "flatness". What does it have to do with "flat"?


问:
Accuracy of monocular visual-inertial odometry
I am currently considering to use monocular visual-inertial odometry (VIO) to localize a smartphone relative to a known object. Now, what I would like to understand is, what kind of accuracy I can expect with such methods.

Is there any rule-of-thumb, which tells me something like a standard deviation from the real position, assuming that the filter is properly initialized and we have average smartphone hardware?


问:
OpenRAVE "fast-forward" physics simulation?
I am working with OpenRAVE simulator and I am using ODE Physics (OpenRAVE plugin to support ODE).

I have a robot in an environment and multiple other objects that are subject to the physics engine. I would like to send velocities to the robot for the duration of 1 second (in real time). I am able to achieve something like this with the following logic:

```<code>penv->StartSimulation(0.001);
while(true) {
    {
        EnvironmentMutex::scoped_lock lock(penv->GetMutex());
        robot->SetVelocity(OpenRAVE::Vector(0.1, 0, 0), OpenRAVE::Vector(0, 0, 0));
    }
}
penv->StopSimulation();
</code>```

Note that although not included in the code above, I keep a timer since the start of the simulation and I am constantly checking it until I reach 1 second of difference, in which case I break out of the while loop.

Now, what I really want to do, is to simulate the real time into simulation time. So the same result to happen in faster time (i.e fast-forward) the result of applying the robot velocities for 1 real time second but let's say in 10, 100ms or any other time less than a second. I am basically looking on how to get the final result of applying the velocities (including how the environment will react to the robot-to-object interactions using the physics engine).

I have tried few things but nothing worked. Here is the closest I can get:

```<code>penv->StartSimulation(0.001, false);
double sim = 0.000001 * penv->GetSimulationTime();
while(true) {
    if (0.000001*penv->GetSimulationTime() > sim+1)
    {
        break;
    }

    {
        EnvironmentMutex::scoped_lock lock(penv->GetMutex());
        robot->SetVelocity(OpenRAVE::Vector(0.1, 0, 0), OpenRAVE::Vector(0, 0, 0));
    }
}
penv->StopSimulation();
</code>```

The false at the StartSimulation indicates that the simulation does not happen in real time and the SimulationStep is called as fast as possible.

The problem I am getting is that the robot (and probably the physics) are not keeping the robot at the right position. When I execute the above solution by the end of it, the robot is fall down from the ground for some reason. I have also tried calling penv->StepSimulation(0.001); but this did nothing more.

Any ideas?


问:
Wheeled Mobile Robot State Estimation Model Using Odometry
I have a process model update that follows the form:

$$
\mathbf{x}_{k|k-1} = \mathbf{x}_{k-1|k-1} + \delta \mathbf{x}_k,
$$

where $\mathbf{x}_{k-1|k-1}$ is the posterior estimate from the last filter update and $\delta \mathbf{x}_k$ is the increment due to odometry (which is treated as a control step).

My question is this:  When trying to implement this as a predict step in a state estimator (e.g. Kalman filter), is it enough to consider $\mathbf{x}_{k-1|k-1}$ and $\delta \mathbf{x}_k$ as being independent Gaussian random variables drawn from distributions ($\mathcal{N}(\mathbf{x}_{k-1|k-1}, P_{k-1|k-1})$ and $\mathcal{N}(\delta \mathbf{x}_k, Q)$, $Q$ is the odometry model covariance and $\mathcal{N}(m, \sigma^2)$ is a Gaussian distribution with mean $m$ and variance $\sigma^2$)?  In this case, I would have the following model covariance update during the prediction step:

$$
\mathbf{x}_{k|k-1} = \mathbf{x}_{k-1|k-1} + \delta \mathbf{x}_k \\
P_{k|k-1} = P_{k-1|k-1} + Q
$$

My hangup is that this does not follow the normal Kalman filter prediction paradigm:

$$
P_{k|k-1} = J P_{k-1|k-1} J^T + Q.
$$

Fundamentally, I believe my update equation is correct because of the fact that the mean and covariance of the sum of two Gaussian distributions is the sum of the mean and the sum of the covariance, respectively, but I am having difficulty reconciling this fact with the Kalman filter equations.  Does my reasoning make sense?  Am I correct?


问:
Kalman filter with measurements indirectly determining state
I have a question regarding a 'fundamental' understanding of the Kalman filter. So, for my application I'm trying to estimate state $x = [\theta~\dot{\theta}~\ddot{\theta}]$ of a robot based on gyroscope readings. However, the gyroscopes are not located on the main platform of the robot but on the pivot points of the wheels (same principle as supermarket shopping cart). The angle $\delta_i$ measures the angle that the pivot makes wrt the robotic platform.

All the examples that I've seen in literature and on the internet used a measurement space that is a direct approximate of the state already (using a GPS when approximating position while the GPS gives a direct estimate of that position). However when closely following the equations of the Kalman filter, nowhere I see a potential place where to implement mapping of the measurement space to the state space (the model relating gyroscope data to theta states)

Even though this model would be really easy ($\dot{\theta} = g_i - \dot{\delta_i}$) I don't know how or whether or not I should implement this. Right now it seems as if I could give the Kalman filter a measurement space and a predicted measurement of whatever I want without giving an indication on how this measurement space is related to the state space.

This would pose an even bigger problem when trying to estimate the $x$ and $y$ state of the robotic pod when using encoders from the wheels as well as IMU data on the pivot since these relations become far more complex than the current one.

Can someone explain this to me? I would really appreciate it.


问:
Create path with minimum curvature for Ackerman drive
I'm trying to create a trajectory between two given poses for an Ackerman drive.
The poses are defined as follows:

Current Pose

position $(x_0, y_0)$

rotation on z-axis $\theta_0$

Final Pose

position $(x_f, y_f)$

rotation on z-axis $\theta_f$

The trajectory has to be 2 times differentiable with minimum curvature and needs a smooth transition to it's predecessor trajectory (the steering change between two trajectories can't be instant).

I always stumble upon parts of the solution but am struggling to put everything together. Some people suggest using bezier curves or cubic polynomials but I couldn't find a concrete example (something like: given current pose $[(0,0),0]$ and final pose $[(1,1),\frac{\pi}{2}]$ you can calculate the trajectory like this ...).

So my questions are:

Is that paper actually solving my problem?

Are there any useful tutorials/implementations/examples out there that are a bit more explanatory?


问:
Detecting and counting the number of junctions
So I am making a robot which is to follow a line. The field has both +(junctions) as well(by the intersection of two lights). I am yet to figure out what exactly is the best way to track the number of junctions.

The following is the code I have thought out. However, if there is a better way I would highly appreciate any suggestions.

```<code>    while (true){
    cout << rlink.request(READ_PORT_4)<<endl;
    go_straight();
    int sensor_counter=0;
    while(sensor_counter<=2){
        if (sensor_values==111){
            while(sensor_values!=0){
                go_straight_before_junction();
            }
            if (sensor_values==0){
                sensor_counter=sensor_counter+1;
            }

        ;}

    ;}
    while(sensor_values!=3){
        turn_left_corner();
    }
    if (sensor_values==3){
        direction-=90;
    }
</code>```

sensor_values=5 -robot Too left to the line

sensor_values=4 -robot Slightly left to the line

sensor_values=3 - exactly on the line

sensor_values=2 -robot slightly right of the line

sensor_values=1 -robot Too right of the line

Sensor_values=0 or 111 - robot crossed junction (0 is for the sensor at the back off the centre line and 111 is for the 3 sensors at the front. )

I'm using a light sensor and it reads 1 when in front of a white line and 0 when in front of a black line(rather like white tapes on a black floor
).


问:
Naze32 flight controller not connecting to RC
Cleanflight reads my Naze32, but the RC values aren't responding at all. Everything is connected and double checked that the wires were connected properly and firmware was flashed. Just not getting any feedback from cleanflight for throttle/pitch/yaw. </p>


问:
DH parameters for lynx robot
Can anyone help me out with DH parameters and axis placement for below kinematic chain?

Title: DH parameters for lynx robot


问:
Magnetometer Calibration
I'm looking at using a magnetometer in a project and I'm struggling to understand the proper procedure for calibration of the sensor.

I understand that magnetometers need to be calibrated to compensate for hard and soft iron effects.

I've seen a number of algorithms for calibration based around ellipsoid fitting, similar to that detailed in https://www.st.com/content/ccc/resource/technical/document/design_tip/group0/a2/98/f5/d4/9c/48/4a/d1/DM00286302/files/DM00286302.pdf/jcr:content/translations/en.DM00286302.pdf. These algorithms all require the magnetometer be spun in three dimensions, to collect enough data for an ellipsoid to be fitted, thereby allowing estimation of scaling and offset parameters to translate and scale the ellipsoid to a unit sphere positioned at the origin.

However, I'm unclear on how the calibration algorithm should be used, for example if say I'm placing my magnetometer in the boot of a car next to several other bits of metal would I be able to calibrate the magnetometer by spinning the magnetometer in three dimensions just above its mounting point? Would this then calibrate out the effect of the hard and soft iron effects in the magnetometers vicinity? Or should I actually be mounting the magnetometer in its mounting point and then spinning the entire platform (in this example the car) in three dimensions? Could someone also explain the physics as to why the calibration should be performed in either manner?

Title: Magnetometer Calibration


问:
How to interpret the passivity criteria equation?
In Passivity-Based Control and Estimation in Networked Robotics (among others), a passive system is defined as one that:

$$\int_0^t y^T(t)u(t) dt \geq -\beta $$

for output $y$ and input $u$.

I have never understood how this describes a system where

the total internal energy stored in the system is bounded above by the energy externally supplied to the system

This comes down to  $y$ and $u$ being under defined.

One way to interpret it, is since we are discussing energy, that $u$ and $y$ have units that result in their product being a measure of energy? And implicitly that the integral defines some energy reservoir or dissipation?

Alternatively, the only way that the above is violated, is if $y$ is negatively proportional to $u$? But what does that mean mechanically? That the robot is compliant?

How should I imagine $y$ and $u$ in order to make sense of the passivity criteria above?


问:
CNC stepper motors are loud and slow after decreasing the steps/mm from 250 to 5
I'm making a drawing robot and have hooked up my steppers to each axis. I'm using the grbl shield with arduino and nema 17 steppers.

Initially the firmware was set at 250 steps/mm but with my setup this would move my axis by 50mm. Therefore I only need 5 step/mm. However now that I have put it to 5mm my steppers run very slowly and vibrate quite a bit.

This is the case even when I command an axis to move a set distance. For example, with 250steps/mm I can tell the axis to move 2mm in universal gcode sender (actually 100mm physically) and it will move swiftly and quietly. However with the setting at 5 steps/mm I can command it to move 100mm and it will be very slow, loud and vibrate like hell. This behaviour is seen even without changing any feedrate, max acceleration and speed settings.

Any ideas how I could fix the issue? Is 5 step/mm reasonable? I'm just using standard T-20 belts and pulleys so other people must also have this setting.

Thanks in advance!


问:
Map initialization problem in Mono SLAM
I'm reading and trying to implement the paper Monofusion(V. Pradeep, C. Rhemann, S. Izadi, C. Zach, M. Bleyer and S. Bathiche, "MonoFusion: Real-time 3D reconstruction of small scenes with a single web camera," 2013 IEEE International Symposium on Mixed and Augmented Reality (ISMAR), Adelaide, SA, 2013, pp. 83-88.)

In this paper, in the map initialization phase the system extracts MxM patches of FAST features in 3-level pyramid images and while the user slowly moves the camera, it also matches feature points by ZNCC score from patches in the initial and current frames, computes the essential matrix, recovers the initial camera pose by SVD and triangulates the matched features to create 3D landmarks.

They said the system automatically bootstraps only once a sufficient motion baseline is reached. However I don't understand how to get the "sufficient motion baseline" from this phase.

As I understand, it seems like the system automatically finishes the initialization when the actual baseline between the initial camera frame and current camera frame has reached some threshold.

Does this mean it computes the relative translation between the initial and current frame using the 3D triangulated points?

If this question is silly sorry in advance, I’m a newbie in the SLAM world.

Any advice is welcome.

Title: Map initialization problem in Mono SLAM


问:
Incremental (on-the-fly) smooth trajectory interpolation algorithm from waypoints?
short version (TL;DR):

I'm receiving x(t) waypoints "online", which I must travel through at the specified times. I need an algorithm that would plan a smooth trajectory in-between them without knowing the whole trajectory in advance.

Longer version:

I have a 1D motion control problem. I control a DOF of a robot, and a primary design goal is for the motion to be very smooth.

A high-level controller (computer) sends my low-level motion controller a waypoint, expressed as an (xi, ti) pair, every N msec. I am running my control loop at a much higher rate, say every 1 msec.

Because of various constraints, I am not able to get the whole trajectory in advance - I am getting new waypoints as the robot is moving through previous waypoints. I am, however, able to receive a few waypoints in advance (say 3 or 4) so I have a short buffer (look-ahead) and not limited to working strictly with the next waypoint only.

I need an algorithm that would build a smooth trajectory in between these waypoints. 'Smooth' here means continuous velocity and continuous acceleration (which also implies bounded jerk).

I have tried to implement a cubic spline interpolation scheme, since it appears to be exactly what my problem requires: a piecewise function of 3rd degree polynomials, which are guaranteed to have continuous first and second derivatives everywhere, including at the 'stitches'. However, all the implementations I saw generate the spline based on all the data points, which unlike my case, are usually known in advance. My attempts to implement incremental spline generation have failed, and I'm starting to figure this is not a technical error but a mathematical impossibility. If you ask, I'll edit and try to formally explain why I think so, but this question is already quite long as it is.

So, for concrete questions:

Do you know if it's possible to build a cubic spline this way, incrementally, and keep its 1st and 2nd derivatives smooth?

If, as I suspect, it's impossible: What other algorithm would you suggest, that will build the trajectory incrementally, pass through all the waypoints at the specified times, while keeping everything smooth up to the 2nd derivative? Alternatively, have I defined a problem with no solution and I must relax some of the requirements?

Thank you!


问:
python code for closed form inverse kinematics solution
I solved the inverse kinematics of a 4DOF robotic arm (which is not planar) and I wrote this code bellow in python. The coordinates for each joint is:

I considered the first three joints as a wrist and solved my inverse problem. For solving my inverse problem I used the DH parameters and by multiplying matrix I found the matrix that describes the end-effector(e) in the base frame(B)(forward kinematics). Also, I considered the first three joints as a wrist. My inverse kinematic method is similar to this onehttp://www.golems.org/papers/OFlaherty13-hubo-kinematics-techreport.pdf
But the allocation of coordinates in my solution is different from that as you see in my picture. This is my python code for inverse kinematics:

```<code>import numpy as np
import math
import cmath

# length parameters are in cm
La1=1
La2=1
La3=1
x=0
i=0
with open( "endefector.txt") as f:
 matrix=np.zeros((4,4))
 data=[[]]
 for line in f:
   matrix[i]=(list(map(float,line.split(',') [:4])))
   i=i+1
 print(matrix)
</code>```

# here i need the inverse of the above matrix for solving inverse kinematics so i reversed it.
    R= matrix[0:3,0:3]
    RT=np.zeros((3,3))
    for i in range( len(R)):
       for j in range(len(R[0])):
         RT[j][i]=R[i][j]

```<code>RT=-1*RT

pos=matrix[0:3,3:4]
Tpos=np.dot(RT,pos)
Px=Tpos[0] # this is "the input x position" of endeffector for my inverse
kinematic solution
print(Px)
Py=Tpos[1]
print(Py)
Pz=Tpos[2]
print(Pz)

sintetha2=(La2*La2+La1*La1-Pz*Pz-Py*Py-(Px+La3)*(Px+La3))/(2*La1*La2)
tetha2= math.atan2(sintetha2,np.sqrt(1-sintetha2*sintetha2))
print(tetha2)
</code>```

this is just a part of my code for solving tetha2 then  I wanted to validate the solution for theta2.so I assume the other joint variables(theta)are equal to zero and in a loop I increased the amount of theta2 and  put it in forward kinematics and achieved the desired matrix (the matrix that describes the end-effector in base frame) and used it for inverse solution as it used in the hubo kinematics too. Then I wanted to compare the input tetha2 (it is mentioned in the second part of code as th2) and the amount of tetha2 that comes from the inverse solution (it is mentioned as tetha2). I printed the difference between these two values at the end of my python code. Here is the python code for testing th2 in a given trajectory:

```<code>import numpy as np
import cmath
import math

#Link Length
La1=1
La2=1
La3=1

pi=math.pi #3.141592653589793238

#initial theta
th1=0.0000
th2=0.0000
th3=0.0000
th4=0.0000
while th2 >= -1*pi:

TBE=[[np.sin(th4)*(np.cos(th1)*np.sin(th3)-np.sin(th1)*np.cos(th2)*np.cos(th3))+np.sin(th1)*np.sin(th2)*np.sin(th4),np.cos(th4)*(np.cos(th1)*np.cos(th3)-np.sin(th1)*np.sin(th2)*np.sin(th3))-np.sin(th1)*np.cos(th2)*np.sin(th4),np.sin(th1)*np.sin(th2)*np.cos(th3)+np.cos(th1)*np.sin(th3),La3*np.sin(th4)*(np.cos(th1)*np.sin(th3)-np.sin(th1)*np.cos(th2)*np.cos(th3))+np.sin(th1)*np.sin(th2)*np.sin(th4) +La2*np.sin(th1)*np.cos(th2)],[np.sin(th4)*np.cos(th2)*np.sin(th3)+np.sin(th2)*np.cos(th4),np.cos(th2)*np.sin(th3)*np.cos(th4)-np.sin(th2)*np.sin(th4),-1*np.cos(th2)*np.cos(th3),La3*np.sin(th4)*np.cos(th2)*np.sin(th3)+np.sin(th2)*np.cos(th4)+La2*np.sin(th2)-La1],[np.sin(th4)*(np.cos(th1)*np.sin(th2)*np.sin(th3)+np.sin(th1)*np.cos(th3))-np.cos(th1)*np.cos(th2)*np.cos(th4),np.cos(th4)*(np.cos(th1)*np.sin(th2)*np.sin(th3)+np.sin(th1)*np.cos(th3))+np.cos(th1)*np.cos(th2)*np.sin(th4),np.sin(th1)*np.sin(th3)-np.cos(th1)*np.sin(th2)*np.cos(th3),La3*np.sin(th4)*(np.cos(th1)*np.sin(th2)*np.sin(th3)+np.sin(th1)*np.cos(th3))-np.cos(th1)*np.cos(th2)*np.cos(th4)-La2*np.cos(th1)*np.cos(th2)],[0,0,0,1]]
T=np.reshape(TBE,(4,4))

R=T[0:3,0:3]
RT=np.zeros((3,3))
for i in range(len(R)):
    for j in range(len(R[0])):
        RT[j][i]=R[i][j]

orientation=-1*RT
pos=T[0:3,3:4]

Tpos=np.dot(orientation,pos)
Px=Tpos[0]
Py=Tpos[1]
Pz=Tpos[2]

sintetha2=(La2*La2+La1*La1-Pz*Pz-Py*Py-(Px+La3)*(Px+La3))/(2*La1*La2)
tetha2= math.atan2(sintetha2,np.sqrt(1-sintetha2*sintetha2))
tetha22=-1*math.atan2(sintetha2,-1*np.sqrt(1-sintetha2*sintetha2))
print('th2',th2)
print('tetha2',tetha2)
print('tetha22',tetha22)

print('th2-tetha2=',th2-tetha2)
print('th2-tetha22=',th2-tetha22)
print(th2)

th2 =round(th2-0.1,2)
</code>```

but as I ran run this code, the difference of the values of th2 and tetha2 as th2 gets near of 1.57, increase accurately and I don't know why. I need the difference between these two to be very small in the whole joint range. I will be so appreciated if someone helps me to find the problem and fix it.


问:
How to implement end effector orientation in generic IK solver
I am trying to script a basic, lightweight IK solver with a Jacobian Transpose method. I have gotten a basic script that reach the desired position very quickly. What I am not able to do is achieve a desired orientation as well. I want to avoid the spherical wrist model in order to test a wide variety of robot configurations, so I need to include orientation as a general kinematic parameter. I haven't taken linear algebra in 14 years and my matrix math is VERY elementary, so ideally, I would love some help finding a more geometrical approach to finding the rotational values for the various axes.

I tried to do it by having three points on a target plane that correspond to points on the end effector thich through iterative calculation approach a solution. However this would get stuck when the rotation was such that the error vectors of the three points added up to less than my convergence threshhold. The working code is here:

```<code>        public List<double> axisDelta(List<Line> axesTemp, Plane eePl, Plane target, double step)
    {
        Point3d targetPoint = target.Origin;
        Point3d eePoint = eePl.Origin;
        Vector3d error = targetPoint - eePoint;
        List<double> deltaTemp = new List<double>();

        for (int i = 0; i < axesTemp.Count; i++)
        {
            Vector3d jointToEE = eePoint - axesTemp[i].ClosestPoint(eePoint, false);
            Vector3d jakobian = Vector3d.CrossProduct(axesTemp[i].Direction, jointToEndCP);

            jakobian.Unitize();
            double dot = Vector3d.Multiply(error, jakobian);

            double theta = 0;
            if (jointToEndCP.Length > 0) theta = Math.Atan(dot / jointToEndCP.Length); //avoid dividing by 0

            deltaTemp.Add(theta * step);
        }
        return deltaTemp;
    }
</code>```

Please help me understand the best way to implement orientation into the script to find the axis rotation values to align the End-Effector plane with the target plane. Thanks!!


问:
Doubts in the implementation of a Kalman filter to merge position data from two identical sensor
I'm currently working on the implementation of a system for calculating the vibrations of a hand using two Leapmotion devices, these devices deliver in real time the spatial location (x,y,z) and the speed of each intersection between the bones of the hand. Both devices point to the same hand from different points of view (~70° difference), the data obtained are merged using homogeneous matrices on one of the sensors (called the "support" leapmotion), obtaining results with very low error rate. After this I must give more robustness to the data resulting from the homogeneous matrix (from "support" leapmotion) by using Kalman to merge the data obtained with the "support" and the "leader" sensors.

Two doubts arise when studying Kalman, the first is about my initial matrices for the process, I can not understand on what basis they are built, I leave what I have so far.

```<code>public class Kalman {

Inverse inv = new Inverse();
public double dt = 0.01;//0.01s
public double Wk = 0;
public double Xx = 2.0;    public double Xy = 2.0;    public double Xz = 2.0;
public double Vx = 3.0;    public double Vy = 3.0;    public double Vz = 3.0;
public double dPVx = 2.0;  public double dPVy = 2.0;  public double dPVz = 2.0;
public double dPXx = 3.0;  public double dPXy = 3.0;  public double dPXz = 3.0;
public double dX = 0.1;    public double dY = 0.1;    public double dZ = 0.1;
public double dVx = 4.0;   public double dVy = 4.0;   public double dVz = 4.0;
public double Zx = 15.0;    public double Zy = 14.0;    public double Zz = 13.0;

public double Xkant [][] = {{Xx},{Xy},{Xz},{Vx},{Vy},{Vz}};

public double Akp [][] = {{1.0, 0.0, 0.0, dt, 0.0, 0.0},
                          {0.0, 1.0, 0.0, 0.0, dt, 0.0},
                          {0.0, 0.0, 1.0, 0.0, 0.0, dt},
                          {0.0, 0.0, 0.0, 1.0, 0.0, 0.0},
                          {0.0, 0.0, 0.0, 0.0, 1.0, 0.0},
                          {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}}; //transition matrix

public double AkpT [][] = {{1,0,0,0,0,0},
                           {0,1,0,0,0,0},
                           {0,0,1,0,0,0},
                           {dt,0,0,1,0,0},
                           {0,dt,0,0,1,0},
                           {0,0,dt,0,0,1}}; //transition matrix trasposed

public double Pkant [][] = {{dPXx*dPXx,0,0,0,0,0},
                            {0,dPXy*dPXy,0,0,0,0},
                            {0,0,dPXz*dPXz,0,0,0},
                            {0,0,0,dPVx*dPVx,0,0},
                            {0,0,0,0,dPVy*dPVy,0},
                            {0,0,0,0,0,dPVz*dPVz}}; //Initial value of covariance of estimation error

public double Q [][] = {{dX,0,0,0,0,0},
                        {0,dY,0,0,0,0},
                        {0,0,dZ,0,0,0},
                        {0,0,0,dVx,0,0},
                        {0,0,0,0,dVy,0},
                        {0,0,0,0,0,dVz}}; //Covariance matrix of process noise

public double R [][] = {{dX*dX,0,0,0,0,0},
                        {0,dY*dY,0,0,0,0},
                        {0,0,dZ*dZ,0,0,0},
                        {0,0,0,dVx*dVx,0,0},
                        {0,0,0,0,dVy*dVy,0},
                        {0,0,0,0,0,dVz*dVz}}; //covariance matrix sensor noise

public double Z [][] = {{dX*dX,0,0,0,0,0},
                        {0,dY*dY,0,0,0,0},
                        {0,0,dZ*dZ,0,0,0},
                        {0,0,0,dVx*dVx,0,0},
                        {0,0,0,0,dVy*dVy,0},
                        {0,0,0,0,0,dVz*dVz}}; //

public double H [][] = {{1,0,0,0,0,0},
                        {0,1,0,0,0,0},
                        {0,0,1,0,0,0}}; //just format for results?

public double HT [][] = {{1,0,0},
                         {0,1,0},
                         {0,0,1},
                         {0,0,0},
                         {0,0,0},
                         {0,0,0}}; //just format for results?
</code>```

The second doubt is where should I insert the data of my Leapmotion "leader" and the rotated data of the Leapmotion device "support" in the Kalman equations for its correct fusion?

In some documents I could observe that matrix Xk* is 3x1 dimensions where the three variables (x, y, z) obtained by the sensors are inserted. So, where is inserted the value of the "leader" sensor? is it my Xk0? or is the data in Xk* the average of the data obtained by both sensors? or maybe I should extend Xk* so that it is 6x1 like ([Xa], [Ya], [Za], [Xb], [Yb], [Zb])?

Thanks for reading!


问:
how to introduce 2 inputs into a matlab block function that processes only one variable at a time?
Using SIMULINK, I wanted to optimize 2 variables with the ant colony optimization algorithm; which is written in a matlab function block, the function is of type function y = f (x) it has only one variable to optimize and since I have 2 variables! how can i do ?


问:
Laser / photosensor pair or similar
I'm looking for a laser / photosensor pair (or product of similar function) for detecting when a beam is interrupted (no more than 3ft apart, probably more like 1ft).

I'd like these to run off of 5V, since I'm using an Arduino. My main requirement, however, is that these parts have nice housings, ideally with some mounting screw holes or something along those lines. This is going into a project where sturdiness and durability are important.

I don't know how to search for parts like what I am looking for. Could you please point me either to some good product sources, give me some better keywords for searching, or link me directly to potentially useful products?

Title: Laser / photosensor pair or similar


问:
Torque Disturbance Observer for Manipulator Arms
Is there a tutorial/guide apart from the research papers ( paper 1 and paper 2 ), for implementation of torque disturbance observers on multi-DOF robotic manipulators to determine sudden collision event or estimate feed-forward torque.</p>


问:
how to normalize PWM if the value exceeds the maximum speed limit
I have implemented inverse kinematics for nonholonomic mobile wheeled robots, and the RPM range of the motor that I used is -50 to 50, while for PWM is from 1300 to 1700. But in certain cases, the RPM that generated from inverse kinematics exceeds the maximum speed. How to normalize it so the RPM or PWM does not exceed the maximum speed so the robot can run properly and reach the target?


问:
How to create a .urp for universal robots?
The polyscope platform of Universal Robots saves programs as *.urp. They are very different to urscrips.
Is there a way to create such files without polyscope?</p>


问:
motor selection for farm rover
I'm designing a rover to use as a harvest aid on our vegetable farm.  It will be GPS guided using an RTK base station to straddle our parallel fixed-width vegetable beds. (66" wide with 48" of growing area, the rest is 18" of path for feet and tires)  The human harvester will be able to control the rover's speed along a fixed path so it can function as a harvest bin holder and carrier. Picture a rolling table 66" tire center to tire center, 48" or so long and 36" high.  The axles are upside down U shaped so that there is clearance for the plants underneath. It will creep along at very slow speeds in front of the person harvesting and then return to our pack-shed at faster speeds.

It will have 4 wheels.  The front axle will pivot on its center in relation to the rest of rover and have an Ackermann steering design, such that one motor, centrally positioned, will turn the front tires via tie-rods connected to steering arms.  This is the first motor I need to select and size. Its shaft will move less than 180 degrees but needs to be precisely controlled.

The rear axle will have 2 drive wheels, each with its own motor, and will be fixed to the main body of the rover. These are the second motors I need to select and size.

My initial design will be for 500lb Max Gross Weight, but if it works well, it would be nice to build one that could carry a ton. I've listed some calculations below that relate the max speed, which is only 5-7mph, and the tire diameter to figure the max RPMs of the drive wheels.  It also looks at the weight, rolling resistance and gradient climbing resistance to figure the power and torque requirements.

Could someone with experience in this check out my numbers, verify I'm on the right track with my calculations and make some recommendations for motors? In-wheel motors look really interesting for the drive wheels, especially with this design, which needs lots of open space in the center, making a differential difficult.  I was thinking I could program differential speed via the 2 motors based on the position of the steering motor.

As for the steering motor, I'm not sure if a servo would be required here of if a stepper would work?  Any suggestions on motor type and size would be greatly appreciated.

max speed       7 miles/hour, 10.27 ft/s,   3.13 meters/sec
tire diameter   22 inches,  1.83 ft,    0.56 meters

tire circumference          5.76    ft

angular velocity            11.20   radians/sec

RPM                         106.95  RPM

total vehicle weight        500 lbs
                            226.80  kilograms
number of drive motors      2 motors
motor efficiency            0.85

weight per drive wheel      113.398 kg
resistance by weight        25%
gradient percentage         15%
rolling force               28.3495 N
gradient force              17.0097 N

motor power                 286 Watts
motor torque                14.91   Nm
wheel traction force        45.36   N

Title: motor selection for farm rover


问:
Writing IMU accelerometer data into as twist messages into cmd_vel
I am reading accelerometer data from the wiimote /wiimote/state topic.

A sample of the data I'm working with is as follows:

```<code>angular_velocity_raw:
  x: 8292.0
  y: 8246.0
  z: 8182.0
angular_velocity_covariance: [0.1915446291709091, 0.0, 0.0, 0.0, 0.3915998474972723, 0.0, 0.0, 0.0, 0.5571460438609092]
linear_acceleration_zeroed:
  x: -3.63209259259
  y: -1.45283703704
  z: 8.67511346154
linear_acceleration_raw:
  x: 120.0
  y: 128.0
  z: 154.0
</code>```

From running the turtlebot3_teleop node I can see that the range of data on the linear-x axis is -2.84 to 2.84 while on the angular-y axis it's -0.22 to 0.22.
My problem is the x axis the data I retrieve from the accelerometer is much larger. For instance, from the output above, angular_velocity_raw.z has a value of 8182.0 and linear_acceleration_raw.x has a value of 120.0.

Can anyone help me with advice on how I can accurately make my accelerometer figures fit into the specified ranges mentioned above?


问:
Motor Controller with micro USB interface
I am a Robotics enthusiast and planning to make a small and simple four wheel car whose motors are supposed to be controlled by an Android device housed inside the car by means of the micro USB port of the device. The car has to move forward or backward only as directed by the signals from the Android device.

So my assumption is that there should be a circuit board which accepts the signals from the microUSB/USB of the Android device and controls the power to the electric DC motor. Also the power for the motor will be supplied from a battery pack inside the car.

Could anyone suggest me a cheap motor driver circuit which supports micro USB or USB? And where can I get the parts for this online? I did a lot research but very confused with the technical terms which I am not familiar with.


问:
Fixed-Delay Kalman smoother with/without augmented measurements
There are several algorithms regarding fixed-lag Kalman smoothing.
In most cases, an augmented state vector is defined in which the elements are the current and delays of the original state vector.

However, in some cases the measurements are augmented to include delays, as in:
https://pdfs.semanticscholar.org/f580/23aa0827e4a5ab1d04957b2b6cf266aad541.pdf

And in some cases, only the k'th measurement is used in the augmented system, as in section 3.2.2 of:
http://robby.caltech.edu/~jwb/courses/CDS112/FixedLagSmoother.pdf

I ran a simulation which yielded better results when augmenting the measurements vector, but as far as I understood, replicating the measurements is a simple linear transformation of the measurements which should not improve the performance of the augmented filter.
Is there a theoretical justification for augmenting the measurements vector?

Matlab simulation code:

```<code>% model:
% x(k+1) = F*x(k) + w(k)
% y(k)   = H*x(k) + v(k)
% R = cov(v), Q = cov(w)

% generate clean data
T = 100;
dt = 1e-2;
t = 0:dt:T;
v0 = [40;50];
r0 = [0;0];
a = [0;-9.81];

v = v0 + a*t; % constant acceleration
r = r0 + cumsum(v*dt,2);

inds = find(r(2,:) > 0);
N = numel(inds);
t = t(inds);
r = r(:,inds);
v = v(:,inds);
a = repmat(a, [1 N]);

% generate measurements
rng(0);
sig_r = 3*1e0;
rm = r + sig_r*randn(2,N);

% kalman filter
x = zeros(6,1); % position, velocity, acceleration
P = 10*eye(6,6);

% acceleration is known...
P(5,5) = 0.5;
P(6,6) = 0.5;

F = [eye(2)   eye(2)*dt eye(2)*(dt^2)/2; ...
     zeros(2) eye(2)    eye(2)*dt; ...
     zeros(2) zeros(2)  eye(2)];

% position measurement only
H = [1 0 zeros(1,4); ...
     0 1 zeros(1,4)];

R = eye(2)*sig_r^2;
vQ = [dt^3/6;dt^3/6;dt^2/2;dt^2/2;dt;dt];
sig_j = 1e0;
Q = vQ*vQ'*sig_j^2;

rf = zeros(size(rm));
xf = zeros(6,N);
Pf = zeros(6,6,N);
Pf_pred_inv = zeros(6,6,N);
Pf_pred = zeros(6,6,N);

D = 20;
Fa = zeros(6*(D+1));
Fa(1:6,1:6) = F;
Fa(7:end,1:end-6) = eye(6*D);
xa = zeros(6*(D+1),1);
Pa = zeros(numel(xa));
Pa(1:6,1:6) = P;
Qa = zeros(size(Pa));
Qa(1:6,1:6) = Q;

% single measurement
xA = xa;
PA = Pa;
Ra = R;
Ha = [H zeros(size(H,1),size(H,2)*D)];

% augmented measurements
RA = kron(eye(D+1),R);
HA = kron(eye(D+1),H); % D+1 replicas

for k = 1:N
    % Kalman filter
    % predict
    P_pred = F*P*F' + Q;
    xA_pred = F*x;

    Pf_pred(:,:,k) = P_pred;
    Pf_pred_inv(:,:,k) = inv(P_pred);

    % update
    y = rm(:,k) - H*xA_pred;
    S = R + H*P_pred*H';
    Sinv = inv(S);
    K = P_pred*H'*Sinv;
    x = xA_pred + K*y;
    P = (eye(6) - K*H)*P_pred;

    xf(:,k) = x;
    Pf(:,:,k) = P;
    rf(:,k) = x(1:2);

    % kalman smoother (augmented measurements)
    if(k+D > N)
        v_rA(:,k) = rf(:,k);
    else
        % augmented measurements
        rA = rm(:,k+D:-1:k);
        rA = rA(:);

        % predict
        PA_pred = Fa*PA*Fa' + Qa;
        xA_pred = Fa*xA;

        % update
        yA = rA - HA*xA_pred;
        SA = RA + HA*PA*HA';
        KA = PA_pred*HA'*inv(SA);
        xA = xA_pred + KA*yA;
        PA = (eye(6*(D+1)) - KA*HA)*PA_pred;

        v_rA(:,k) = xA(end-5:end-4); % last position in buffer
    end

    % kalman smoother (single measurement)
    if(k+D > N)
        v_ra(:,k) = rf(:,k);
    else
        % augmented measurements
        ra = rm(:,k+D);

        % predict
        Pa_pred = Fa*Pa*Fa' + Qa;
        xa_pred = Fa*xa;

        % update
        ya = ra - Ha*xa_pred;
        Sa = Ra + Ha*Pa*Ha';
        Ka = Pa_pred*Ha'*inv(Sa);
        xa = xa_pred + Ka*ya;
        Pa = (eye(6*(D+1)) - Ka*Ha)*Pa_pred;

        v_ra(:,k) = xa(end-5:end-4); % last position in buffer
    end

end

% position
figure(1);
plot(r(1,:), r(2,:), '.b');
hold on;
plot(rm(1,:), rm(2,:), '.r');
plot(rf(1,:), rf(2,:), '.k');
plot(v_rA(1,:), v_rA(2,:), '.c');
plot(v_ra(1,:), v_ra(2,:), '.g');

legend('true position', 'measured', 'filtered', 'smoothed, augmented measurement', 'smoothed, single measurement');

% error
figure(2);
Ef = sqrt(sum((r-rf).^2));
EA = sqrt(sum((r-v_rA).^2));
Ea = sqrt(sum((r-v_ra).^2));
plot(Ef, '.k');
hold on;
plot(EA, '.c');
plot(Ea, '.g');

legend('filter', 'smoother: augmented measurements', 'smoother: single measurement');
</code>```


问:
help to find the bug for calculating inverse kinematic using jacobian
I am calculating inverse kinematic for 5dof robotic arm using Jacobian matrix (first time). i used following formulas to do my calculation and also implemented in my code.

for more information please follow this link --> https://www.youtube.com/watch?v=SefTCXrpL8U&t=872s

I am not understanding what I am doing wrong. I am getting the wrong theta values and while loop is running maximum two times, it does not matter which values I give as x, y, z.
- I have searched, how others have calculated their Jacobian matrices. but still could not find my mistake.

please help me to debug my code. I might be doing some silly mistake.
This is a code for calculating forward kinematics

```<code>class Fkine:
    def __init__(self, PT):
        """ Initialize the DH parameters for forward kinematics
            DH peramteres columes should be DH = [theta alpha  r  d]
        """
        if not isinstance(PT, list):
            print("[ERROR] dh parameter should be matrix {}".format(PT))
        if not np.shape(PT)[1] == 4:
            print("[ERROR] enter proper dh parameter {}".format(PT))

        self.PT = np.array(PT)

    def homog_trans_matrix(self, row_num):

        cos_theta_n = np.cos(self.PT[row_num][0])
        sin_theta_n = np.sin(self.PT[row_num][0])

        cos_alpha_n = np.cos(self.PT[row_num][1])
        sin_alpha_n = np.sin(self.PT[row_num][1])

        r_n = self.PT[row_num][2]
        d_n = self.PT[row_num][3]

        homog_trans_matrix = [
            [cos_theta_n, -sin_theta_n * cos_alpha_n, sin_theta_n * sin_alpha_n, r_n * cos_theta_n],
            [sin_theta_n, cos_theta_n * cos_alpha_n, -cos_theta_n * sin_alpha_n, r_n * sin_theta_n],
            [0, sin_alpha_n, cos_alpha_n, d_n],
            [0, 0, 0, 1],
        ]
        return np.matrix(homog_trans_matrix)

    def fk(self):
        """ function to generate forward kinematic from DH parameters
            by multiplying homogeneous transformation matrix Hn = H_0 * H_1 * ....H_n-1
        """
        Hn = []

        # taking the number of rows from the DH parameters
        for i in range(np.shape(self.PT)[0]):
            H = self.homog_trans_matrix(i)

            if i == 0:
                Hn = H

            else:
                Hn = np.dot(Hn, H)

        return Hn

def get_jacobian(fkine_obj, PT):
    Hn = fkine_obj.fk()
    # print(Hn)
    R0_0 = [
        [0.],
        [0.],
        [1.]
    ]
    D0_n = Hn[:, 3][:-1]

    upper_part = np.cross(R0_0, D0_n, axis=0)
    # print(upper_part)

    for i in range(np.shape(PT)[0] - 1):
        H0_i = fkine_obj.homog_trans_matrix(i)

        # accessing 3rd column of the matrix and removing last row
        R0_i = H0_i[:, 2][:-1]

        # accessing 4th column of the matrix and removing last row
        D0_i = H0_i[:, 3][:-1]

        next_col = np.cross(R0_i, np.subtract(D0_n, D0_i), axis=0)

        upper_part = np.concatenate([upper_part, next_col], axis=1)

    return upper_part
def main():

    THETA_1 = 0.1
    THETA_2 = 90.
    THETA_3 = 0.1
    THETA_4 = 0.1
    THETA_5 = 0.1

    L_1 = 33.  # mm 3.3cm
    L_2 = 105.  # mm 10.5cm
    L_3 = 98.  # mm 9.8cm
    L_4 = 27.  # mm 2.7cm
    L_5 = 65.  # mm 6.5cm

    # DH parameter table for my robotic arm
    PT = [
        [math.radians(THETA_1), math.radians(90.0), 0, L_1],
        [math.radians(THETA_2), 0, L_2, 0],
        [math.radians(THETA_3), 0, L_3, 0],
        [math.radians(THETA_4) + math.radians(90.0), math.radians(90.0), 0, 0],
        [math.radians(THETA_5), 0, 0, L_4 + L_5]
    ]

    x_dst = 1.
    y_dst = 1.
    z_dst = 1.

    while 180. >= math.degrees(PT[0][0]) >= 0. \
            and 180. >= math.degrees(PT[1][0]) >= 0. \
            and 180. >= math.degrees(PT[2][0]) >= 0. \
            and 180. >= math.degrees(PT[3][0]) >= 0. \
            and 180. >= math.degrees(PT[4][0]) >= 0.:

        fk_5d = fk.Fkine(PT)
        jacobian = get_jacobian(fk_5d, PT)

        jacobian_inv = np.linalg.pinv(jacobian)
        current_cord = fk_5d.fk()[:, 3][:-1]

        theta_dots = jacobian_inv.dot([
            [x_dst],
            [y_dst],
            [z_dst],
        ])

        PT[0][0] += theta_dots.item(0) / 100
        PT[1][0] += theta_dots.item(1) / 100
        PT[2][0] += theta_dots.item(2) / 100
        PT[3][0] += theta_dots.item(3) / 100
        PT[4][0] += theta_dots.item(4) / 100
</code>```

sorry for the bad indentation, I do now know how to do it.

thank you very much in advance


问:
hand-eye calibration for wheeled mobile robots
I am new to robotics and working with a wheeled mobile robot with one Kinect mounted on top of it currently.

The robot knows its position in the environment via some means.
I encounter one problem. For example there is one table, and The Kinect is
expected to guide the robot to one specific side of the table. The problem is known as  pose estimation.

By browsing the internet, I guess that hand-eye calibration may be required. However, most materials about hand-eye calibration is about robotic arms.
I am not sure whether hand-eye calibration is needed for wheeled mobile robots.
I can't find much information about hand-eye calibration for wheeled mobile robots.
Any help are appreciated.


问:
Tracking object in realtime using depth image stream from a RGBD camera
I have an Intel RealSense camera which can stream RGB and depth image. Using those information I want to track an object moving on a table top. More specifically, I want to know position $(x,y)$ and orientation $\theta_z$ of the object. Can any body help providing me the pipeline for accomplishing what I mentioned? Thank you in advance.


问:
Can I combine BEAM robotics with fuzzy control?
Beam robotics invented by Mark W. Tilden is about analog circuits for controlling robots. The idea is to avoid microcontrollers and software and use instead resistors, breadboards and capacitor for controlling a line following robot. The problem with BEAM robots is, that they don't provide complex features and it's not possible to build walking robots out of analog circuits, because dynamic walking is non-linear problem which has to be treated with digital computers and algorithms.

On the other hand, there is a topic in the Artificial Intelligence domain available called Fuzzy logic which is based on continuous signals. A value like “left” can become -2, -1, 0 and also numbers with fractions. This makes it a natural choice for simulating fuzzy logic with analog circuits. Some papers are available which are explaining how to do this in detail for example “Alfredo Sanz: Analog Implementation of Fuzzy Controller, 1994“ which demonstrated a working fuzzy logic controller in the SPICE software.

Is it possible to combine BEAM robotics and fuzzy control to build analog intelligent robots?


问:
Stereo Rectification when Extrinsics are known
Suppose I have two cameras $C_1$ and $C_2$ arranged in a stereo configuration. Let us assume that the camera is free of distortion with ideal intrinsics and that the camera extrinsics $R$ and $t$ are known. Let  $C_1$ be the reference for the extrinsics of the system (i.e $R$ and $t$ are defined w.r.t to $C_1$). The two cameras have different focal lengths $f_{c_1}$ and $f_{c_2}$

Now, in order to calculate disparity given the images $I_1$ and $I_2$ of the cameras $C_1$ and $C_2$, it is necessary that the images need to be rectified.

I have understood rectification as making the the images captured lie in the same plane in space. The baseline of the stereo $b$ is equal to the norm of the translation vector $t$. Let ($x_2$, $y_2$) be a pixel in the Image $I_2$ and ($x_1$, $y_1$) a pixel in $I_1$. I would like to rectify Image $I_1$ such that the it lies in the same plane as $I_2$.

Let
$$P_1 = \begin{pmatrix}{x_1} \\{y_1} \\f_{c_1}\end{pmatrix}$$ and $$P_2 = \begin{pmatrix}{x_2} \\{y_2} \\f_{c_2}\end{pmatrix}$$
Would it be correct to say a pixel $P_2$ in $I_2$ is given by:

$$P_2 = \frac{f_{c_2}} {f_{c_1}}.(R.P_1 + t)$$

where $P_1$ is a pixel in $I_1$? We could then extend this and write
$$ P_1 = R^T.(\frac{f_{c_1}} {f_{c_2}}P_2 - t)$$
and that
$$P_{1_{rectified}} = R^T.(\frac{f_{c_1}} {f_{c_2}}P_2 - t) + t $$ since $t$ defines the baseline in the stereo system

Would the difference between $P_{1_{rectified}}$ and $P_2$ for all points $(x2, y2)$ in $P_2$ give me disparity? If not what would be the correct way to calculate disparity given the camera extrinsics and two cameras of different focal lengths. I have calculated the above expressions from my understanding of the geometry of the system. Please correct me if there are errors.


问:
Stereo-Calibration with Variable focal cameras
Is it possible to perform stereo-calibration with cameras having variable focus? and if so,  how is it done basically?</p>


问:
Defining C-space state for an arm in OMPL
I'm very new to Robotics, so please bear with my ignorance about the subject. That said, I'm trying to use Open Motion Planning Library (OMPL) to set up the configuration space for a robotic arm modeled after the human arm. In my model, the shoulder has 3 DOFs for translation and 3 DOFs for rotation. The elbow has only 1 DOF in rotation.

What would the correct way to setup the state space in OMPL? I can think of the following ways:

Create a compound state space and add $SE(3)$ subspace (for shoulder translation and rotation) and $SO(2)$ subspace for elbow rotation.

Create a real vector state space $R^7$.

My confusion comes from the fact that since different dimensions of my C state space will have different bounds then, correct me if I'm wrong, shouldn't those be normalized to a common range before distance computations? Otherwise, they'll contribute disproportionately to the distance. If OMPL does this automatically, will it do this for both $R^7$ as well as the compound state space?

Another source of confusion is that one also needs to assign weights to individual subspaces in the compound state space. What's the rationale for having weights? Is it just to prioritize some motions over the others?


问:
Path Planning for Spherical Wrist (RRR)
I have found the inverse kinematics of a spherical wrist. The wrist is not attached to any other type of arm, it is the sole controller. Based on this IK solution, how can I generate a path plan for my robot?

As far as I understand, its motion is restricted to a spherical surface.

I have seen examples for path planning for linear motion using parametric equations, but not for spherical motion.

Note that this is the Forward Jacobian

The J-1 Solution from MATLAB is written below.

```<code>    J_inv =

[                                                                                (cos(theta1)*cos(theta2))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2),                                                                                (cos(theta2)*sin(theta1))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2),                                                                                1]
[                                                                                (sin(conj(theta1))*(cos(conj(theta1))*cos(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta1)*cos(theta2) + sin(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta1))*(sin(conj(theta1))*cos(theta1) + cos(conj(theta2))*sin(conj(theta1))*cos(theta1)*cos(theta2) - cos(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2),                                                                                (sin(conj(theta1))*(cos(conj(theta1))*sin(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta2)*sin(theta1) - sin(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta1))*(sin(conj(theta1))*sin(theta1) + cos(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2) + cos(conj(theta2))*sin(conj(theta1))*cos(theta2)*sin(theta1)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2),                                                                                0]
[ (cos(conj(theta1))*sin(conj(theta2))*(cos(conj(theta1))*cos(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta1)*cos(theta2) + sin(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta2))*cos(theta1)*cos(theta2))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2) + (sin(conj(theta1))*sin(conj(theta2))*(sin(conj(theta1))*cos(theta1) + cos(conj(theta2))*sin(conj(theta1))*cos(theta1)*cos(theta2) - cos(conj(theta1))*sin(conj(theta2))*sin(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2), (cos(conj(theta1))*sin(conj(theta2))*(cos(conj(theta1))*sin(theta1) + cos(conj(theta1))*cos(conj(theta2))*cos(theta2)*sin(theta1) - sin(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2) - (cos(conj(theta2))*cos(theta2)*sin(theta1))/(sin(theta2)*cos(theta1)^2 + sin(theta2)*sin(theta1)^2) + (sin(conj(theta1))*sin(conj(theta2))*(sin(conj(theta1))*sin(theta1) + cos(conj(theta1))*sin(conj(theta2))*cos(theta1)*sin(theta2) + cos(conj(theta2))*sin(conj(theta1))*cos(theta2)*sin(theta1)))/(sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*cos(conj(theta1))^2*sin(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*cos(theta1)^2 + sin(conj(theta2))*sin(theta2)*sin(conj(theta1))^2*sin(theta1)^2), (cos(conj(theta1))^2*cos(conj(theta2))*sin(conj(theta2)))/(sin(conj(theta2))*cos(conj(theta1))^2 + sin(conj(theta2))*sin(conj(theta1))^2) - cos(conj(theta2)) + (cos(conj(theta2))*sin(conj(theta1))^2*sin(conj(theta2)))/(sin(conj(theta2))*cos(conj(theta1))^2 + sin(conj(theta2))*sin(conj(theta1))^2)]

>>
</code>```

Title: Path Planning for Spherical Wrist (RRR)


问:
Compile webots for Python 3.6 for Windows
I would appreciate if someone could assist me in compiling webots source for Python 3.6 to allow me and others to use TensorFlow with it. I already tried but apparently there is something not mentioned about a missing step. Thanks </p>

<p>Link:
https://github.com/omichel/webots/issues/105 </p>


问:
How to calculate observation matrix Jacobians in EFK based mono slam? What are projection Jacobians?
I have been trying to understand an implementation of monoSLAM 1. But my question is generic to EKF based single camera SLAM.

My Jacobian calculation leads to complex and lengthy expressions while the c++ implementation [3] seems to have done it very easily with what they call 'projection Jacobian' and its inverse. I don't understand the math involved and why they are calculated. Please point out where my understanding is wrong.

The following is my current understanding of the subject of VSLAM using EKF.

In monoslam, the 3D landmarks are measured indirectly using image coordinates as we do not get the depth information from a single camera. They are either parametrized as 3D points $L$ ($L_x, L_y, L_z$) or as 6D inverse depth formulation $L_{inv}$ ($C_x, C_y, C_z, \theta, \phi, \rho $). In EKF, the non-linear function $h()$ that converts the state vector into image measurements of landmarks involves the 3D to 2D projection equations (Camera model). The formula for converting a 3D point or a inverse depth formulation involves camera position, orientation, camera parameters and landmark representations $L$ or $L_{inv}$.

I have difficulty in calculating the Jacobians of the $h()$ function as part of the update equations. I tried calculating the partial differentiation of $h()$ with respect to the different elements of my state vector (used matlab symbolic tool) and got lengthy Jacobians.

The SLAM paper uses the projection model and calculates the Jacobians as mentioned in the section 2.5 of [2]. With respect to what the partial differentiation is done? To my understanding they have calculated w.r.t the image coordinates and not the elements from the state vector.

Hope my write up is clear.

References:

Russo, Ludovico, et al. "A ROS implementation of the mono-slam
algorithm" International Journal of Computer Science & Information
Technology 6.1 (2014): 339-351

https://github.com/rrg-polito/mono-slam/blob/master/reference/Master%20Thesis%20Ludovico%20Russo%20-%20oneside.pdf

https://github.com/rrg-polito/mono-slam


问:
Problem with UKF weights when calculating predicted state
I'm having troubles implementing a UKF during the calculation of $\overline{\mu_t}$, specifically in this step:
$$\overline{\mu_t} = \sum_{i=0}^{2n}w_m^{[i]}\chi_t^{*[i]}$$

My problem I'm facing is that $\overline{\mu_t}$ seems to not be close to the values of the sigma points at all, and I have no idea why.

My state is composed of $x, y, v, a, \theta, \dot{\theta}$. Here is one example of an iteration:
$$\mu_{t-1} = \begin{bmatrix}2.7 \\ 8.3e-05 \\ 1 \\ 0.2 \\ 3.2e-15 \\ 1e-9 \end{bmatrix}\\
\Sigma_{t-1} = \begin{bmatrix}
   0.038&  7.6e-07&    0.013&   0.0018& -1.7e-09&  1.5e-11 \\
 7.6e-07&     0.99&    4e-07&  1.5e-07&        1&    8e-10 \\
   0.013&    4e-07&    0.005&  0.00095&  1.1e-17&  4.7e-12 \\
  0.00187&  1.5e-07&  0.00095&  0.00036& -1.8e-15&  5.7e-13 \\
-1.7e-09&        1&  1.1e-17& -1.8e-15&      1.1& -1.9e-23 \\
 1.5e-11&    8e-10&  4.7e-12&  5.7e-13& -1.9e-23&    1e-08 \\
\end{bmatrix}$$

The $x$ values of the sigma points after the transition model:
\begin{bmatrix}
     2.82606 &     2.85113   &    2.8251   &   2.82614   &   2.82606      2.82603 &     2.82606    &    2.801  &     2.8251 &     2.82599  &    2.82606   &  2.82603   &   2.82606
\end{bmatrix}

For my weights, I chose $\alpha$ such that weights would be integers, since I was worried that this was a floating point error:
$$w_m^{[i]} = \begin{bmatrix}-384\\32\\32\\32\\32\\32\\32\\32\\32\\32\\32\\32\\32\end{bmatrix}$$

However, calculating $\overline{\mu_t}$ yields $-0.064$, which is evidently wrong. Wolfram alpha confirms that this is not a floating point error. I have no idea where I might be going in the calculation here.

Any idea how I would go about solving this problem?


问:
L298N briefly works, then shorts when controlling Nema 17
I'm trying to control a Nema 17 stepper motor with a L298N H-Bridge.

My wiring is as below (except I'm using a ESP8266 not a YUN, and I have a mains 12V supply rather than batteries). This is consistent with other tutorials I have found online.

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

const int stepsPerRevolution = 400;  // change this to fit the number of steps per revolution
// for your motor

// initialize the stepper library on pins 8 through 11:
Stepper myStepper(stepsPerRevolution, D1, D2, D3, D4);

void setup() {
  // set the speed at 60 rpm:
  myStepper.setSpeed(1);
  // initialize the serial port:
  Serial.begin(9600);
}

void loop() {
  // step one revolution  in one direction:
  Serial.println("clockwise");
  myStepper.step(stepsPerRevolution);
  delay(500);

  // step one revolution in the other direction:
  Serial.println("counterclockwise");
  myStepper.step(-stepsPerRevolution);
  delay(500);
}
</code>```

The issue I am having is that the L298N has it's RED LED on, until I actually try to move the stepper motor. As soon as I try to move it a single step the LED turns off and it takes a while for the board to come back online. I believe this could be an issue with the motor drawing too much current but I have no clue how to fix this.

Cheers in advance!


问:
How to handle sensor data for a sensor-fusion algorithm
I am implementing the explicit complementary filter(see below) with a 9DOF MEMS sensors (accelerometer, magnetometer and gyroscope) for attitude estimation.

Currently, the gyro and accel update rates are at ~1kHz and I am running my filter at 100Hz. The magnetometer refresh rate is running at ~20hz.

The constraints are that I can't modify the sensors reading rate and the algorithm can't be run at more than 400Hz.

As it is implemented right now, I read the gyro and accel if available, optionally the magnetic field if new data is available and run the maths, trying to keep my update rate at 100Hz.

But in addition to the poor performance I observe, it seems the current implementation is not a good way to handle the sensors reading.

From what I read, it seems there are more efficient way to handle the data:

Running my filter at 400Hz where I run a buffer which accumulates gyro and accel samples, averages them and use it every 100Hz to update my filter (with the risk to increase bias and noise) ?

Running a low pass filter @400Hz on the raw data and update my ECF every 10ms ?

A combination of the two previous solutions ?

ECF Algorithm:

$$
\dot{\hat{q}} = \frac{1}{2} \hat{q}\otimes p(\Omega_{y}-\hat{b} + k_{P}\omega_{meas} ).
$$

$$
\dot{\hat{b}} = -k_{I}\omega_{meas}.
$$

where $\otimes$ is the quaternion cross operator, $w_{meas}$ the sum of cross product between accel and mag reading with respectively their estimates. $\Omega_{y}$ is the gyro measurements. $k_I$ and $k_P$ are the filter gains.

[EDIT]

The chosen strategy was to accumulate and compute the mean from sensors data as the quaternion propagation step is considering these values being constants. After the update step, the buffer is emptied.


问:
How to quantify motion?
I am working on a calibration algorithm where the motion is related to the observability of the parameters that I am calibrating.

Let's say we have a 6 dof trajectory and wants to quantify its motion to see how dynamically it has moved. (e.g, to see if it is covering a wide angle in rpy space.)

For example to quantify the SO(3) we can convert it into so(3) then apply PCA to decompose its covariance to see the scatterness along the principal axises. A widely spread motion will give more observability on the parameter calibration.

Is there any reference paper or document for this problem?

Title: How to quantify motion?


问:
Choosing a position sensor (encoder) and obtaining velocity from it
How do I know how many counts per revolution I need for an encoder for my application?

I need to be able to accurately capture a 1000 Hz signal on an input side running from 1-133 rev/sec (60-8000 RPM) and a 1000 Hz signal on an output side with gear ratio 1:100, so speeds are .01-1.33 rev/sec (.6 - 80 RPM).

Here is the math I have but am not sure if it is correct:

For a 1000 Hz signal, it takes .001 sec for one period. But if I want to accurately capture this and accurately obtain velocity from this position data, I need 100 counts (is this a good assumption? what would be a better value?). Hence, I need 100/.001 = 100,000 cts/sec. Now, if I divide this by my rev/sec, I'll get cts/rev which is what encoders are rated at. Assuming input is running at 10 rev/sec, I need 100,000/10 = 10,000 cts/rev. Assuming output is running at .1 rev/sec, I need 1,000,000 cts/rev.

I strongly suspect my math is incorrect however because the best encoders that meet the speed requirement of my application are 1024 cts/rev for input and 72,000 cts/rev for output. Would these cts/rev actually be good enough for my application then?

For context, I need to obtain the Bode plot of a gear drive and hence plan to give sinusoidal inputs to my motor. I do not expect this the bandwidth of my gear drive to be more than 1000 Hz, but since I am not sure what it's exact number is, I would like my design to be capable of accurately measuring a 1000 Hz signal. This sinusoid will be in terms of velocity which is why I want to obtain velocity data. Also, I know it may seem odd that I am using position first and then getting velocity rather than getting velocity directly, but I want this system to be capable of positional control so I would like to only use position sensors if possible.

TLDR: How many position counts are needed to get an accurate velocity reading? How many position counts per revolution do you need to capture velocity changing at a frequency of 1000 Hz?


问:
Overcoming or managing deadband in vehicle steering controller
I found out there is a deadband in my steering controller after several runs. And the transfer function for it in the \$s\$-domain is
$$
H(s)=\frac{270\cdot e^{-0.03s}}{0.015\cdot s + 1}
$$
To overcome this deadband, should I introduce a compensator ? My control theory background is weak so am not sure of a proper way to manage the deadband.


问:
Overcoming or managing dead time in vehicle steering controller
I found out there is a dead time in my steering controller after several runs. The transfer function for it in the $s$-domain is:

$$
H(s)=\frac{270 \cdot e^{-0.03s}}{0.015 \cdot s + 1}
$$

To overcome this dead time, should I introduce a compensator? My control theory background is weak so I am unaware of a proper way to manage the dead time.

OwnerDisplayName: djack


问:
robot Hand Eye Calibration problem
I am trying to understand the implementations and the relative performance of various classical and new Hand-eye calibration approaches.

Note: The following discussion assumes implementations are in Matlab for the AX=XB hand-eye calibration approach.

I have been able to successfully implement Wengerts and Lazax implementation of Tsia approach. These open-source implementations take in the gripper and the camera poses and further performs the specific operations.
However, I am facing trouble understanding Milli Shah's implementation of Tsai and Liangs method. The implementation takes in AA (for gripper) and BB (for the camera), which are definitely not the relative (i to j) gripper and camera poses but some processed version of these.

Can anybody please explain this AA and BB in more detail. In case somebody has an extraction code for this, it would be helpful
Thanks

Title: robot Hand Eye Calibration problem


问:
Can a RPI3B+ handle all this?
I am building a robot that needs to accomplish the following tasks:

Keep track of 4 quadrotor encoders, each 979.62 CPR @ ~ 500RPM, so ~ 8163.5 counts per second

Read from an RPlidar A1M8, giving out 8000 samples per second (each sample containing distance, heading, a quality rating of the reading, and a boolean that is true if the device is starting a new revolution).

Simon Levy's "Breezy SLAM" algorithm

Read from a PixyCam's object recognition outputs @ 50Hz

Read from gyro, 60Hz

Read from 4 color sensors, 4 ultrasonic sensors, and a TOF sensor, each at around 10Hz

Fusing above data to get accurate 2D position information, as well as positions of obstacles, within a room of known dimensions (some of the sensors are for other desired pieces of information that I won't go into).  (I'm thinking of going with a Kalman filter - any thoughts on this?)

Running a task-accomplishing algorithm (possibly a premade Q-learning model)-- for instance, drive towards the nearest obstacle (though my end goal is much more sophisticated than this).

I am wondering if a Raspberry Pi 3B+ could handle all this?  If not what are my options?  Do I need a more powerful computer?  I am thinking I could probably offload the encoder counting to a separate board, and am wondering if there are dedicated ICs for this.

Title: Can a RPI3B+ handle all this?


问:
Rigid body system identification with machine learning, but how?
Most problems in robotics have to do with a controlling a system. An easy example for a system is the forward kinematic of a robot arm. A joint in the model gets a certain value, and as the result the arm is moving to a new position in space. More complex problems are visible if a grasping task gets simulated. Here is the problem, that objects are in the system which can't moved by it's own, but need to be pushed. It's a typical example for an underactuated non-linear system.

A system which contains rigid body dynamics has a certain behavior. A box is colliding with another box and as a result they are changing their position in 3d space. Simulating a rigid body system in realtime is called a physics engine. And creating such an engine without having access to the original one is called system identification.

Machine learning is the natural choice in doing so. Because machine learning is datadriven and it's able is reproduce the behavior of a non-linear system. In the training dataset, all the values are stored which are recorded by the sensors. That is the position of the rigid body boxes, their angle and the forces applied to them. But how exactly can machine learning be utilized for system identification of such system? Is a multi-layer-perceptron the right choice, or is the LSTM network with it's ability to store information about the past the only option in doing so?


问:
Sensor model and Inverse sensor model using occupancy grid mapping with lidar for particle filter
I'm a bit confused about how one goes about calculating the sensor model $p(z_t|x_t, m) $ and inverse sensor model for position $p(x_t |z_t,m_{t-1})$.

From this answer, it seems like one way for calculating the inverse sensor model would be to compare the current lidar scan with the raycasted lidar scan, and then comparing them by using ICP for example.

For the sensor model $p(z_t|x_t, m)$, I've just been taking a product of the probabilities from the map, but I know that this is an incorrect method.

What are the standard ways of computing $p(z_t|x_t, m)$ and $p(x_t|z_t, m_{t-1})$ when using a lidar with an occupancy grid for a particle filter?


问:
Tetrix Prizm and ROBOTC
I am currently trying to find if its possible to program a Tetrix Prizm motor controller with ROBOTC? Because I have an EV3 brain connected with the Prizm to a robot assembly with two motors, and I was searching and here it says it can use Arduino.  </p>

Title: Tetrix Prizm and ROBOTC


问:
How to use MoveIt and ros_control (from setup assistant) to control Gazebo robot?
I've noticed that in the MoveIt setup assistant, there is a "ROS Control" that auto-generates 2 files:
- ros_controllers.yaml
- ros_controllers.launch

I wanted to ask if it was possible to use these files to control my Gazebo robot? How can I do this?

The robot arm I am working with is https://github.com/Interbotix/widowx_arm/blob/master/widowx_arm_description/urdf/widowx.urdf.xacro


问:
Max angle of beam against surface (ultrasonic sensor)
I am trying to localize a robot within a rectangular room (182cm x 243cm) with completely black walls (so IR sensors and LIDAR are out of the question). I am trying to figure out if I can position enough ultrasonic sensors around my robot so that I can calculate the distance to each wall and the orientation of the robot at any heading or position within the room (assuming the bot does not get too close to any of the walls).

What is the maximum angle at which an ultrasonic sensor can reflect against a wall and still get an accurate reading?  I am using these sensors specifically, which have 1mm resolution:

Maxbotix HR-LV-MaxSonar-EZ4 High Performance Sonar Module

Any other tips on localization in this kind of environment?


问:
Kalman filter to fuse ultrasonic altitude sensor and accelerometer
I'm a little confused on how to set up the Kalman filter matrices to fuse an ultrasonic altitude sensor and accelerometer from IMU. I am trying to implement this for a quadcopter. And what I'm confused about specifically is when the quadcopter is tilted ($\phi \neq 0, \theta \neq 0$). Do I just simply rotate the z-accelerometer, $a_z$, measurement by $\cos(\phi) \cos(\theta)$? As such:

\begin{equation*}
\begin{matrix}
\begin{bmatrix}
z\\ w
\end{bmatrix}_{k+1}
=
\begin{bmatrix}
1 & \Delta t \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
z \\
w
\end{bmatrix}_{k}
\end{matrix}
+
\begin{bmatrix}
\frac{1}{2} \Delta t^2\\
\Delta t
\end{bmatrix}
(a_z \cos\phi \cos\theta)
\end{equation*}

Or just keep it as $a_z$?

Also how would my observation matrix H take into the account the roll/pitch angles? Since the ultrasonic is not pointing directly downwards.

I've seen implementations where they do not include the cosines, but I'm not sure if that's because they assume $\cos(x)$ is ~1 for small angles, which would not be valid for my quad (>30deg maneuvers).


问:
TGY-6114MD Servo motor needs push to start
I currently have an inspection robot deployed in a remote part of Africa that has a failing Turnigy TGY-6114MD servo. The issue appeared randomly. The servo won't respond to commands unless given a slight push or nudge and then it will move to it's commanded position. We thought there may be some binding in the gearbox. After removing the gearbox and the gears we found that the motor shaft itself needs a slight push to start spinning. So with no load on the motor at all it still won't start unless given a slight push.

Does anyone know what may be causing this behavior? We are unfortunately unable to get a replacement part in a reasonable amount of time so any repair ideas are welcome!

EDIT: Update. We measured the voltage across the motor solder joints during operation. When the motor is turning (with no physical load) the voltage is 4.98V. When the motor stops, the voltage drops to 1.4V. On an identical working servo on the robot, the voltage is 0V when the motor stops. What would cause the 1.4V at the motor when stopped?


问:
Servo LIneSensor conflict
We are using a Prizm Genuino controller. We have been using a line sensor to successfully detect black lines but we seem to be getting a conflict with our servos now.  The lineSensor does not seem to be giving us a reading.  We think this has also happened with our ultraSonic sensor. Hoping someone can point us in the right direction.</p>

Title: Servo LIneSensor conflict


问:
Have you done take-home robotics(ROS) coding test in interviews?
Have you done take-home robotics(ROS) coding test in interviews?

Do you want to share the coding test instructions with me? or point me in the right direction where I can find some to practice?

I'm looking for something similar to this:

https://github.com/andrestoga/take_home_robotics_coding_test

. Those are the tests I've done so far.

Thank you very much!


问:
Can't get iRobot Create to respond
We have an iRobot Create connected to a Win 10 PC with the cable supplied with the unit.
We have followed the instructions here:
http://www.robotappstore.com/Knowledge-Base/How-to-Use-a-RealTerm-Terminal/16.html
and were unable to get any response except for an occasional single beep when sending a numeric 7 and very rarely when sending 128 132. The remaining examples (and other codes) get no response. The com port the iRobot is connected to is showing VCP, baud rate 115200. (We modified the serial port driver to use the FTDI driver, no impact to the iRobot responsiveness.) - other than that, the real term port configuration is exactly as the example shows

Can anyone provide any insight

Title: Can't get iRobot Create to respond


问:
Maze wall hugging both clockwise and anti-clockwise
So I did the algorithm for the vehicle to go in a clockwise direction(Refer to my pseudocodes below). But now that my school had said there will be an anti-clockwise direction as well, I'm not sure what to do. Do I only reverse the logic of my pseudocode below? Do I use switch cases as well? I'm not sure. So the vehicle must be able to move both clockwise and anti-clockwise around the 'maze'

```<code>/* Check if the left sensor is blocked*/
if(left sensor ==true )
{
    if(Obstacle detected)
    {
        move_forward;
    }
}
/* Check if the back-left sensor is blocked*/
if(back-left sensor ==true)
{
    if(Obstacle detected)
    {
        move_diagonal_front_left;
    }
}

/* Check if the front-left sensor is blocked*/
if(front-left sensor== true )
{
    if(Obstacle detected)
    {
        move_diagonal_front_right;
    }

}

/* Check if the front sensor is blocked*/
if(front sensor== true )
{
    if(Obstacle detected)
    {
        move_right;
    }

}

/* Check if the front-right sensor is blocked*/
if(front-right sensor== true )
{
    if(Obstacle detected)
    {
        move_diagonal_back_right;
    }

}

/* Check if the right sensor is blocked*/
if(right sensor== true )
{
    if(Obstacle detected)
    {
        move_back;
    }
}

/* Check if the back-right sensor is blocked*/
if(back-right sensor== true )
{
    if(Obstacle detected)
    {
        move_diagonal_back_left;
    }
    else if(Obstacle cleared)
    {
        Move_backwards;
    }
}
</code>```