问:
How can i consider orientation of 6dof manipulator for inverse kinematic?
i been trying to code IK solver for few days, and i finally move the robot to the desired position.
However im having hard time to orient the end-effector as i want it.

I read the "Robotics Modeling, Planning control -Siciliano" and few simillar question&answers.

As i understand, there are two ways.

Use analytic Jacobian to map the $ \dot{p}  =[v_x, v_y, v_z, w_x, w_y, w_z]'$ to $ \dot{p}'  =[v_x, v_y, v_z, \dot{\phi}, \dot{\theta}, \dot{\psi}]'$

using $R_d=[n_d, s_d, a_d], R_e=[n_e, s_e, a_e]$ rotation matrix of the end-effector and desired rotation matrix, find orientation error. (angle-axis method maybe?)

I tried first method since I didn't really understand the second method.
as far as i know analytic Jacobian can be obtained by

\begin{equation*}
J_a(q) =
\begin{bmatrix}
I & 0 \\
0 & B^{-1}(r,p,y) \\
\end{bmatrix}*J(q)
\end{equation*}

where B matrix is
\begin{equation*}
B =
\begin{bmatrix}
-sin(r)cos(p)/sin(p) & cos(r)cos(p)/sin(p) & 1\\
cos(r) & sin(r) & 0 \\
sin(r)/sin(p) & -cos(r)/sin(p) & 0
\end{bmatrix}
\end{equation*}

With this analytic jacobian i tried to find
\begin{equation*} \dot{q}_{1-6} = Ja(q)^{-1}\dot{p}'\end{equation*}
Considering there is no problem on getting jacobian. I will briefly explain my code in case there is something i am missing.

For the desired pos, orientation i gave x,y,z,r,p,yaw desire value with initial joint anlge.
and I find roll,pitch,yaw value for each iteration to compare with desired value.
I make geometric jacobian and analytic jacobian to get a thetadot by multiplying pseudoinvser analytic jacobian with [x,y,z,roll,pitch,yaw] and check if position and r,p,y values are converges. Also update theat value with thetaddot value.

```<code>error_X = Xdesire - Xcurrent;
error_Y = Ydesire - Ycurrent;
error_Z = Zdesire - Zcurrent;
error_Roll = Rolldesire - Rollcurrent;
error_Pitch = Pitchdesire - Pitchcurrent;
error_Yaw = Yawdesire - Yawcurrent;
</code>```

For the error part, position error always convserge but not r,p,yaw error. Maybe problem here?
Everything seems okay for me but orientation doesn't considered.
Am i doing something wrong or is there something i am missing?

Mycode is based on the this work.
https://kr.mathworks.com/matlabcentral/fileexchange/61380-3dof-inverse-kinematics-pseudoinverse-jacobian?s_tid=prof_contriblnk

I added my code if ther e is anyone who is kind enough to review it, i would really thankful. It doesn't requires any function so if you have matlab u can just run it.

thx for reading a long question. If there is any unclear things let me know!

```<code>clc
clear
close

goal_pos =[575 50 425 0 90 0]; % x, y, z, roll, pitch, yaw
init_ang =[10 20 5 10 10 10];

theta1=init_ang(1);  d1=0;   a1=50;  alpha1=-90; %parameter link1
theta2=init_ang(2);  d2=50;   a2=425;  alpha2=0; %parameter link2
theta3=init_ang(3);  d3=0;  a3=0;  alpha3=90; %parameter link3
theta4=init_ang(4);  d4=425;   a4=0;  alpha4=-90; %parameter link4
theta5=init_ang(5);  d5=0; a5=0;  alpha5=90; %parameter link5
theta6=init_ang(6);  d6=100; a6=0;  alpha6=0; %parameter link6
offset2 = -90;
offset3 = 90;
T01=[cosd(theta1) -cosd(alpha1)*sind(theta1) sind(alpha1)*sind(theta1) a1*cosd(theta1);
    sind(theta1) cosd(alpha1)*cosd(theta1)  -sind(alpha1)*cosd(theta1) a1*sind(theta1);
    0,sind(alpha1),cosd(alpha1),d1;
    0,0,0,1];
T12=[cosd(theta2+offset2) -cosd(alpha2)*sind(theta2+offset2) sind(alpha2)*sind(theta2+offset2) a2*cosd(theta2+offset2);
    sind(theta2+offset2) cosd(alpha2)*cosd(theta2+offset2)  -sind(alpha2)*cosd(theta2+offset2) a2*sind(theta2+offset2);
    0,sind(alpha2),cosd(alpha2),d2;
    0,0,0,1];
T23=[cosd(theta3+offset3) -cosd(alpha3)*sind(theta3+offset3) sind(alpha3)*sind(theta3+offset3) a3*cosd(theta3+offset3);
    sind(theta3+offset3) cosd(alpha3)*cosd(theta3+offset3)  -sind(alpha3)*cosd(theta3+offset3) a3*sind(theta3+offset3);
    0,sind(alpha3),cosd(alpha3),d3;
    0,0,0,1];
T34=[cosd(theta4) -cosd(alpha4)*sind(theta4) sind(alpha4)*sind(theta4) a4*cosd(theta4);
     sind(theta4) cosd(alpha4)*cosd(theta4)  -sind(alpha4)*cosd(theta4) a4*sind(theta4);
     0,sind(alpha4),cosd(alpha4),d4;
     0,0,0,1];
T45=[cosd(theta5) -cosd(alpha5)*sind(theta5) sind(alpha5)*sind(theta5) a5*cosd(theta5);
     sind(theta5) cosd(alpha5)*cosd(theta5)  -sind(alpha5)*cosd(theta5) a5*sind(theta5);
     0,sind(alpha5),cosd(alpha5),d5;
     0,0,0,1];
T56=[cosd(theta6) -cosd(alpha6)*sind(theta6) sind(alpha6)*sind(theta6) a6*cosd(theta6);
     sind(theta6) cosd(alpha6)*cosd(theta6)  -sind(alpha6)*cosd(theta6) a6*sind(theta6);
     0,sind(alpha6),cosd(alpha6),d6;
     0,0,0,1];
T01;
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
T05 = T01*T12*T23*T34*T45;
T06 = T01*T12*T23*T34*T45*T56;
%%%%%                      POSITION                %%%%%%%%%%%%%%%
ne = T06(1:3,1);
se = T06(1:3,2);
ae = T06(1:3,3);
Re = [ne se ae] % orientation of ee
roll = rad2deg(atan2(ne(2), ne(1)));
pitch = rad2deg(atan2(-ne(3),sqrt(se(3)^2+ae(3)^2)));
yaw = rad2deg(atan2(se(3),ae(3)));

%R = inv(R03')*R36;
disp("init ee");
INIT_EE = T06;
% DH paramter

Xc=goal_pos(1);Yc=goal_pos(2);Zc=goal_pos(3);
r_e=goal_pos(4);p_e=goal_pos(5);yaw_e=goal_pos(6);
% init pos and orientation(Euler)

iteration = 0;
b = 0 ;
deltatheta1=0;deltatheta2=0; deltatheta3=0; %theta1,2,3 velocity
deltatheta4=0;deltatheta5=0; deltatheta6=0; %theta4,5,6 velocity
while b == 0
%     result = [theta1 theta2 theta3 theta4 theta5 theta6];
%     disp("result"); disp(result);
    theta1=theta1+deltatheta1/2;
    theta2=theta2+deltatheta2/2;
    theta3=theta3+deltatheta3/2;
    theta4=theta4+deltatheta4/2;
    theta5=theta5+deltatheta5/2;
    theta6=theta6+deltatheta6/2;
    result = [deg2rad(theta1) deg2rad(theta2) deg2rad(theta3) deg2rad(theta4) deg2rad(theta5) deg2rad(theta6)];
    if(iteration > 1000)
        % avoid too many calculation
        b=1;
        iteration = 0;
    end
    T01=[cosd(theta1) -cosd(alpha1)*sind(theta1) sind(alpha1)*sind(theta1) a1*cosd(theta1);
        sind(theta1) cosd(alpha1)*cosd(theta1)  -sind(alpha1)*cosd(theta1) a1*sind(theta1);
        0,sind(alpha1),cosd(alpha1),d1;
        0,0,0,1];
    T12=[cosd(theta2+offset2) -cosd(alpha2)*sind(theta2+offset2) sind(alpha2)*sind(theta2+offset2) a2*cosd(theta2+offset2);
        sind(theta2+offset2) cosd(alpha2)*cosd(theta2+offset2)  -sind(alpha2)*cosd(theta2+offset2) a2*sind(theta2+offset2);
        0,sind(alpha2),cosd(alpha2),d2;
        0,0,0,1];
    T23=[cosd(theta3+offset3) -cosd(alpha3)*sind(theta3+offset3) sind(alpha3)*sind(theta3+offset3) a3*cosd(theta3+offset3);
        sind(theta3+offset3) cosd(alpha3)*cosd(theta3+offset3)  -sind(alpha3)*cosd(theta3+offset3) a3*sind(theta3+offset3);
        0,sind(alpha3),cosd(alpha3),d3;
        0,0,0,1];
    T34=[cosd(theta4) -cosd(alpha4)*sind(theta4) sind(alpha4)*sind(theta4) a4*cosd(theta4);
         sind(theta4) cosd(alpha4)*cosd(theta4)  -sind(alpha4)*cosd(theta4) a4*sind(theta4);
         0,sind(alpha4),cosd(alpha4),d4;
         0,0,0,1];
    T45=[cosd(theta5) -cosd(alpha5)*sind(theta5) sind(alpha5)*sind(theta5) a5*cosd(theta5);
         sind(theta5) cosd(alpha5)*cosd(theta5)  -sind(alpha5)*cosd(theta5) a5*sind(theta5);
         0,sind(alpha5),cosd(alpha5),d5;
         0,0,0,1];
    T56=[cosd(theta6) -cosd(alpha6)*sind(theta6) sind(alpha6)*sind(theta6) a6*cosd(theta6);
         sind(theta6) cosd(alpha6)*cosd(theta6)  -sind(alpha6)*cosd(theta6) a6*sind(theta6);
         0,sind(alpha6),cosd(alpha6),d6;
         0,0,0,1];
    T01;
    T02 = T01*T12;
    T03=T01*T12*T23;
    T04=T01*T12*T23*T34;
    T05=T01*T12*T23*T34*T45;
    T06=T01*T12*T23*T34*T45*T56;

    ne = T06(1:3,1);
    se = T06(1:3,2);
    ae = T06(1:3,3);
    Re = [ne se ae] % orientation of ee
    roll = rad2deg(atan2(ne(2), ne(1)));
    pitch = rad2deg(atan2(-ne(3),sqrt(se(3)^2+ae(3)^2)));
    yaw = rad2deg(atan2(se(3),ae(3)));

%     if theta6 < 90
%         theta6 = pi-theta6;
%     elseif theta6 > 90
%         theta6  = theta6;
%     end

    P0=[0 0 0];
    P1=transpose(T01(1:3,4));
    P2=transpose(T02(1:3,4));
    P3=transpose(T03(1:3,4));
    P4=transpose(T04(1:3,4));
    P5=transpose(T05(1:3,4));
    P6=transpose(T06(1:3,4));
    %%%%%%%%%%%%%%%%%%%%%%%
    Z0  = [0;0;1];Ori =[0;0;0]; O6=T06(1:3,4);
    Jv1 = cross(Z0,(O6-Ori));

    Z1  = T01(1:3,3);Ori_1=T01(1:3,4);
    Jv2 = cross(Z1,(O6-Ori_1));

    Z2  = T12(1:3,3);Ori_2=T12(1:3,4);
    Jv3 = cross(Z2,(O6-Ori_2));

    Z3  = T23(1:3,3);Ori_3=T23(1:3,4);
    Jv4 = cross(Z3,(O6-Ori_3));

    Z4  = T34(1:3,3);Ori_4=T34(1:3,4);
    Jv5 = cross(Z4,(O6-Ori_4));

    Z5  = T45(1:3,3);Ori_5=T45(1:3,4);
    Jv6 = cross(Z5,(O6-Ori_5));
    Jg=[Jv1 Jv2 Jv3 Jv4 Jv5 Jv6;
       Z0 Z1 Z2 Z3 Z4 Z5];

    B = [-sind(roll)*cosd(pitch)/sind(pitch) cosd(roll)*cosd(pitch)/sind(pitch) 1;
        cosd(roll) sind(roll) 0;
        sind(roll)/sind(pitch) -cosd(roll)/sind(pitch) 0];

    Ja =[eye(3) zeros(3);
        zeros(3) inv(B)] * Jg;
    Xinit=P6(1,1);
    Yinit=P6(1,2);
    Zinit = P6(1,3);
    Rinit=roll;
    Pinit=pitch;
    Yawinit = yaw;
    Xend=Xc;Yend=Yc; Zend = Zc;
    Rend=r_e; Pend=p_e; Yawend = yaw_e;
    Xspeed=(Xend-Xinit);
    Yspeed=(Yend-Yinit);
    Zspeed=(Zend-Zinit);
    Rspeed=(Rend-Rinit);
    Pspeed=(Pend-Pinit);
    Yawspeed=(Yawend-Yawinit);
    thetadot=pinv(Ja)*[Xspeed;Yspeed;Zspeed;Rspeed;Pspeed;Yawspeed];
    error_x = Xspeed^2;
    error_y = Yspeed^2;
    error_z = Zspeed^2;
    error_r = Rspeed^2;
    error_p = Pspeed^2;
    error_yaw = Yawspeed^2;

    if abs(error_x)<=0.2 && abs(error_y)<=0.2 && abs(error_z)<=0.2 && abs(error_r)<=0.5 && abs(error_p)<=0.5 && abs(error_yaw)<=0.5
        End_EE = T06;
        b=1;
    end
    theta1dot=thetadot(1,1);
    theta2dot=thetadot(2,1);
    theta3dot=thetadot(3,1);
    theta4dot=thetadot(4,1);
    theta5dot=thetadot(5,1);
    theta6dot=thetadot(6,1);

    deltatheta1=rad2deg(theta1dot);
    deltatheta2=rad2deg(theta2dot);
    deltatheta3=rad2deg(theta3dot);
    deltatheta4=rad2deg(theta4dot);
    deltatheta5=rad2deg(theta5dot);
    deltatheta6=rad2deg(theta6dot);
    Q1=[P0(1,1) P1(1,1) P2(1,1),P3(1,1),P4(1,1),P5(1,1)];
    Q2=[P0(1,2) P1(1,2) P2(1,2),P3(1,2),P4(1,2),P5(1,2)];
    Q3=[P0(1,3) P1(1,3) P2(1,3),P3(1,3),P4(1,3),P5(1,3)];
    plot3(Q1,Q2,Q3, '-o','LineWidth',4);
    axis([-700 700 -700 700 -700 700]);
    grid on
    pause(0.001);
    iteration = iteration +1;
end
</code>```


问:
Displaying a GPS Route (By Waypoints) on Mission Planner with Dronekit
Hope you are all doing well. I am working on a navigation project right now. According to a starting and goal location, I use Open Route Service to create a GPS Route. I have the coordinates (lateral and longitudinal) of the route (it has around 50 waypoints). Is there any way for me to add those coordinates to Mission Planner (Ardupilot) one by one? I mean I do not want an autonomous navigation. I just want to have the route showing up on mission planner. I tried creating a mission, but the vehicle (ArduRover) moves autonomously and I do not see the route on Mission Planner - by the way vehicle just moves forward, not following my route. For an example I will show you my code:

```<code>import openrouteservice
import folium
from openrouteservice.directions import directions
import json
import itertools
import pickle
import numpy as np
import matplotlib.pyplot as plt
from geopy.geocoders import Nominatim
from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, LocationGlobal
from pymavlink import mavutil

print('Connecting...')
vehicle = connect('tcp:127.0.0.1:5762')
vehicle.mode = VehicleMode("AUTO")

coords = ((vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.lat),(28.87180, 41.00560))
client = openrouteservice.Client(key='hidden for safety purposes') # Specify your personal API key

geometry = client.directions(coords)['routes'][0]['geometry']
decoded = openrouteservice.convert.decode_polyline(geometry)
coordsArray = np.zeros((len(decoded["coordinates"]), 2))
for i in range(len(decoded["coordinates"])):
    coordsArray[i][0] = decoded["coordinates"][i][1]
    coordsArray[i][1] = decoded["coordinates"][i][0]

cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
# Save the vehicle commands to a list
missionlist=[]
# Modify the mission as needed. For example, here we change the
# first waypoint into a MAV_CMD_NAV_TAKEOFF command.

for i in range(len(coordsArray)):
    cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
    mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,coordsArray[i, 1], coordsArray[i, 0], 100)

    missionlist.append(cmd)
# Araca yükleniyor...
missionlist[0].command=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF
cmds.clear()
for cmd in missionlist:
    cmds.add(cmd)
cmds.upload()
</code>```

This script results with the vehicle (rover) just going forward, without following a route. Also I cannot see the pre-calculated route on Mission Planner. Can you help me with that? The only thing I need is to show the route on Mission Planner.

ContentLicense: CC BY-SA 4.0


问:
Is there a way I can achieve Torque control from pulse control?
I am using the Contec SMC-8DL Motion control card to control the Oriental Stepper motors via the AZD-C driver. The Contec helper functions only allow for sending pulses to the driver, is there a way of instead of sending pulse I send the torque commands?

Best


问:
How to implement PID position control for 4wheel mecanum robot?
I want to implement PID control on my 4 wheel mecanun robot. I want to give a position instruction to my robot and that it follow the following curves of velocity and position. The only feedback that my robot have is an encoder on each wheel . How can I code this?


问:
Faulty GPS data on Serial Monitor
I am trying to interface Stonex S900A GPS with nVidia TX2 which has ROS melodic installed. The GPS data cable is a DB9 type (RS232). I connected a DB9 to USB converter with the cable and connected it to the USB port of TX2. I intend to use nmea_driver_navsat ros package to read the GPS data. However, when trying to monitor the GPS data on a serial monitor (such as puTTY or CuteCom), I am getting messages in the following format: \

 \

Could it be an issue with the serial driver? When I connect the same GPS with the same cable to another system, I get valid information on the serial monitor. What could be the issue here?

Title: Faulty GPS data on Serial Monitor


问:
Deduce all possible configuration for a given CONF param FANUC
I am working on the "CONF" parameter of FANUC Robots.

The "CONF" param determine the pose of the robot (maybe to have only one inverse kinematics solution). This configuration is based on 3 mains parameters : the position of the wrist (Flip/No-flip), the elbow (Up/Down)  and waist (Top/Bottom).

I want to find a way to develop a function which the purpose is to give in enter a CONF param (like NUT of FUT) and return all the possible angles configurations of the robots which satisfied this CONF param (n,6 array).

I have managed to find the min and max angles for J5 to satisfied the flip or no-flip CONF parameter (which is dependent of +/-180° on J4 and J6).

If we take a 6 DOF robot (J1,J2,J3,J4,J5,J6), it's quite simple to find all the possible configuration with a fixed J2 and fixed J3.

But when I want to include J2 and J3, the problem become way more complex.

Have you any clue ?

Thanks in advance for your help, have a good day!


问:
Convert ECEF Velocity to NED Velocity
My GNSS modules gives me one NMEA packet with ECEF velocity. My doubt is about how i convert these 3 vectors to NED frame. Can i use the conventional matrix rotation MathWorksRef. If yes, i use the latitude and longitude with the same timestamp of the velocity points, or other?</p>

Title: Convert ECEF Velocity to NED Velocity


问:
Difference between CHOMP and DRL
in terms of motion planning, what are the difference between gradient-based motion planning (for instance, CHOMP http://www.nathanratliff.com/thesis-research/chomp) and deep reinforcement learning?

Both of the approaches are iterated many times to optimize the given gradient function/ reward function in the simulation. Can someone give his/her insights into these two methods?

Title: Difference between CHOMP and DRL


问:
How to set "fast mode" in low cost DRO
I have this cheap DRO from Amazon/China
https://www.amazon.com/gp/product/B00E7XFF5Q/ref=ppx_yo_dt_b_asin_title_o07_s00?ie=UTF8&psc=1

I'm able to read the values out of it using a small FPGA.
The documentation says the update rate can be as fast as every 20 ms if it's in fast mode, but it doesn't say how to enable fast mode.  Currently the output is only every ~125 ms.

I've tried long-pressing the zero and inch/mm buttons, and various combinations of buttons (including power) but there is no change in mode.  There are no other buttons.  There is no input wire for example to send a command.

Does someone know how to get this thing into fast mode?

thanks

Title: How to set "fast mode" in low cost DRO


问:
What should be my spray handle-servo interface for pull mechanism?
I am an electronics engineering student who recently started working on Arduino. As part of my efforts to wake up on time, I decided that I should make an automatic water spray alarm clock. I have all the raw material ready (Spray Bottle, Arduino Uno, MG995 servo, external power supply for servo). I also have the test code ready. The only thing I am having trouble is how my servo should pull the spray handle.

Here is the spray bottle:

And here is my thought configuration of where servo would be and the interface location (red doodling):

For this interface, my options are:

Nylon fishing line. It is small enough to go thru mounting holes of servo arm of servo, but not strong enough to pull the spray handle.

Something that sticks to plastic servo arm as well as plastic spray handle, should it be a rod or a long piece of wood that can be put in place with screws? This is the part where I am stuck, as to what should I choose as the interface that sticks tightly to the servo arm and the spray handle and pushes spray handle when servo moves.


问:
Find viscous friction for DC motor transfer function
How do I find the viscous friction for my DC motor without measuring the input current and output torque.
I have to do a theoretical model on simulink and can't find any answers on how to find it</p>


问:
Closed-form solution of extrinsic in 2D to 3D projection problem
The title is pretty much it.

Anyone have seen a closed-form solution of extrinsic on 2D to 3D projection problem?

Seems like this should exist in old camera calibration literature but I can't really find it.


问:
Derivative of the Jacobian using Hessian
I am trying to calculate the derivative of the Jacobian matrix without using any symbolic methods or toolbox. The problem is that the calculation leads to a different result that the what it should be after manual differentiation. The method of calculation I am using for the derivative is the using the Hessian tensor $\dot J= (H(q) \overline \times_3 \dot q)$ where $\overline \times _3$ is the n-mode vector product of a tensor as in [1]. The Hessians are computed as [2].

The example I am using for the calculation is a double pendulum but with the second joint being about the x-axis (perpendicular to the first axis of rotation) such that the double pendulum rotates in 3-dimensions.

Assume the Jacobian is given as:

\begin{equation}
J_2=\begin{pmatrix} l_1cos(\alpha_1) & 0\\ 0 & l_2cos(\alpha_2)\\ l_1sin(\alpha_1)+l_2sin(\alpha_1)cos(\alpha_2) & l_2cos(\alpha_1)sin(\alpha_2)\\ 0 & cos(\alpha_1)\\ -1 & 0 \\0 & sin(\alpha_1)\end{pmatrix}
\end{equation}

Starting from the given Jacobian the results of the Hessians are as follows:
\begin{align}
\frac{\partial^2X_2}{\partial \theta_1^2} &= \begin{pmatrix}\begin{pmatrix}0\\-1\\0\end{pmatrix} \times \begin{pmatrix}l_1cos(\alpha_1)\\0\\l_1sin(\alpha_1)+l_2sin(\alpha_1)cos(\alpha_2)\end{pmatrix} \\ 0 \\0\\0 \end{pmatrix} = \begin{pmatrix}-l_1sin(\alpha_1)-l_2sin(\alpha_1)cos(\alpha_2)\\0\\l_1cos(\alpha_1)\\0\\0\\0\end{pmatrix}\\
\frac{\partial^2X_2}{\partial \theta_1\theta_2} &= \begin{pmatrix}\begin{pmatrix}0\\-1\\0\end{pmatrix} \times \begin{pmatrix}0\\l_2cos(\alpha_2)\\l_2cos(\alpha_1)sin(\alpha_2)\end{pmatrix} \\ \begin{pmatrix}0\\-1\\0\end{pmatrix} \times \begin{pmatrix}cos(\alpha_1) \\0\\sin(\alpha_1) \end{pmatrix}\end{pmatrix} = \begin{pmatrix}-l_2cos(\alpha_1)sin(\alpha_2)\\0\\0\\-sin(\alpha_1)\\0\\cos(\alpha_1)\end{pmatrix}\\
\frac{\partial^2X_2}{\partial \theta_2^2} &= \begin{pmatrix}\begin{pmatrix}cos(\alpha_1)\\0\\sin(\alpha_1)\end{pmatrix} \times \begin{pmatrix}0\\l_2cos(\alpha_2)\\l_2cos(\alpha_1)sin(\alpha_2)\end{pmatrix} \\ 0\\0\\0 \end{pmatrix} = \begin{pmatrix}-l_2cos(\alpha_2)sin(\alpha_1)\\-l_2cos^2(\alpha_1)sin(\alpha_2)\\l_2cos(\alpha_1)cos(\alpha_2)\\0\\0\\0\end{pmatrix}
\end{align}

The Hessian Tensor becomes:
\begin{align}
H_1 &= \begin{pmatrix}-l_1sin(\alpha_1)-l_2sin(\alpha_1)cos(\alpha_2) & -l_2cos(\alpha_1)sin(\alpha_2)\\ -l_2cos(\alpha_1)sin(\alpha_2)& -l_2cos(\alpha_2)sin(\alpha_1)\end{pmatrix}\\
H_2 &= \begin{pmatrix}0&0\\0&l_2cos^2(\alpha_1)sin(\alpha_2)\end{pmatrix}\\
H_3 &= \begin{pmatrix}l_1cos(\alpha_1) &0\\0 & l_2cos(\alpha_1)cos(\alpha_2)\end{pmatrix}\\
H_4 &= \begin{pmatrix}0 & -sin(\alpha_1)\\-sin(\alpha_1)&0\end{pmatrix}\\
H_5 &= \begin{pmatrix}0&0\\0&0\end{pmatrix}\\
H_6 &= \begin{pmatrix}0&cos(\alpha_1)\\cos(\alpha_1)&0\end{pmatrix}
\end{align}

This results in:
\begin{equation}
\dot J = \begin{pmatrix} (-l_1sin(\alpha_1)-l_2sin(\alpha_1)cos(\alpha_2))\dot \alpha_1 + -l_2cos(\alpha_1)sin(\alpha_2)\dot \alpha_2 & -l_2cos(\alpha_1)sin(\alpha_2)\dot \alpha_1 + -l_2cos(\alpha_2)sin(\alpha_1)\dot \alpha_2\\ 0 & l_2cos^2(\alpha_1)sin(\alpha_2)\dot \alpha_2\\ l_1cos(\alpha_1) \dot \alpha_1 & l_2cos(\alpha_1)cos(\alpha_2)\dot \alpha_2\\ -sin(\alpha_1)\dot \alpha_2 & -sin(\alpha_1)\dot \alpha_1 \\ 0 &0 \\ cos(\alpha_1)\dot\alpha_2 & cos(\alpha_1)\dot \alpha_2\end{pmatrix}
\end{equation}

Am I doing something wrong with the n-mode vector product of a tensor? I don't think the Hessian are wrong since the 2nd column in the rotation term of the $\dot J$ is right. Any help would be appreciated.

1

2


问:
How to perform a cart-pole swing up task using MPC?
I originally thought this was a really simple problem. However, to my surprise I did not find any resource talking about it, the only thing I found was that using Matlab's nonlinear MPC toolbox you could do it. I thus tried to solve it myself. I tried to use a trapezoidal direct collocation  at each step with a quadratic cost on both the state error and control, and I got rid of the terminal state constraint. I then noticed that it would not be able to swing it up. Also I noticed that without the terminal state constraint, even the planned state trajectory will not swing the pole to the top.

This is to me a surprising result. Thus, I want to ask if this is really the case or would this be due to a bug in my code? And how would people usually solve this problem using MPC?

My implementation is attached below. I used Matlab to program it since I wanted to used fmincon, instead of spicy.minimize.

```<code>%% Cartpole Parameters
m1=1;
m2=0.3;
l=0.5;
g=9.81;

% get next state
state = [0; 0; 0; 0];
tspan = [0, 0.02];
states = [state];

%% Optimization Configurations
N = 100;
dt = 0.02;
T = 2.0;

x0 = [];
for i = 0:N
    xtmp = i*dt/T*([1.0; pi; 0.0; 0.0] - state) + state;
    x0 = [x0; xtmp];
end

init_guess = [x0; zeros(N+1, 1)];

%% MPC
for i = 1:100
    [control, planned_states] = get_control(state, init_guess);
    next_state = planned_states(5:8)';
    states = [states, next_state'];
    state = next_state';
    disp(i)
    init_guess = [next_state'; planned_states(9:end); 1.0; pi; 0.0; 0.0; control(2:end); 0.0];
end
</code>```

the get_control function is defined as

```<code>function [control, states] = get_control(x_init, x0)
    %% Optimization Configurations
    N = 100;

    %% Set up objective
    q1 = ones(1, 4*N+4);
    q1(1:4) = 0.5;
    q1(end-3:end) = 100.5;
    Q1 = diag(q1);

    q2 = ones(1, N+1);
    q2(1) = 0.5;
    q2(end) = 0.5;
    Q2 = 0.01*diag(q2);

    x_target = [1.0; pi; 0.0; 0.0];
    x_targets = repmat(x_target, N+1, 1);
    u_targets = zeros(N+1, 1);

    targets = [x_targets; u_targets];

    Q = blkdiag(Q1, Q2);

    fun = @(x)(x-targets).'*Q*(x-targets);

    %% Set up lower and upper bound
    bound_piece = [10; 10; 10; 10];
    state_bnd = repmat(bound_piece, N+1, 1);
    control_bnd = 20*ones(N+1, 1);

    ub = [state_bnd; control_bnd];
    lb = -ub;

    %% Set initial and terminal condition
    I = eye(4);
    zeros1 = zeros(4, 4*N+N+1);
    left1 = [I, zeros1];
    Aeq = left1;
    beq = x_init;

    %% Collocation Constraints
    nonlcon = @collocateConstraint;

    %% fmincon
    A = [];
    b = [];

    options = optimoptions(@fmincon,'Algorithm','sqp','MaxIterations',100);
    x = fmincon(fun, x0, A, b, Aeq, beq, lb, ub, nonlcon, options);

    control = x(405:end);
end
</code>```

and the nonlinear constraints collocateConstraint are defined as

```<code>function [c, ceq] = collocateConstraint(x)
    dt = 0.02;
    N = 100;

    ceq = [];

    for i = 0:N-1
        x_i = x((i*4+1):(i+1)*4);
        x_ip1 = x((i+1)*4+1:((i+2)*4));

        u_i = x(4*N+4+i+1);
        u_ip1 = x(4*N+4+i+2);

        m1=1;
        m2=0.3;
        l=0.5;
        g=9.81;

        denominatori = m1 + m2*(sin(x_i(2))^2);

        Ai = [x_i(3);
              x_i(4);
              (l*m2*sin(x_i(2))*(x_i(4)^2) + m2*g*cos(x_i(2))*sin(x_i(2))) / denominatori;
              -(l*m2*cos(x_i(2))*sin(x_i(2))*(x_i(4)^2) + (m1 + m2)*g*sin(x_i(2))) / (l*denominatori)
            ];

        Bi = [0;
             0;
             1 / denominatori;
             -cos(x_i(2))/(l*denominatori)
            ];

        denominatorip1 = m1 + m2*(sin(x_ip1(2))^2);

        Aip1 = [x_ip1(3);
                x_ip1(4);
                (l*m2*sin(x_ip1(2))*(x_ip1(4)^2) + m2*g*cos(x_ip1(2))*sin(x_ip1(2))) / denominatorip1;
                -(l*m2*cos(x_ip1(2))*sin(x_ip1(2))*(x_ip1(4)^2) + (m1 + m2)*g*sin(x_ip1(2))) / (l*denominatorip1)
            ];

        Bip1 = [0;
                0;
                1 / denominatorip1;
                -cos(x_ip1(2))/(l*denominatorip1)
            ];

        ceqi = x_ip1 - x_i - (dt/2)*(Bi*u_i + Bip1*u_ip1) - (dt/2)*(Ai + Aip1);

        ceq = [ceq; ceqi];
    end

    c = [];
end
</code>```

Thanks in advance.


问:
Pose-Graph Optimization in MATLAB does not give expected results
I am trying to optimize a pose graph I created in MATLAB using either the optimizePoses(viewSet)-function on the imageViewSet that I have or the optimizePoseGraph(poseGraph)-function after converting that imageViewSet to a poseGraph using createPoseGraph(vSet).
Both functions do not optimize the pose graph correctly.

This is the uncorrected pose graph, with loop closures in red:

and this is the pose graph after the optimization:

Clearly, the optimization is not correct. I used a dataset from Sensefly, and you can look at the "ground-truth" here: https://senseflycom.s3.amazonaws.com/datasets/small-village-merlishachen/postflight-report/merlischachen_report.pdf

As you see, the UAV flew some kind of a grid-trajectory.

I don't have a real clue where to start looking for the problem. There seems to be an issue with the distribution of the error over all poses.
The graph was created using visual odometry, i. e. matching features and calculating and decomposing homographies between consecutive images.

Can anyone point me in the right direction?

If you need more information, feel free to ask.


问:
How to create pseudo code for Braitenberg vehicle 2b?
I am asked to make Braitenberg vehicle for small robotic exhibition. I am asked to use GL5528 light sensor. My job is to construct and program Braitenberg vehicle 2b, which uses GL5528 light sensors and wheels.

Can anyone please help me with and create pseudo code for Braitenberg vehicle 2b please would be really appreciated as I have wired the robot but am weak at programming part just a general pseudo code would do thank you.


问:
How do you derive the soft iron matrix for magnetometer calibration?
Can someone send reliable sources for how this is derived? I understand the diagonal values are for scale, but how do you determine all 6 values needed for this matrix? I would also like to normalize the radius to 1 while doing so. I've found many papers of ellipsoid fitting, but am unsure of how to translate this for my use, and honestly could just use some outside input.</p>


问:
Looking for a course (online?) RE: systems with XY stages
Using XY stages is common in many fields:

3D printing

Genetics instruments

semiconductor lithography

automated microscope slide imaging

See for example https://www.asml.com/en/technology/lithography-principles/mechanics-and-mechatronics

Optimizing an XY stage for:

precision

accuracy

speed

Is a challenging problem of controls, structural engineering, and motion.

Is there an online or in person course that teaches the fundamentals of systems built for these purposes?

Perhaps a graduate level course?  Coursera? LinkedIn? Kahn Academy? a stage vendor?


问:
Are there any problems with this design?
https://www.reddit.com/r/robotics/comments/nbqx55/does_this_design_for_a_leg_make_sense_description/?utm_medium=android_app&utm_source=share

So essentially I'm trying to make a leg that's able to kick. Motor A drives a pole, and that pole is connected to another pole with a hinge. That second pole can either be extended or retracted like a calf, it can do this by a string that's connected to the end of it that goes through the first pole, into the shaft of motor A, out the center of the shaft, and into Motor B that retracts or let's go of the line in order to control whether the calf is extended or not, determined by code.


问:
How to control the Dynamixel Motor in PWM mode without having it locked when PWM is zero
I want the motor to be free to move by hand when I set the PWM to zero. Instead, there is a torque that makes the motor still in one position. Is that possible to do?</p>


问:
Unparable Bluetooth module
I'm doing application in Android Studio that will scan for devices within reach of a phone. What it does exactly is not important. The problem is, that when I scan for devices, I'd get only the ones that aren't paired. Paired ones are stored in separate list and Android's discovery process automatically skips them. As far as I looked it seems I can't change that.

So I need Bluetooth module that won't be able to get paired with a phone, to be 100% sure the whole project will work correctly.

Is it possible to set a module (like HC-06) to make it unparable?

Can I buy a module like that somewhere? I couldn't find anything.

Title: Unparable Bluetooth module


问:
Inverse Kinematics for a 4-DoF Leg
I am beginner in the robotics space and I am trying to apply inverse kinematics to robot leg that has 4 degrees of freedom. I do not know how to think about solving these problems, so any guidance would be greatly appreciated!

The legs are from this system and I am working to understand only one currently (this may be the wrong approach):
https://www.robotshop.com/en/lynxmotion-t-hex-4dof-hexapod-robot-kit-hardware-only.html

My first question is, is inverse kinematics the correct approach? To me it seems to be since I want to determine the angles for the servos based on the foot position and trajectory. If anyone has any recommendations with respect to this I would greatly appreciate it so that I know am not headed down the right path. That being said, I am struggling to find solutions to problems of this sort. It seems as though 4 DoF is more challenging and is often not solved analytically. I would still like to solve this analytically, but I have moved on to try out numerical solutions.

In MATLAB, I have worked through several examples and I have tried to transition them to the case of this system. I have attached my code so far. Currently, I am struggling with applying joint constraints, getting the configuration that is more physical, and getting a useable solution from this approach. So my questions to this are:

Is this a valid approach to finding the solution to inverse kinematics of this system?

As in, if I manage to get this to work, is the solution useable? I may be picturing this wrong, but I foresee getting a sequence of servo angles that will correspond to the trajectory. If I get these servo angles (and the joint constraints, lengths, etc. are all physically accurate) will I be able to give the leg the sequence? If this is even a valid approach, I have some MATLAB questions to follow.

Will this approach transition well to when there are more legs (i.e. the complete robot with 6 legs)?

How do joint constraints work in the robotics toolbox?

It seems like rotation is right-handed with respect to the axis of rotation. In addition, when a positive angle is applied, it seems to rotate the rigidbody below the local coordinate system of the joint. However, there are joint constraints to the servos and when I try to apply these, the IK solver that I setup fails to follow the defined trajectory and will give errors saying that the joint angle is outside of the constraints set, even when I make sure that they are valid ranges. I have not been able to find useful documentation on this.

If I get the joint constraints set correctly, will the solution be in the correct configuration?

Should I consider other constraints to apply?

Is the invverseKinematics from MATLAB what I need or should I be trying generalizedInverseKinematics?

Currently, I defined some random arc for the trajectory. Should I consider something else for the walk of a hexapedal robot?

I am open to any suggestions since it is clear that I do not have any direction. Please let me know if more information is needed and I will edit the post. Physical dimensions of the robot are included in the code. The height of base is arbitrary currently. I have tried to comment the code heavily so that it is easy to understand. Lastly, here are 4 images showing the current results of the MATLAB simulation. blue lines are the body segments and the black ark is the trajectory.

Code:

```<code>% Robot_Advanced_Real.m
% Copied from matlab and adapted for my scenario.
% Creates robot, uses IK to calculate positions, Creates animation

% Initialize MATLAB
clear all;
close all;
clc;
tic;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Building Robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Start with Blank RigidTreeBody Model
robot = rigidBodyTree('DataFormat','column','MaxNumBodies',5);

% Specify Arm Lengths of the Robot Arm. Units: [mm]
L1 = 28.45;
L2 = 75.41;
L3 = 70.02;
L4 = 87.14;

% Height of robot body off of Ground
G = 100;

% Add first body with joint. Set as robot base.
body = rigidBody('link1');
joint1 = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint1,trvec2tform([0 0 G]));
joint1.JointAxis = [0 0 1];
joint1.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint1;
addBody(robot, body, 'base');

% Add second body with joint. Attach to First
body = rigidBody('link2');
joint2 = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint2, trvec2tform([L1,0,0]));
joint2.JointAxis = [0 1 0];
joint2.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint2;
addBody(robot, body, 'link1');

% Add third body with joint. Attach to second
body = rigidBody('link3');
joint3 = rigidBodyJoint('joint3','revolute');
setFixedTransform(joint3, trvec2tform([L2,0,0]));
joint3.JointAxis = [0 1 0];
joint3.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint3;
addBody(robot, body, 'link2');

% Add fourth body with joint. Attach to third
body = rigidBody('link4');
joint4 = rigidBodyJoint('joint4','revolute');
setFixedTransform(joint4, trvec2tform([L3,0,0]));
joint4.JointAxis = [0 1 0];
joint4.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint4;
addBody(robot, body, 'link3');

% Add End Effector with Fixed Joint. Attach to final body.
body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([L4, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link4');

% View Robot Input details to review setup
showdetails(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Define Trajectory
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Define series of [x y z] for arc step.
% Define parameters of the arc.
xCenter = 0;
yCenter = 120;
zCenter = 0;
xRadius = 70;
yRadius = 20;
zRadius = 20;

% Define the angle theta as going from 30 to 150 degrees in 100 steps.
theta = linspace(30, 150, 100);
% Define x and y using "Degrees" version of sin and cos.
x = (xRadius * cosd(theta) + xCenter)';
y = (yRadius * sind(theta) + yCenter)';
z = (zRadius * sind(theta) + zCenter)';

% Points around circle
% points = center + [x y z];
points = [x y z];
count  = length(x);

% Check
% plot3(points(:,1),points(:,2),points(:,3))
plot3(x,y,z)
hold on
plot3(points(:,1),points(:,2),points(:,3),'k')
xlabel('x')
ylabel('y')
zlabel('z')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Inverse Kinematics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Use IK object to find a solution to the joint configurations that achieve
% end effector coordinates along ttrajectory

% Pre-allocate configuration solutions as a matrix, qs
q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);

% create the IK Solver.
ik = inverseKinematics('RigidBodyTree', robot);

% Use desired weights for solution (First three are orientation, last three are translation)
% Since it is a 4-DOF robot with only one revolute joint in Z we do not
% put a weight on Z rotation; otherwise it limits the solution space
% weights = [0, 0, 0, 1, 1, 0];
weights = [0.1, 0.1, 0, 1, 1, 1];
endEffector = 'tool';

% Loop through the trajectory and store the configurations. IK is used to
% give joint configurations.
qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ik(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Animate Solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Show first configuration. Show trajectory.
figure
show(robot,qs(1,:)');
ax = gca;
ax.Projection = 'orthographic';
hold on
plot3(points(:,1),points(:,2),points(:,3),'k')
view([-37.5 30])

% Set up animation. Show robot in each configuration on trajectory
framesPerSecond = 7;
r = rateControl(framesPerSecond);
for i = 1:count
    show(robot,qs(i,:)','PreservePlot',false);
    drawnow
    waitfor(r);
end

toc
</code>```

Title: Inverse Kinematics for a 4-DoF Leg


问:
Multidimensional obstacle avoidance in ODE. Part II
Multidimensional obstacle avoidance

https://math.stackexchange.com/questions/4146255/multidimensional-obstacle-avoidance-in-ode

For some time, I studied this question more closely and came to the conclusion that it is best to assign its own constraint for each selected coordinate. Here are the results.

I choose those system of ODE:

$F=\begin{cases} \dot{x}=g_x+\frac{d(U_{rep_1})}{dx} \\  \dot{g_x}=-g_x+\frac{df}{dx} \\ \dot{y}=g_y+\frac{d(U_{rep_2})}{dy} \\ \dot{g_y}=-g_y+\frac{df}{dy} \end{cases}$

where $x,y,g_x,g_y$ - variables;

$f=e^{-(x^2+y^2)}$

$U_{rep_1}=\begin{cases} \frac{1}{2}(\frac{1}{\rho_1}-\frac{1}{\rho_0})^2, & \mbox{if } \rho_1<=\rho_0\mbox{} \\ 0, & \mbox{if } \rho_1>\rho_0\mbox{} \end{cases}$

$U_{rep_2}=\begin{cases} \frac{1}{2}(\frac{1}{\rho_2}-\frac{1}{\rho_0})^2, & \mbox{if } \rho_2<=\rho_0\mbox{} \\ 0, & \mbox{if } \rho_2>\rho_0\mbox{} \end{cases}$

$\rho_1=\sqrt{(x-\psi_1)^2}-d_{obs}$

$\rho_2=\sqrt{(y-\psi_2)^2}-d_{obs}$

$\psi_1=\delta +2 e^{-t T}$

$\psi_2=\delta +\frac{1}{2.2}e^{-t T}$

$\rho_0 = 0.15, T = 0.5, \delta = 0.35,d_{obs}=0.1$

The figure below shows the results of numerical simulations from Mathematica. It can be seen that the selected variables cannot cross the selected barrier.

The following is interesting. When a variable touches the barrier, it kind of "sticks" to it, holds on for a while, and then "unsticks". At this time, $\frac{d(U_{rep_1})}{dx}$ and $\frac{d(U_{rep_2})}{dy}$ is not zero.

In general, I ask for help with this task - how to quickly "detach" the variable from the barrier? Are there any tricks? Maybe experts in differential equations and dynamical systems can help. I will be glad to help.


问:
Robotic arm object/item pile relocation?
I am somewhat new to robotics. As such, I might be unaware of the correct terms or lingo that I need to use. I am trying to find relevant information on autonomous robotic arm operations specifically in terms of relocating piles of semi-indistinguishable objects. The pile could be anything from a pile of stones, pebbles, sticks, etc.

I am trying to find literature/ examples of similar operations that have been performed in order to get things started on my end. (I have already tried in a simulated environment - gazebo and VREP)

I am able to understand how to autonomously detect and relocate a single inanimate object from point A to point B, but when it comes to piles of the said object I am not sure how to start with the pile. The simulated arm has the capability to cluster pick multiple objects at the same time. (like a clamp)

I have tried to look up online to see if I could find any related articles or research papers but I have been hitting a dead end. Not exactly even sure what I should be searching as the common terms I have used here didn't work.

Any advise, articles, papers, or points in the correct direction would be greatly appreciated!


问:
Jetson tx2 to Teensy 3.6 Serial communication issue
I am trying to establish serial communication between Nvidia Jetson Tx2 and Teensy 3.6. Using pyserial, while reading the data sent from Teensy 3.6 on Rx pin on the Jetson, I come to an error that shows "device reports readiness to read but returned no data" after 3-4 seconds. Before that, it works fine (it is unreliable).

Does anyone know why is this happening and how to rectify it?

Thank you


问:
Cheap global shutter cameras with external synchronization
Please recommend cameras for multi-camera setup satisfying the following conditions:

Relatively cheap (up to 100$ per camera).

Global shutter.

No filters on lenses or pixels.

Resolution of 1 MP or higher.

Capture frequency of 25 Hz or higher.

Wide field of view, at least 80° horizontally, ideally ~110°.

Ability to synchronize frame capture using external source.

(Optional) USB 3 interface for data transfer and Video4Linux compatibility.

The setup is planned for visual SLAM research. Currently we use Intel RealSense cameras, but they have the following drawbacks for us:

Redundant hardware: color camera, IR projector, IMU, proprietary ASIC for data processing.

IR cameras have short baseline (60-90 mm).

Cameras face in the same direction, while we would like to work with partially overlapping fields of view.

D455 sensor uses Bayer-pattern on its "infrared" cameras.

librealsense2 is somewhat bloated and has not the greatest C API.

We also work with cameras produced by The Imaging Source, but unfortunately they are more expensive than we would like.

It would be really nice if you will recommend camera models with which you have hands-on experience, but of course it is not mandatory.


问:
FABRIK Inverse Kinematics Convergence Proof For Multiple End-Effector Case
Below, I've linked the FABRIK papers, the first of which is particularly relevant to my question, as it shows the convergence proof for the single end effector case, and this makes sense to me. But I have trouble extending the proof and proving convergence for the multiple end effector case discussed in the paper. If someone can give a proof that this also indeed converges, that would be much appreciated!

http://andreasaristidou.com/publications/papers/Extending_FABRIK_with_Model_C%CE%BFnstraints.pdf

http://andreasaristidou.com/publications/papers/FABRIK.pdf


问:
Power pack and servo driver for robotic arm?
I have a robotic arm that uses about eight 7.4 volt volt high torque servos. (https://www.servocity.com/2000-series-dual-mode-servo-25-2/)

I need to know what Kind of battery system I can use to power the claw servos, and what servo controllers are available for such a heavy duty servo type.

BATTERY OPTIONS:
I cant use lead acid, because my robot cant fit such a massive battery on it. Li-ion I'm not sure would work, because each cell is 3.7v, and the max amp-hours for a typical battery cell is 5Ah so the battery pack for All the servos would be 2 li-ion batteries, and 10Ah, which would very quickly drain the cells.

I'm guessing my best option would be to make a NiCd D-cell battery pack, resulting in 5 NiCd batteries 1.2v each and 25Ah.

SERVO CONTROLLER:
I am not sure what servo controllers are the best for controlling 7v servos. I have one that can control small 5v servos, but I dont know if it could handle the heavy duty servos that are on my claw.


问:
Can a state lattice planner be used purely as a local planner?
I've been reading a bit about state lattice motion planning recently. The question I would like to ask is if a lattice-based motion planning system can be used purely as a local planner without a lattice-based global planned path for mobile robots.

In other words, given I have a global plan as a sequence of waypoints to follow, can I use a lattice-based local planner for obstacle avoidance?


问:
Is possible this robot arm cut well around this plastic plate?
Good Afternoon.

I am trying my best to put this Omron Viper 850 6-axis robot arm, to cut around some car plast plates.

The Viper 850 robot is this:

The car plastic plate that supposedly it to cut around, is this:

I`ve tried do it by programming in the robot software, inserting coordinates and positioning the robot, but it went far from the perfection, as you can see:

Therefore, I am trying just now to discover a best way to do this.

I though I could get a 3D model with g-code coordinates from a photo of the plate.

With the g-code in hands, I could take the current coordinate point, and the next coordinate point, make some calcs and get the adjust angle of the robot spindle cutter. Further, I might make a program to translate g-code to the programming language of the robot (eV+ language), with the due calculations already done.

But I think G-code generators doesn`t generate coordinate points in a ordered way. This is the first problem.

The second problem would be to adjust the angle of the Z-Axis.

Obs: The software that came with the robot not import drawings to robot cut.

Then, after all, my question: Is it possible to do with this robot? If no, why?


问:
Retrieving Denavit-Hartenberg-Parameters from homogenous transformation-matrix
i am trying to implement a matlab script where the user can enter quaternions and retrieve the corresponding denavit hartenberg parameters for this link. My approach is to create the homogenous transformation matrix from the entered quaternions and then calculate the DH-Parameters from it.

For now it works for a and d, but it has difficulties with negative alpha angles. I am calculating alpha with 'acosd(Matrix(3,3))', but this always returns positives angles. For example if i have a link with -pi/2 for alpha i retrieve pi/2. I discovered that the transformation matrix has exact the same values for a link with a negative and a link with positive alpha angle. Therefore it is unclear for me how i should identify the sign of alpha with this approach.

Is it even possible what i am trying to do here? And do you have an idea how i can calculate alpha from the transformation matrix with taking into account the sign?


问:
Data gathering and selling
a friend of mine is building various Robots with different structures, sensors,  limbs  executing different tasks. He gathers data from all sensors etc regarding technical and scientific range of subjects. He is asking if there is somewhere where he can sell those data. He can provide raw data as well. Does anyone know if there such a thing or service?</p>

Title: Data gathering and selling


问:
difference between particle filter in slam and normal particle filter
I would like to ask you about what is the difference between particle filter when we use it in Slam methods and when we use it normally to correct our sensor measurement.

I mean, is there any difference in purpose or how we use this method (PF)

for example, I use a particle filter to localize my robot. Is it the same if I run PF SLAM to localize the robot or they have other uses aspect rather then localize.

thanks in advance.


问:
Beginner needs help with Self-Balancing-Robot build
I'm looking for help on a 1st build self balancing robot, something that seemed pretty easy but not. I'm retired so have plenty of time but no real robot skills, electrical, mechanical yes.

I have been trying to get it working for weeks now, I have a chassis with two Nema17 motors, 65mm wheels, its 300mm high and the battery is at the top. I am using a Pi Pico as control with the MPU6050 IMU and two A4988 drives.

A friend on another group was building a game controller using the MPU6050 and has written a whole new driver for it that gives out filtered data - Kalman or Complimentary and it also does calibration etc.

I have tried my one code and failed but also today tried anothers code that was written specifically to show how easy it was - and it still failed for me.

This tells me maybe the motors are no good or the MPU is a bad choice, he used a BNO055 IMU.

The effect i get is that the bot will just jump about , even when static. Video - https://youtu.be/nNxNHaKN5w0

The code has a PID loop, makes little difference so far.

Is there maybe a recommended motor for these 'bots?

I am at a complete loss now as it seems nothing works, even code that is proven to work! It must be hardware???

Thanks for any help


问:
Zero crossing events in trapezoidal commutation
I would like to ask a question about zero crossing event in a trapezoidal commutation on a brush-less DC motor. Here is the waveform that shows that the zero crossing event occurs every 180 electrical degrees in a sinusoidal commutation:

But what about trapezoidal commutation. Here is the waveform that I found about the trapezoidal commutation:

So as you see, the zero crossing occurs 30 electrical degrees after the previous commutation and 30 electrical degrees before the next commutation.
In a motor with one pole pair, we would have 30 electrical degrees = 30 mechanical degrees, so we would have this waveform:
You see that the zero crossing in phase A occurs when the magnet faces the phase C, or in other words, after 30 electrical degrees from the last commutation.
My question in why does the zero crossing happen at that moment, why not after 60 electrical degrees, or 15 electrical degrees?
Is it related to some law's of induction? What are those law and how do this law's appear in this motor?
Can someone explain to me this with some pics?

ContentLicense: CC BY-SA 3.0


问:
Potential FIeld Calculation
We have to implement an algorithm for potential field path planning.

I have implemented so far everything for this formula:

```<code>$ U(q) = attractive Force + Sum Repulsive Forces (of all obstacles) $
</code>```

The result should be a velocity vector for x and y direction.

Based on the lecture slides we have the hint:

```<code>F = - \nabla U(q) = - [dU/dx, dU/dy]
</code>```

Can somebody explain me, how to transform the Energy U to the velocities?

Given is a map with a startposition and a targetPosition of a robot in [m] and some positions of obstacles. Furthermore an updatetime of 0.2s

Title: Potential FIeld Calculation


问:
Navigation with obstacle avoidance without precise velocity measurement
I'm working on autonomous navigation and obstacle avoidance using a few small, education-purpose differential drive robots. The robot's motor can be programmatically controlled via PWM but the robot itself does not come with any useful sensors (wheel encoders, IMU etc.) What I try to do now is to attach AR markers (like AruCo) to the robots to get their poses, but the velocity measurement comes from the finite differences of positions and headings, thus quite noise. This makes it challenging to use velocity-based local planning methods, like Dynamic Window. I've used a Extended Kalman Filter on the position measurement, but the outcome is still than ideal. The other constraint is that the update rate on the robot motor is bounded by the camera FPS (<60 fps), thus tracking the velocity precisely is not easy either. Are there other local planner I should try? Should I get additional sensors? Thanks very much!</p>


问:
What autopilot options are available for nano helicopters?
I am new to the world of drones and UAVs. While there are a ton of resources available online for quad-copters and multi-copters in general, I am really struggling with finding resources on helicopters.

Specifically I wanted to know if there are existing autopilot systems for RC helicopters. Now I am not interested in large helicopters. What I want to work on is modifying helicopters like Blade Nano S3 or Blade 70S i.e helicopters that are no larger than my palm.

I found some Discontinued autopilots on 1bitsquared i.e Lisa/S developed by TU Delft and Elle0.
I don't need anything necessarily fancy, just the very basic autopilot stuff but I need it to be configurable so that I can use my own GPS system or program the MCU with some additional routines using Arduino or easy to program ecosystems. It should be configurable in some way so to speak.

Since whatever I found seems to be discontinued I do not expect to find good support for it and hence I do not want to waste time reinventing the wheel.

Are there basic autopilots out there that I can use to replace the existing flight controller on the Blade Helicopters and add a few more functionality to them like FPV and custom GPS telemetry?


问:
Spherical Parallel Manipulator Lagrangian problem
I faced a very serious problem and I urgently need the help of specialists in robotics, mechanics, physics and mathematics.

I am trying to derive equations of motion from the Lagrangian of a spherical parallel manipulator based on work (3.2 Inverse dynamic modeling): https://www.sciencedirect.com/science/article/abs/pii/S0921889014001250

According to this article, the Lagrangian for the corresponding drive axle consists of two parts:

Lagrangian of distal and proximal links.

$L_i=\frac{1}{2}I_{l_1}\theta_i^2+\frac{1}{2}\boldsymbol{\omega_i}^T\boldsymbol{I}_{l_2}\boldsymbol{\omega_i}-m_{l_1}\chi_1\boldsymbol{g}^T\boldsymbol{h_i}-m_{l_2}\chi_2\boldsymbol{g}^T\boldsymbol{e_{ix}}$

where $\boldsymbol{h_i}=\frac{u_i+v_i}{||u_i+v_i||}$;$\boldsymbol{e_{ix}}=\frac{v_i+w_i}{||v_i+w_i||}$; $m_{l_1},m_{l_2},\chi_1,\chi_2,I_{l_1}$ - masses, center of masses of proximal and distal links and moment of inertia of proximal link; $u_i,v_i,w_i$ - design vectors depending on design parameters of the mechanism, angles of rotation of drives and something else? ...

Platform Lagrangian

$L_p=\frac{1}{2}\boldsymbol{\Omega}^T\boldsymbol{I}_{p}\boldsymbol{\Omega}-m_{p}R\cos(\beta)g^T\boldsymbol{p}$

where $\boldsymbol{p}$ - unit vector of platform direction; $\boldsymbol{I}_{p}$ - tensor of inertia of platform; $\boldsymbol{g}=[0;0;-9.81]^T$;$m_p,R,\cos(\beta)$ - mass, radius of sphere and angle beetwen platform and center of rotation;

In general, the Lagrangian is written as follows:

$L=L_1+L_2+L_3+L_p$

In the article, the authors took as generalized coordinates:

$\boldsymbol{q}=[\theta_1,\theta_2,\theta_3,\phi,\Theta,\sigma]$;

where - $\theta_1,\theta_2,\theta_3$ - drive angle of rotation; $\phi,\Theta,\sigma$ - platform angles of rotation;

Further, the authors derive the equations of motion according to the classical formula, but they do not give more visual and additional calculations:

$\frac{d}{dt}(\frac{dL}{d\dot{\boldsymbol{q}}})-\frac{dL}{d\boldsymbol{q}}=0$

From that moment on, I encountered problems of the following nature, which I cannot solve on my own:

Lagrangian structure assumes that the vector of generalized coordinates $q$ proposed by the authors lacks the position and velocity vector for the distal link and platform, i.e. $q≠[\theta_1,\theta_2,\theta_3,\phi,\Theta,\sigma]$, but $q=[\theta_1,\theta_2,\theta_3,\boldsymbol{\omega},\boldsymbol{\Omega}]$. Their generalized coordinates do not allow deriving the equations of motion, since angular velocity of the distal link $\boldsymbol{\omega}$ in the article there is no corresponding angular position.

Moments of inertia of the platform $\boldsymbol{I}_{p}$ and the distal link $\boldsymbol{I}_{l_2}$ are nonstationary and, strictly speaking, depend on the angle of rotation of the drives, i.e. from $\theta_1,\theta_2,\theta_3$. There is absolutely no indication of this in the article, but this should be taken into account when deriving the equations of motion from $L$. But the question is that it is not clear what all these moments of inertia depend on - on the angle of rotation of the drives or on the angle of rotation of the platform, i.e. what to include in the Lagrangian, $\boldsymbol{I}_{p}(\theta_1,\theta_2,\theta_3)$ or $\boldsymbol{I}_{l_2}(\phi,\Theta,\sigma)$ ?

The third problem follows from the second and is connected with the fact that we can still find the coordinates of the design vectors $u_i$ and $v_i$ using the known design parameters and angles of rotation of the drive $\theta_1,\theta_2,\theta_3$, but the position of the vector $w_i$ is calculated iteratively using the problem of direct kinematics, and one position of the drives $\theta_1,\theta_2,\theta_3$ corresponds to 8 platform positions, i.e. it is impossible to establish a direct analytical connection between the angles of rotation of the drives $\theta_1,\theta_2,\theta_3$ with the position of the platform $\phi,\Theta,\sigma$ and the coordinates of the vector $w_i$, and therefore, it is impossible to differentiate the potential energy component of the Lagrangian $L$ by the generalized coordinate $q$.

Dear experts,
please help me understand the principle of working with this Lagrangian $L$ as soon as possible.
I would be grateful for your help in resolving these issues.
I have Mathematica, Maple and Simulink at hand and I am ready to do the proposed calculations and present the results.

Some excerpts from the article are presented below:


问:
unable to integrate local planner plugin into move_base
I am trying to integrate my local path planner plugin into move_base. On launching the modified move_base launch file, I am getting the following error -

```<code>[FATAL] [1626211675.635218297, 96.984000000]: Failed to create the local_planner/LocalPlanner planner, are you sure it is properly registered and that the containing library is built? Exception: Failed to load library /home/skpro19/catkin_ws/devel/lib//liblocal_planner_lib.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = /home/skpro19/catkin_ws/devel/lib//liblocal_planner_lib.so: undefined symbol: _ZN13local_planner12LocalPlannerD1Ev)
[move_base-1] process has died [pid 26446, exit code -11, cmd /home/skpro19/catkin_ws/devel/lib/move_base/move_base /odom:=odometry/filtered __name:=move_base __log:=/home/skpro19/.ros/log/29b07b08-e421-11eb-b77b-c8e2656b0bb1/move_base-1.log].
log file: /home/skpro19/.ros/log/29b07b08-e421-11eb-b77b-c8e2656b0bb1/move_base-1*.log
</code>```

Also, is it normal that it is looking for the library at lib//lib_local_planner_lib instead of lib/lib_local_planner_lib ? I am also curious about the undefined symbol: _ZN13local_planner12LocalPlannerD1Ev part.

I have inserted the line PLUGINLIB_EXPORT_CLASS(local_planner::LocalPlanner, nav_core::BaseLocalPlanner);
in my source (my_local_planner_ros.cpp) file.

I am attaching my CMakeLists.txt, package.xml and local_planner_plugin.xml files below.

CMakeLists.txt

```<code>cmake_minimum_required(VERSION 3.0.2)
project(my_local_planner)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS

    base_local_planner
    costmap_2d
    #dynamic_reconfigure
    nav_core
    nav_msgs
    pluginlib
    sensor_msgs
    roscpp
    tf2
    tf2_geometry_msgs
    tf2_ros
)

#find_package(Eigen3 REQUIRED)
#remove_definitions(-DDISABLE_LIBUSB-1.0)
find_package(Boost REQUIRED COMPONENTS system)

catkin_package(
    CATKIN_DEPENDS
        base_local_planner
        #dynamic_reconfigure
        nav_msgs
        pluginlib
        sensor_msgs
        roscpp
        tf2
        tf2_ros
)

include_directories(
    include
    <span class="math-container">${catkin_INCLUDE_DIRS}
    $</span>{Boost_INCLUDE_DIRS}

)
#add_definitions(${EIGEN3_DEFINITIONS})

# dynamic reconfigure
#generate_dynamic_reconfigure_options(
 #   cfg/DWAPlanner.cfg
#)

add_library(local_planner_lib src/my_local_planner_ros.cpp)
</code>```

package.xml

```<code><?xml version="1.0"?>
<package format="2">
  <name>my_local_planner</name>
  <version>0.0.0</version>
  <description>The my_local_planner package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="skpro19@todo.todo">skpro19</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>

  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/my_explore_lite</url> -->

  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->

  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>

  <depend>costmap_2d</depend>
  <depend>move_base</depend>
  <depend>actionlib</depend>
  <depend>move_base_msgs</depend>
  <depend>visualization_msgs</depend>
  <depend>nav_core</depend>
  <depend>base_local_planner</depend>
  <depend>pluginlib</depend>
  <depend>sensor_msgs</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

  <depend>nav_msgs</depend>

  <export>

    <nav_core plugin="${prefix}/local_planner_plugin.xml" />

  </export>

</package>
</code>```

local_planner_plugin.xml

```<code><library path="$HOME/catkin_ws/devel/lib/liblocal_planner_lib">
  <class name="local_planner/LocalPlanner" type="local_planner::LocalPlanner" base_class_type="nav_core::BaseLocalPlanner">
    <description>
      A implementation of a local planner using either a DWA approach based on configuration parameters.
    </description>
  </class>
</library>
</code>```

move_base.launch

```<code><?xml version="1.0"?>
<!--
Software License Agreement (BSD)

\file      description.launch
\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Prasenjit Mukherjee <pmukherj@clearpathrobotics.com>
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that
the following conditions are met:
 * Redistributions of source code must retain the above copyright notice, this list of conditions and the
   following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
   following disclaimer in the documentation and/or other materials provided with the distribution.
 * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
   products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE
)ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>

  <arg name="no_static_map" default="false"/>
  <arg name="ns" default="/"/>
  <!-- arg name="base_global_planner" default="navfn/NavfnROS" / -->

  <arg name="base_global_planner" default="global_planner/GlobalPlanner" />

  <arg name="base_local_planner" default="local_planner/LocalPlanner"/>
  <!-- <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/> -->

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" ns="$(arg ns)">

    <remap from="/odom" to="odometry/filtered" />

    <!-- Loading parameters for the move_base node -->
    <rosparam file="$(find my_global_planner)/config/move_base_params.yaml" command="load" />

    <!-- Loading parameters for the local and global planners used by move_base-->
    <param name="base_global_planner" value="<span class="math-container">$(arg base_global_planner)"/>
    <param name="base_local_planner" value="$</span>(arg base_local_planner)"/>
    <rosparam file="$(find my_global_planner)/config/planner.yaml" command="load"/>

    <!-- observation sources located in costmap_common.yaml -->
    <rosparam file="<span class="math-container">$(find my_global_planner)/config/costmap_common.yaml" command="load" ns="global_costmap" />
    <rosparam file="$</span>(find my_global_planner)/config/costmap_common.yaml" command="load" ns="local_costmap" />

    <!-- local costmap, needs size -->
    <rosparam file="$(find my_global_planner)/config/costmap_local.yaml" command="load" ns="local_costmap" />
    <param name="local_costmap/width" value="10.0"/>
    <param name="local_costmap/height" value="10.0"/>

    <!-- static global costmap, static map provides size -->
    <rosparam file="<span class="math-container">$(find my_global_planner)/config/costmap_global_static.yaml" command="load" ns="global_costmap" unless="$</span>(arg no_static_map)"/>

    <!-- global costmap with laser, for odom_navigation_demo -->
    <rosparam file="<span class="math-container">$(find my_global_planner)/config/costmap_global_laser.yaml" command="load" ns="global_costmap" if="$</span>(arg no_static_map)"/>
    <param name="global_costmap/width" value="50.0" if="<span class="math-container">$(arg no_static_map)"/>
    <param name="global_costmap/height" value="50.0" if="$</span>(arg no_static_map)"/>
  </node>

</launch>
</code>```


问:
Nonlinear Sensor Fusion with Space-Time Finite Element and Static Condensation?
I have recently implemented an algorithm for the nonlinear fusion of GNSS, barometer, magnetometer, accelerometer and gyroscope data.

The algorithm is based on a space-time finite element where the Hermitian shape functions represent the position coordinates, the local magnetic north vector and a rotation angle. The latter two can be transformed into the orientation quaternion. Each finite element represents a certain timespan and contains the corresponding sensor data. The error of the sensor data interpolation within an element is minimized with the help of a Newton based approach. Due to computational constraints it is not possible to connect an increasing number of finite elements as time progresses. I have therefore used static condensation to get the gain from previous elements so that only one element is considered at any time.

This approach works remarkably well. My problem is that I am not an expert in the field of sensor fusion and thus have only a (very) limited insight. However, I would be surprised if such an approach is not already widely used as it is remarkably simple and efficient. Hence I would be grateful if some experts could give me feedback. I am particularly interested if static condensation can be compared to or is equivalent to the Kalman gain.

I have uploaded the preprint of this approach to arXiv that contains all the details and an example

https://arxiv.org/pdf/2107.09021.pdf


问:
Takeoff service not working as expected
I'm simulating Iris quad sing px4+mavros+gazebo. I'm trying to get my drone to takeoff using python code. When I publish height using mavros/setpoint_position/pose, it takes off. But I have one problem. Whenever my other code finishes its job, this takeoff code takes over and sends it back. To eliminate the use of setpoints for takeoff, I'm trying to use the takeoff service. But it's not working as expected.
Ubuntu 18.04
Python 2.7
Px4

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

import rospy
import mavros
import time

from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import *
from mavros_msgs.srv import *

class Services():

    def __init__(self):
        self.local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
        self.current_state = State()

    def state_cb(state):
        global current_state
        current_state = state

    def arm(self):
        rospy.wait_for_service('/mavros/cmd/arming')
        try:
            arm = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
            arm(True)
        except rospy.ServiceException, e:
            print "Service arm call failed: %s"%e

    def offboard(self):
        rospy.wait_for_service('/mavros/set_mode')
        try:
            flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode)
            flightModeService(custom_mode='OFFBOARD')
        except rospy.ServiceException, e:
            print "Service set_mode call failed: %s. Offboard Mode could not be set."%e

    def takeoff(self, height=3):
        rospy.wait_for_service('mavros/cmd/takeoff')
        try:
            takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', CommandTOL)
            takeoffService(altitude = height)
        except rospy.ServiceException, e:
            print "takeoff call failed: %s"%e

    def land(self):
        rospy.wait_for_service('/mavros/cmd/land')
        try:
            l = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
            l(altitude = 0)
        except rospy.ServiceException, e:
            print "Service land call failed: %s"%e

#!/usr/bin/env python

import rospy
import mavros
import time

from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import *
from mavros_msgs.srv import *
from services import Services

current_state = State()

def state_cb(state):
    global current_state
    current_state = state

def command():
    service = Services()
    state_sub = rospy.Subscriber('/mavros/state', State, state_cb)
    rospy.init_node('mavros_takeoff_python')
    rate = rospy.Rate(10)

    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2

    for i in range(100):
        service.local_pos_pub.publish(pose)
        rate.sleep()

    prev_state = current_state
    while not current_state.connected:
        rate.sleep()

    last_request = rospy.get_rostime()
    while not rospy.is_shutdown():
        now = rospy.get_rostime()
        if current_state.mode != "OFFBOARD" and (now - last_request > rospy.Duration(5.)):
            service.offboard()
            last_request = now
        else:
            if not current_state.armed and (now - last_request > rospy.Duration(5.)):
               service.arm()
               last_request = now

        # older versions of PX4 always return success==True, so better to check Status instead
        if prev_state.armed != current_state.armed:
            rospy.loginfo("Vehicle armed: %r" % current_state.armed)
        if prev_state.mode != current_state.mode:
            rospy.loginfo("Current mode: %s" % current_state.mode)
        prev_state = current_state
        service.takeoff(3)
        rate.sleep()

if __name__ == '__main__':
    try:
        command()
    except rospy.ROSInterruptException:
        pass
</code>```

This is the output I get when using takeoff service [INFO] [1629208144.895857, 700.624000]: Current mode: AUTO.TAKEOFF.
This output is for position pub [INFO] [1629206964.285276, 93.652000]: Current mode: OFFBOARD [INFO] [1629206969.289668, 98.652000]: Vehicle armed: True
Advance thanks for the help.

Edit 1 - I have another motion planning codes which should takeover after this code gets my drone to takeoff. So when I publish velocity setpoints or other setpoints, this code should not interfere with the other code. So I think if I use the takeoff service instead of setpoint_position, there won't be any problem. But the drone is not taking off when I use the takeoff service.

Presently I'm using the following code which interferes when i publish other setpoints.

```<code>def takeoff(self, height=3):
    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 3
    try:
        self.local_pos_pub.publish(pose)
    except rospy.ServiceException, e:
        print "takeoff call failed: %s"%e
</code>```

Title: Takeoff service not working as expected


问:
Building a wired remote control car using a raspi and drone motors
I have a small raspi controlled remote control car using regular drone motors such as these ones and tried using this 60w power supply unit that can be downregulated to 3v and connected everything using two drv8833 motor drivers. However, the spikes in power drainage generated from one motor alone seem to be enough to drain the entire amperage of the circuit, in spite of each motor only consuming 0,15a. Is there a reason for this?

It's possible that there is some additional resistance when the motors start up causing them to drain more than the specified 0,15a, I am not using any gears but the car itself is very light so I don't think the spike in amperage drainage justifies all the other motors completely stalling when the power source is said to provide 60W.

The current limit for the DRV8833 is 2A, I am using two of those modules with each having two motors connected to them.

Is there a good solution to this problem? I tried placing a 3.300uF 6.3V capacitor around the motor driver but am still having problems with only one of the motors actually starting up


问:
ROS Camera Calibration for Webcam - Not working
As a part of learning, I am trying to perform Camera calibration with my laptop's Webcam. I am really new to this process. I will explain everything about how far I have come so far, and where I have issues in coming to an successful calibration.

My system specifications: Ubuntu 18.04, ROS Melodic.

For Camera streaming, I am using usb_cam ros package http://wiki.ros.org/usb_cam. And it is working perfectly fine.

After I launch the usb_cam-test.launch, I looked into the rostopic list, where I could find the following two topics: /usb_cam/image_raw /usb_cam/camera_info

So, I progressed forward to perform the camera calibration with the ros package "camera_calibration", but instead of getting a camera stream with calibration options as given in the tutorials, I get a black screen like below:

https://user-images.githubusercontent...

This calibration task is crucial to perform a task at my work. So, I would really need to get some help from someone who has been through this issue before. Thank you very much for the help.


问:
Is feedback control fundamentally efficient than open-loop control?
Is feedback control more efficient in principle than open-loop control? Is there a name for the principle? (I'd like to do an internet search to understand that theorem.)

Under ideal conditions, ignoring the power and processing time required for feedback, intuitively it seems obvious that feedback control is always more efficient than open loop control.

I believe this has been established with mathematical rigor in control theory as a key principle, but have not been able to find it in any textbooks I have.

I am new to control theory, so apologies if this is a silly question.

Update

I think of efficiency in terms of the following problem. (See also figure)

Concrete problem setting

Gas particles are enclosed with probability 0.5 on the left or right side of a rigid box (like a cylinder) separated by a movable wall. There are two destinations to move the center wall, the right and left side edges of the box.

If you move the center wall toward the correct direction, you can get the energy out of this box. However, in the wrong direction, you will lose the energy.

We consider two types of controllers, feedback and open-loop controller. Both of the controllers can move the wall to the destinations. So, it is not necessary to observe the states after starting moving wall.

A feedback controller observes only the initial state of the box to know which direction the gas particles are enclosed (the left or right side in the box). Thus, the feedback controller knows which direction is appropriate to extract the energy from the box.

An open-loop controller does not observe the state of the box.

We assume that the box and wall works as an ideal engine often appeared in thermodynamics. So the controller can move the wall very smoothly (quasi-statistically) without feedback.

We can summarize our setting as followings:

Efficiency

We now define efficiency $\eta$ by the following equation:

$$\eta = \frac{\text{Increase in potential energy of the weight}}{\text{energy consumption during the control}}$$

Because the initial and final state of the weight is the same in any case, the numerator above is constant.

The fuels used can be different because the pressure from the gas is available to lift up the weight by moving the right direction.

Since a feedback controller can know the right direction by its observation, she moves the wall toward the right direction. Then the fuel is zero in our ideal setting.

Meanwhile, becuase a open-loop controller do not know which direction is appropriate, the fuel is necessary to lift up the weight.

Therefore, the efficiency $\eta$ for the feedback controller is greater than one for the open-loop controller in principle.

FB control is more efficient in principle?

But, this inference is not rigorous because there might be some tricky methods that allows the open-loop controller to lift up the weight with zero-fuel. (I could not find it, though)

My question is that whether there is the theory that confirms that there is no such tricky strategy in general and justifies our intuition.


问:
Decidability of Pursuit-Evasion games and self driving cars
I was wondering if there is any theory describing the decidability of a continuous pursuit-evasion game with constraints.

Scenario:

Two cars approach each other on a street with two lanes. Let's say one car is hostile and wants to cause a collision, while the other car tries to avoid one.

Under the assumptions that:

The cars cannot leave the street

None of the cars can change directions (too dangerous with traffic). So in their coordinate system $v_x>0$

There is a terminal velocity $v_T$  they can not exceed

The acceleration and deceleration of both cars are limited. So $|\vec{a}| \in
    \left[a_B(v_x), a_F(|\vec{v}|) \right]$ where $a_B$ is a negative backward acceleration from breaking and $a_F$ a forward acceleration. Both would need to be dependent on velocity, otherwise $ v_x < 0$ or $ |\vec{v}| > v_T $ would be possible. Same for the other car with switched signs, e.g. assuming the cars are from the same model.

Question:

Would the hostile car always win? Is there some theory on that problem, or does a strategy come in your mind to avoid such collisions?

Example what I have in mind:

If you consider a more simple scenario in 1-d, drop assumption (2.) and allow for backward acceleration equal to forward acceleration the scenario should become decidable based on the initial distance and velocity of the cars. If the cars are very far apart the evading car can just accelerate to terminal velocity away from the pursuing car and it will never reach it. If they are to close it is obviously not possible to pull that move and it is only in the power of the pursuing car to cause or avoid a collision.

Context:

I was wondering if hostile cars are considered in current research on self-driving cars or if everything is based on a non-hostile environment and that question came to my mind.


问:
How to calibrate a lens with wide fov and low F number
To calibrate a wide fov lens either a calibration board should be large or bring the calibration board closer to the lens. As making a large calibration board is expensive, I usually prefer the second solution but making a short distance does not work for a lens with low F number(e.g F1.2). Simply, the image will be out focused.

Does anyone know an open source calibration program for this situation?

For example, multiple small calibration boards can be used instead of a large single board. Which open source support it?


问:
C program to find the rotation matrix (orientation) of a 6DOF robot from its end effector position and link parameters
I'm writing a C program to find the inverse kinematics of a 6DOF robot. I've been able to write it successfully based on the general concept of Inverse Kinematics using DH parameters (i.e. finding the joint angles of the robot from its known orientation and position).

But I only have the values of the position of the robot's end effector, and link parameters (link lengths, link twists and the distance between links). How do I calculate the orientation of the robot's end effector with these values?

I'll be glad if someone can help. I'm also open to further questions for clarifications. Thank you in advance.


问:
Focus calibration to infinity
Well, it sounds easy problem but can we do it without pointing the camera to a faraway place?
How does the manufacturer do this during mass production? Is there any focusing machine?</p>

Title: Focus calibration to infinity


问:
Joint space trajectory script unclear
I have a script provided by The MathWorks Inc. that makes a robot end-effector follow both a task space and a joint space trajectory through several waypoints. These trajectories are then plotted in the same figure (shown below). For some reason the joint space trajectory makes these huge loops. Can anybody explain this behaviour? I do know that the difference between joint space and task space is that in joint space, you know the start and last position of the joint configuration from inverse kinematics, and interpolate it. While in task space you know start and last position of end-effector, then interpolate it and the inverse kinematics in every point in the path to get the joint configuration for every point in the path.

Shown below is the part of the script that creates the joint space trajectory. I hope this can help somebody explain the weird curvy behaviour.

```<code>ikInitGuess = jointAnglesHome';\
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;\
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

% Solve Inverse Kinematics for all waypoints\
numWaypoints = size(waypoints,2);\
numJoints = numel(Robot.homeConfiguration);\
jointWaypoints = zeros(numJoints,numWaypoints);\
for idx = 1:numWaypoints\
    tgtPose = trvec2tform(waypoints(:,idx)');\
    [config,info] = ik(end-Effector-Name,tgtPose,ikWeights,ikInitGuess);\
    cfgDiff = config - ikInitGuess;\
    jointWaypoints(:,idx) = config';\
end

% Trajectory Generation\
[qJoint,qdJoint,qddJoint] = trapveltraj(jointWaypoints,numel(trajTimes), ...\
    'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
    'EndTime',repmat(diff(waypointTimes),[numJoints 1]));

%Trajectory evaluation (only needed to find end effector position)\
for idx = 1:numel(trajTimes)
    eeTform = getTransform(gen3,qJoint(:,idx)',eeName);\
    posJoint(:,idx) = tform2trvec(eeTform)';\
end
</code>```

Thanks in advance for any help.

The complete script, This script also requires the CreateWaypointData script, which is shown in a different segment at the end.

```<code>% Compares joint space vs. task space trajectories
% Copyright 2019 The MathWorks, Inc.

%% Setup
clear; close; clc;
createWaypointData;
figure, hold on
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko:','LineWidth',2);
title('Trajectory Waypoints');
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
grid on
view([45 45]);

% Define IK solver
ik = inverseKinematics('RigidBodyTree',Panda_ML);
ikWeights = [1 1 1 1 1 1];
% Use a small sample time for this example, so the difference between joint
% and task space is clear due to evaluation of IK in task space trajectory.
ts = 0.02;
trajTimes = 0:ts:waypointTimes(end);
% Initialize matrices for plots
qTask = zeros(numJoints,numel(trajTimes)); % Derived joint angles in task space trajectory
posJoint = zeros(3,numel(trajTimes)); % Derived end effector positions in joint space trajectory

%% Create and evaluate a task space trajectory
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

disp('Running task space trajectory generation and evaluation...')
tic;

% Trajectory generation
[posTask,velTask,accelTask] = trapveltraj(waypoints,numel(trajTimes), ...
    'AccelTime',repmat(waypointAccelTimes,[3 1]), ...
    'EndTime',repmat(diff(waypointTimes),[3 1]));

% Trajectory evaluation
for idx = 1:numel(trajTimes)
    % Solve IK
    tgtPose = trvec2tform(posTask(:,idx)');
    [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
    ikInitGuess = config;
    qTask(:,idx) = config;
end

taskTime = toc;
disp(['Task space trajectory time : ' num2str(taskTime) ' s']);

%% Create and evaluate a joint space trajectory
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

disp('Running joint space trajectory generation and evaluation...')
tic;

% Solve IK for all waypoints
numWaypoints = size(waypoints,2);
numJoints = numel(Panda_ML.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);
for idx = 1:numWaypoints
    tgtPose = trvec2tform(waypoints(:,idx)');
    [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
    cfgDiff = config - ikInitGuess;
    jointWaypoints(:,idx) = config';
end

% Trajectory Generation
[qJoint,qdJoint,qddJoint] = trapveltraj(jointWaypoints,numel(trajTimes), ...
    'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
    'EndTime',repmat(diff(waypointTimes),[numJoints 1]));

% Trajectory evaluation (only needed to find end effector position)
for idx = 1:numel(trajTimes)
     eeTform = getTransform(Panda_ML,qJoint(:,idx)',eeName);
     posJoint(:,idx) = tform2trvec(eeTform)';
end
jointTime = toc;
disp(['Joint space trajectory time : ' num2str(jointTime) ' s']);

%% Create comparison plots
% Compare trajectories in Cartesian space
close all
figure, hold on
plot3(posTask(1,:),posTask(2,:),posTask(3,:),'b-');
plot3(posJoint(1,:),posJoint(2,:),posJoint(3,:),'r--');
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko','LineWidth',2);
title('Trajectory Comparison');
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
legend('Task Space Trajectory','Joint Space Trajectory','Waypoints');
grid on
view([45 45]);
</code>```

CreateWaypointData:

```<code>% Create sample waypoint data for trajectory generation

%% Load robot Panda
load panda_ML

%% Common parameters
% Rigid Body Tree information
load Panda_MLpositions
eeName = 'panda_link8';
numJoints = numel(Panda_ML.homeConfiguration);
ikInitGuess = zeros(1,(numel(Panda_ML.homeConfiguration)));

% Maximum number of waypoints (for Simulink)
maxWaypoints = 20;

% Positions (X Y Z)
waypoints = toolPositionHome' + [-.1 -.2 -.3; -.3 -.5 -.2; -.6 -.5 0; -.9 -.4 .1; -1.1 -.6 .2]';

% Euler Angles (Z Y X) relative to the home orientation
orientations = [0 0 0; pi/8 0 0; 0 pi/2 0; -pi/8 0 0; 0 0 0]';

% Array of waypoint times
% Increase the speed of the robot
waypointTimes = 0:4:16;
% waypointTimes = [1 2 3 4 5 6 7 8 9 10];

% Trajectory sample time
ts = 0.2;
trajTimes = 0:ts:waypointTimes(end);

%% Additional parameters

% Boundary conditions (for polynomial trajectories)
% All are chosen arbitrarily.
% Velocity (cubic and quintic)
waypointVels = 0.1 *[0 1 0; -1 0 0; 0 -1 0; 1 0 0; 0 1 0]';

% Acceleration (quintic only)
waypointAccels = zeros(size(waypointVels));

% Acceleration times (trapezoidal only)
waypointAccelTimes = diff(waypointTimes)/4;
<span class="math-container">```
```

Title: Joint space trajectory script unclear


问:
How to drive a motor by Python
I need to drive any small motor (with small delay) by Python without low level programming. What hardware and software I need? Does "Robot Mesh Studio" fit for this? I want to make a robotic friend to my dog but don't want him to damage a rospberry pi. I am on my start on robotics science so I didn't try anything.</p>

Title: How to drive a motor by Python


问:
Pure Pursuit controller slow response for long look ahead
The pure pursuit calculates the steering angle based on that equation:

$$\delta = \tan^{-1}\left(\frac{2L\cdot \sin(\alpha)}{ld}\right)$$
Where $L$ is the wheelbase, $\alpha$ is the target angle, and $ld$ is the look-ahead distance.

But as $ld$ is bigger, the controller decreases the steering angle... When $ld$ is big, the internal term became zero and the steering angle became zero. For example, say my car has $L=1$ and its in $(0,0)$ and I have two target points $t_1=(10, 10)$ and $t_2=(100, 100)$. In both cases $\alpha=45^{\circ}$

For $t_1$: $\delta = \tan^{-1}\left(\frac{2\cdot \sin(45^{\circ})}{14.142}\right) = 5.7106^\circ$

For $t_2$: $\delta = \tan^{-1}\left(\frac{2\cdot \sin(45^{\circ})}{141.42}\right) = 0.57106^\circ$

So, even though the points have the same angle, the steering angle for $t_2$ is very weak.

What is the best practice of choosing the look ahead distance? I want it to look far, but response quickly enough. The track has limited width, too weak steering angle (like in $t_2$) makes the robot getting off track...


问:
IK Jacobian is always a singular matrix
I am trying to solve an inverse kinematics equation for 6DOF system, 3 rotation and 3 prismatic joints. I am trying to solve using perturbation method, by adjusting each joint and calculating the final Euler angels and transformation.

The math is as follows:

```<code>// 1.  Save min and max (position or rotation) variable Θ for each of N joints, start with each joint in the middle position, choose Δ to be some small value
    //Compute Jacobian
// 2.  Find transformation matrices for current value Θ for each joint (Call them #M matrix)
// 3.  Find transformation matrices for Θ+Δ and Θ-Δ of each joint independently from the current position (Call them #M+ and #M- matrices)
// 4.  Multiply (1M+)·(2M)·(3M)·(4M)... and (1M-)·(2M)·(3M)·(4M)...  to get F1M+ and F1M- matrices
// 5.  Convert F1M+ and F1M- to quaternion and translation vectors, combine into 1 vector [qw, qx, qy, qz, x, y, z]
// 6.  Subtract (F1M+)-(F1M-) vectors
// 7.  These 7 variables become first column of the Jacobian matrix
// 8.  Repeat steps 4-7 for 2M+-, 3M+-, etc to complete jacobian matrix with N columns and 7 rows
    //Compute Error
// 9.  Compute (1M)·(2M)·(3M)·(4M)... convert to quaternion plus translation vector denote as 7 axis vector S
// 10. Take end effector matrix, convert to quaterion plus translation vector denote as 7 axis vector T (only needs to be done once)
// 11. Find E = T - S
    //Update Θ
// 12. Solve equation E = J · ΔΘ for ΔΘ using Jᵀ·E = Jᵀ·J·ΔΘ  then (Jᵀ·J)¯¹·Jᵀ·E = ΔΘ
// 13. Add ΔΘ to current Θ

// 14. Repeat Steps 2-13 Until E is sufficiently small or T¯¹·S is close enough to identity matrix
</code>```

In the code below, I am using euler angles instead of quaternion, and I am also not looking at end effector rotation to simplify

The problem is when I calculate Jᵀ·J (Matrix j_T_j below) I always get a matrix with a zero determinant

```<code>    Final S

1.000000   0.000000   -0.000000   0.000000
0.000000   1.000000   0.000000   127.000000
-0.000000   0.000000   1.000000   -0.000000
0.000000   0.000000   0.000000   1.000000

Joint ptb# 0

1.000000   0.000000   -0.000000   -0.100000
0.000000   1.000000   0.000000   127.000000
-0.000000   0.000000   1.000000   0.000000
0.000000   0.000000   0.000000   1.000000

Joint ptb# 1

1.000000   0.000000   -0.000000   -0.000000
0.000000   1.000000   0.000000   127.000000
-0.000000   0.000000   1.000000   -0.100000
0.000000   0.000000   0.000000   1.000000

Joint ptb# 2

1.000000   0.000000   -0.000000   -0.000000
0.000000   1.000000   0.000000   127.100006
-0.000000   0.000000   1.000000   0.000000
0.000000   0.000000   0.000000   1.000000

Joint ptb# 3

1.000000   -0.000000   -0.000000   -0.000000
0.000000   0.995004   0.099833   126.746216
-0.000000   -0.099833   0.995004   -5.071538
0.000000   0.000000   0.000000   1.000000

Joint ptb# 4

0.995004   0.000000   -0.099833   0.000000
0.000000   1.000000   0.000000   127.000000
0.099833   -0.000000   0.995004   -0.000000
0.000000   0.000000   0.000000   1.000000

Joint ptb# 5

0.995004   -0.099833   -0.000000   0.000000
0.099833   0.995004   0.000000   127.000000
-0.000000   0.000000   1.000000   -0.000000
0.000000   0.000000   0.000000   1.000000

v_T =
 [0;
 0;
 -100]

v_S =
 [2.273736754432321e-13;
 127;
 -1.355252715606881e-20]

v_E =
 [-2.273736754432321e-13;
 -127;
 -100]

v_jacobian =
 [-1, -1.192092895507813e-07, -5.960464477539063e-08, -3.022872147515284e-06, 7.563471514076884e-08, 0;
 0, 0, 1.00006102025418, -2.537841759058211, 0, 0;
 5.960464477539063e-08, -1, 3.552713678800501e-15, -50.71537895924779, -1.511437494843092e-06, 0]

j_T =
 [-1, 0, 5.960464477539063e-08;
 -1.192092895507813e-07, 0, -1;
 -5.960464477539063e-08, 1.00006102025418, 3.552713678800501e-15;
 -3.022872147515284e-06, -2.537841759058211, -50.71537895924779;
 7.563471514076884e-08, 0, -1.511437494843092e-06;
 0, 0, 0]

j_T_j =
 [1.000000000000004, 5.960464477539063e-08, 5.960464477539084e-08, 0, -7.563480522946383e-08, 0;
 5.960464477539063e-08, 1.000000000000014, 3.552713678800501e-15, 50.71537895924815, 1.511437485826732e-06, 0;
 5.960464477539084e-08, 3.552713678800501e-15, 1.000122044231834, -2.537996618807416, -4.508185698358049e-15, 0;
 0, 50.71537895924815, -2.537996618807416, 2578.490303774142, 7.665312509554948e-05, 0;
 -7.563480522946383e-08, 1.511437485826732e-06, -4.508185698358049e-15, 7.665312509554948e-05, 2.290163910951988e-12, 0;
 0, 0, 0, 0, 0, 0]

j_T_j determinant =
 0

j_T_j_inv =
 [0, 0, 0, 0, 0, 0;
 0, 0, 0, 0, 0, 0;
 0, 0, 0, 0, 0, 0;
 0, 0, 0, 0, 0, 0;
 0, 0, 0, 0, 0, 0;
 0, 0, 0, 0, 0, 0]

j_T_j_inv_j_T =
 [0, 0, 0;
 0, 0, 0;
 0, 0, 0;
 0, 0, 0;
 0, 0, 0;
 0, 0, 0]

d_theta =
 [0;
 0;
 0;
 0;
 0;
 0]
</code>```

Title: IK Jacobian is always a singular matrix


问:
HECTOR SLAM, odometry in a low cost differential drive robot
I'm a noob in robotics trying to evaluate requirements for a low cost differential drive indoor robot. Would highly appreciate comments on the following questions.

Given the inaccuracies in the wheel odometry (speed mismatch between motors, mismatch in wheel dimensions, unequal wear and tear of the two wheels with time and wheel slipping),

a) Is it fair to assume that a "Lidar (Ex: RPLIDAR A1M8 360 Degree 2D Laser Range Lidar) only" HECTOR SLAM solution will be good enough for an indoor robot? and should be the goto solution not needing wheel odometry.

b) Another doubt I have about the SLAM is let's say the robot has the environment MAP that it had crated earlier. Can it be cold started (switched ON) at any location within the MAP and it will be able to determine its position or does it have to always cold start from a "fixed point" in the MAP?

Thanks in advance,


问:
Obtain a State Space System given Denavit-Hartenberg parameters and a Rigid Body Tree model in MATLAB
I'm confused regarding the representatoin of a 6 D.O.F. robot arm with multiple links. I have the denavit-Hartenberg parameters and have used these to create a Rigid Body Tree model in MATLAB. The model works perfectly in MATLAB and is capable of tracking a trajectory in Task Space. The next step is to implement a Model Predictive Controller. But in order to a apply a Model Predictive Control sheme I need the state space representation.

How can i get from the Denavit-Hartenberg parameters to a state space representation?