问:
Depth map with Raspberry Pi
Is it possible to get two images from the Raspberry Pi camera mounted on a remote controlled bot and have them sent to a computer through Wi-Fi and process the images in the computer to generate a depth map?

All this is to be done in a very short time so that the robot can be helped with its locomotion without making it completely autonomous.

答:
I am currently using the Compute Module IO board for my undergraduate project and up to now, the best I can come up with is a refresh rate of 2Hz implemented using c++.

Mr. David Barker at Argon design was able to implement it at 1Hz using C and 12 frames per second by using machine language on the video core.

Mr. Nalan Karunanayake was able to implement stereo vision using a model B+ and two usb webcams set at 10 frames per second in c++.

I read an IEEE paper if I am remembering it correctly being able to sync both camera modules using v4l2 drivers (Raspberry PI Based Stereo Vision For Small Size ASVs†).

† Requires IEEE Xplore subscription or purchase.

Title: Depth map with Raspberry Pi


问:
Quadcopter refuses to fly when the Yaw PID component is added
Good day,

I would like to ask why is it that when I add the Yaw control to my PID controller for each motor. The quadcopter refuses to take off or maintain its altitude. I am curently using a Cascaded PID controller for attitude hold using an Accelerometer, a Magnetometer and a Gyroscope, and a 40Hz Ultrasonic Sensor for Altitude Hold. Since the scope is indoor I have done away with the barometer due to its +-12m error.

Resulting Response

Without Yaw Control, the plot below shows the response of the quadrotor.

With Yaw Control, the plot below shows the response of the quadrotor.

Debugging

I found out that each of the outputs from each PID's give a too high of a value such that when summed together goes way over the PWM limit of 205 or Full Throttle.

Without yawPID contribution
The limiter kicks in without damaging the desired response of the system thus is still able to fly albeit with oscillatory motion along the z axis or height

With yawPID contribution
The added yaw components increases the sum of the PID's way above the limit thus the limiter compesates the excess too much resulting in an over all lower PWM output for all motors thus the quad never leaves the ground.

//Motor Front Left (1)
float motorPwm1 =  pitchPID + rollPID + yawPID + baseThrottle + baseCompensation;

//Motor Front Right (2)
float motorPwm2 =  pitchPID - rollPID - yawPID + baseThrottle + baseCompensation;

//Motor Back Left (3)
float motorPwm3 = -pitchPID + rollPID - yawPID + baseThrottle + baseCompensation;

//Motor Back Right (4)
float motorPwm4 = -pitchPID - rollPID + yawPID + baseThrottle + baseCompensation;

Background

The PID parameters for the Pitch, Yaw and Roll were tuned individually meaning, the base throttle was set to a minimum value required for the quadcopter to be able to lift itself.

The PID parameters for the Altitude Sensor is tuned with the other controllers active (Pitch and Roll).

Possible Problem

Limiter algorithm

A possible problem is that the algorithm I used to limit the maximum and the minimum throttle value may have caused the problem. The following code is used to maintain the ratio of the motor values instead of limiting them. The code is used as a two stage limiter. In the 1st stage, if one motorPWM is less than the set baseThrottle, the algorithm increases each motor PWM value until none of them are below that. In the 2nd stage, if one motorPWM is more than the set maxThrottle, the algorithm decreases each motor PWM value until none of them are above that.

```<code>//Check if PWM is Saturating - This method is used to fill then trim the outputs of the pwm that gets fed into the gpioPWM() function to avoid exceeding the earlier set maximum throttle while maintaining the ratios of the 4 motor throttles.

float motorPWM[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
float minPWM = motorPWM[0];
int i;
for(i=0; i<4; i++){ // Get minimum PWM for filling
    if(motorPWM[i]<minPWM){
        minPWM=motorPWM[i];
    }
}

cout << " MinPWM = " << minPWM << endl;

if(minPWM<baseThrottle){
    float fillPwm=baseThrottle-minPWM; //Get deficiency and use this to fill all 4 motors
    cout << " Fill = " << fillPwm << endl;
    motorPwm1=motorPwm1+fillPwm;
    motorPwm2=motorPwm2+fillPwm;
    motorPwm3=motorPwm3+fillPwm;
    motorPwm4=motorPwm4+fillPwm;
}

float motorPWM2[4] = {motorPwm1, motorPwm2, motorPwm3, motorPwm4};
float maxPWM = motorPWM2[0];
for(i=0; i<4; i++){ // Get max PWM for trimming
    if(motorPWM2[i]>maxPWM){
        maxPWM=motorPWM2[i];
    }
}

cout << " MaxPWM = " << maxPWM << endl;

if(maxPWM>maxThrottle){
    float trimPwm=maxPWM-maxThrottle; //Get excess and use this to trim all 4 motors
    cout << " Trim = " << trimPwm << endl;
    motorPwm1=motorPwm1-trimPwm;
    motorPwm2=motorPwm2-trimPwm;
    motorPwm3=motorPwm3-trimPwm;
    motorPwm4=motorPwm4-trimPwm;
}
</code>```

This was obtained from pixhawk. However the difference is that they employ only upper bound compensation limiting, while mine also performs lower bound compensation limiting which may cause more saturation once it reaches the second stage.

From:https://pixhawk.org/dev/mixing

Gains are set too high.

It is also possible that I've set my P gains too high thus exceeding the max RPM limit of the motors causing the Limiter algorithm to overcompensate.

Current PID Settings:

The minimum motor value for the quad to lift itself is 160 while the maximum limit is 200 from the PWM time high of 2000ms

Pitch (Cascaded P-PID controller)
Rate P = 0.07
Rate I = 0.03
Rate D = 0.0001
Stabilize P = 2

Roll (Cascaded P-PID controller)
Rate P = 0.09
Rate I = 0.03
Rate D = 0.0001
Stabilize P = 2

Yaw (Cascaded P-PID controller)
Rate P = 0.09
Rate I = 0.03
Rate D = 0.0001
Stabilize P = 2

Hover (Single loop PD controller)
P = 0.7
D = 35

Possible Solution

I think I have set the PID parameters particularly the P or D gain too high that the computed sum of the outputs of the controller is beyond the limit. Maybe retuning them would help.

I would just like to ask if anyone has encountered this problem or if you have any suggestions. Thank you :)

EDIT

I have added the plots of the response when the control loop is fast (500Hz) and Slow (300Hz)

500Hz: Does not fly

300Hz: Flies

答:
disclaimer: This is more of a longer comment than an answer. I'm not sure what the problem is. There is a lot going on here (thankfully a lot of information is provided, too) and my attention shifted around from one thing to others.

my original thought

from the comments

There's not much between 160 min value and 200 max.
  In either case (with or without the yaw controller), you are constantly hitting the limit as I see it in your diagram.
  The more often you limit the controller value, the more it is actually the limiter acting as a (nonlinear!) controller and not the controller itself.
  A limiter should kick in occasionally and not all the time.

This is mostly based on how clearly the limit cuts down the PWM:

hence the following analogy came to my mind:

Strap a belt tightly around your breast and run a marathon.

And I concluded that:

I'd try to use stronger motors so that your controller has enough air to do its job and not hit the limit so often.

A stronger motor would not have run at full power and would not hit the limit so often. Hitting the limit is not bad in of itself. Imagine your PWM would be either at the upper or the lower limit. You'd have a (nonlinear) bang-bang controller. Such a controller isn't bad in of itself either, but it somewhat overrides the PID controller.

It can happen that you can manipulate the PID parameters at will and nothing changes. One reason for that can be a limit that makes your changes irrelevant. New parameters might change the output from 260 to 280 or to 220, which will have to effect if they are all going to be limited to 200.
This is why constantly hitting the limit can be a bad thing.

With the yaw part, the phenomenon seems to be more pronounced, but not by much. I guess this is something to be aware of especially when summing up controller values, but not the actual problem.

Anyway, from the given diagrams I cannot tell why one would fly and the other would not.

500Hz -> 300Hz

Then another attempt was made and again one configuration works and the other does not.

I have tried to run the control loop at a slower rate and noticed that there is a drastic change in each of the PID components such that it is not exceeding the limit too much and now its flying again.

I tried to find differences between both diagrams. You are right, the limit is not exceeded as often as before in the flying operation. But what else is different?

May I ask what happens to the P gain contribution to the output if the speed of the control loop is decreased (dT increased). Does it have any significant effect like on the I and D gain which is time dependent (D decreases I increases)

Now I had a look at the separate controller error values. I combined them into the following image for comparison:

The first thing that I notice is that there's a steady state error and thus $I_{error}$ constantly grows aka the red is the integral of blue and blue is bigger than zero all the time. This is not a good thing.

If there's a steady state error that the P gain cannot compensate, the integral part should grow and compensate the constant offset error.

The integral part should converge to some value. At least if it's compensating for a constant offset. I don't know what integral error from what controller this is, but an error that keeps growing is undesirable.

But maybe there is something inherently different between fly and nofly?
The diagrams are a little hard to read because the axis are of different scale.

I tried to scale the images in gimp to fit and this is the result:

The pixelated plots are those two when it did not fly.
Their overall $P_{error}$ is bigger and thus the $I_{error}$ is steeper.

But I don't think this is the problem. Look at the difference between the two $I_{error}$ of the cases when it flew. (the non-pixelated ones)

I'd say there's no other significant difference between the two cases.

conclusion

There's a constant error in the system. The controller cannot reduce that error. It looks as if the control loop is not closed properly.

Take a step back.
Even in your first image, the height value oscillates and $I_{error}$ increases constantly. Inspect what's going on there.

Then create the angle controller separately. Put the quadcopter onto a pivot point or pole and see if it can maintain its angle. The height controller should be turned off.

When both controllers work on their own, try to combine both controllers in a weighted sum. $PID_{total} = 0.5 PID_{yaw} + 0.5PID_{height}$ Change the ratio as you see fit.

ContentLicense: CC BY-SA 3.0


问:
What rating Li-Po battery should I get for this configuration?
I will have this configuration:

A2212 Brushless Motor 1000KV - 4 each

ECS - 30A Electronic Speed Control (ESC) - 4 each

Propeller - 1045 Propeller CW & CCW Pair 10 inch * 4.5 pitch

Arduino Mega - 2560 board

Raspberry Pi 3

Open pilot CC3D flight controller

I want to know what rating Li-Po battery should I get for this configuration.
The reason behind my asking here is because a simple google search is not able to satisfy me with an explanation...

Also, my weight will be 1.5 kg for the quadcopter, so I need a stable current discharge.

This is my first quadcopter, I am a Computer Science guy, so I have little knowledge of electronics, I'm learning, but need help...

答: Firstly its important to consider the worst case current draw by your motors. Since this determines how fast your battery will drain out. The size of the battery will be determined by its capacity and cell count. Generally bigger batteries like in laptops with their 4-cell 48Wh can power them up to an hour under intensive load. Which brings me to mention that one of the factors is how long do you want the quad to run. Though the laptop batteries run long, they are big in size so depending on the size of your quad, the battery size will have to be chosen. Again since, the battery adds weight, motors will draw more current, thus inline with the first point, determine the peak current draw of the entire system. I used motors since it draws the most. You can find more info on how to choose a LiPo battery here</p>


问:
How do we write a STOP to a continuous Servo?
I'm using processing to send strings to Arduino, using functions like

```<code>else {
    int u=90;
    port.write(u+"z");
}
</code>```

on the processing side and in the Arduino side I'm using calls like

```<code>  case 'z':
    z.write(v);
    v = 0;
    break;
  case 'L':
    z.write(0);
    //v = 0;
    break;
}
</code>```

yet I can't get the servo to stop at all. How do I make it shut off?

If it was a regular servo I wouldn't even ask because that's easy but I write 0 or 90 or LOW and nothing, it just keeps spinning in one direction but when it meets one of the conditions in my statements it switches polarity/direction and that's good - I want that but I made this function to make it stop and it is not doing so, does anyone have any ideas ?

I am using a Parallax Continuous Rotation Servo.

答:
According to the data sheet and example code, this continuous rotation servo takes an input of 1.3ms full speed clockwise to 1.7ms full speed counter-clockwise, with 1.5ms in the middle being stopped.

As jwpat suggests you are better off using Servo.writeMicroseconds(1500) to stop your servo, since this guarantees that you are sending the correct value, even if you accidentally get the min and max calibration values wrong in your Servo object.


问:
How to know the payload of the chassis from its motors?
I'm doing a mobile robot project with robotic arms, I wanted to buy a chassis for my robot that can carry enough weight, but many websites don't give definitive answers about maximum payload.

Is there is a way to figure this out just by knowing details about the motors?

答:
If I told you my truck has 200 horsepower, could you tell me the cargo weight capacity?  Not really.   There are more factors than just the motor specs that play into your assessment.   You might be able to ball-park it, but be prepared to be off by a factor of 2 or more.   Here's why...

A motor needs current to produce torque.   The available power supply current comes into play.

Total power available depends on both current and voltage.   If you run your motor at a higher voltage (without saturating the back iron) you will have more power available for a given current.

Every motor has a torque vs speed inverse relationship.   How fast you want the device to go affects how much torque each motor can generate.

The gear trains (or other transmission) and frictional losses also affect the capability to operate at given torque and speed.

You can use the ratings of the motors to get you in the ballpark.   They can tell you the maximum power available to be transmitted to the device.   However, the other factors play more than a nominal role.


问:
Best sensor to determine "up" versus "down"
I want to start designing an Arduino project and have telemetry readings that indicate tilt or angle of placement. </p>

<p>Would an accelerometer be the best for determining tilt? Are there good tutorials? </p>

答:
From my understanding a simple accelerometer and magnetometer works.

Currently I use a 9DOF grove IMU. It has an I2C connection and contains both of those plus a gyroscopic sensor.

They've got this github for examples too, see Seeed-Studio/Grove_IMU_9DOF.


问:
Dynamic Model of a Manipulator
I'm stuck on equation 4.30 of page 176 in

http://www.cds.caltech.edu/~murray/books/MLS/pdf/mls94-complete.pdf

This equation:

$\frac {\partial M_{ij}} {\partial \theta_k} = \sum_{l=\max(i,j)}^n \Bigl( [A_{ki} \xi_i, \xi_k]^T A_{lk}^T {\cal M}_l' A_{lj} \xi_j + \xi_i^T A_{li}^T {\cal M}_l' A_{lk} [A_{kj} \xi_j, \xi_k] \Bigr)$

seems impossible to process because it requires adding a 2x1 to a 1x2 matrix.
going by ROWSxCOLUMNS notation. Matrices M and A are 6x6 and $\xi$ is a 6x1, so how does this addition statement fit the rules of matrix addition?  This must be my mistake, I just don't see how.

答: The Lie Bracket is 6x1, not 6x2, so both terms should be 1x1.  </p>

Title: Dynamic Model of a Manipulator


问:
Extended Kalman Filter using odometry motion model
In the prediction step of EKF localization, linearization must be performed and (as mentioned in Probabilistic Robotics [THRUN,BURGARD,FOX] page 206) the Jacobian matrix when using velocity motion model, defined as

$\begin{bmatrix} x \\ y \\ \theta \end{bmatrix}' = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} + \begin{bmatrix} \frac{\hat{v}_t}{\hat{\omega}_t}(-\text{sin}\theta + \text{sin}(\theta + \hat{\omega}_t{\Delta}t)) \\ \frac{\hat{v}_t}{\hat{\omega}_t}(\text{cos}\theta - \text{cos}(\theta + \hat{\omega}_t{\Delta}t)) \\ \hat{\omega}_t{\Delta}t \end{bmatrix}$

is calculated as

$G_{T}= \begin{bmatrix}
  1 & 0 & \frac{υ_{t}}{ω_{t}}(-cos {μ_{t-1,θ}} + cos(μ_{t-1,θ}+ω_{t}Δ{t})) \\
  0 & 1 & \frac{υ_{t}}{ω_{t}}(-sin {μ_{t-1,θ}} + sin(μ_{t-1,θ}+ω_{t}Δ{t})) \\
  0 & 0 & 1
 \end{bmatrix}$.

Does the same apply when using the odometry motion model (described in the same book, page 133), where robot motion is approximated by a rotation $\hat{\delta}_{rot1}$, a translation $\hat{\delta}$ and a second rotation $\hat{\delta}_{rot2}$ ? The corresponding equations are:

$\begin{bmatrix} x \\ y \\ \theta \end{bmatrix}' = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} + \begin{bmatrix} \hat{\delta}\text{cos}(\theta + \hat{\delta}_{rot1}) \\ \hat{\delta}\text{sin}(\theta + \hat{\delta}_{rot1}) \\ \hat{\delta}_{rot1} + \hat{\delta}_{rot2} \end{bmatrix}$.

In which case the Jacobian is

$G_{T}= \begin{bmatrix}
  1 & 0 & -\hat{\delta} sin(θ + \hat{\delta}_{rot1}) \\
  0 & 1 & -\hat{\delta} cos(θ + \hat{\delta}_{rot1}) \\
  0 & 0 & 1
 \end{bmatrix}$.

Is it a good practise to use odometry motion model instead of velocity for mobile robot localization?

答:
You have asked two questions. As I interpret them they are:

Is it necessary to linearize the odometry motion model for use with an extended Kalman filter (EKF)?

Is it better to use the odometry motion model instead of the velocity motion model.

Regarding question 1, the short answer is "yes." The guarantees of the Kalman filter (KF) only apply to linear systems. We linearize a non-linear system in hopes of retaining some of those guarantees for non-linear systems. In fact linearizing the non-linear components of a system (i.e. the motion model and/or the observation model) is the very thing that differentiates KFs and EFKs.

Regarding question 2, Dr. Thrun argues on page 132 of Probabilistic Robotics that "[p]ractical experience suggests that odometry, while still erroneous, is usually more accurate than velocity." However I would not interpret this statement as an argument for supplanting the velocity model. If you have both velocity and odometric information then it is generally better to use both sources of information.


问:
Implementing an analytic version of an inverse kinematic
People have recommended me implement an analytic version of inverse Jacobian solver, such that I won't be forced only the least square solution, but would have an local area of solution near to the one I desire.

I can't seem to implement it correctly, I mean how much does it differ from the least square inverse kinematics which I have implemented here?

```<code>Eigen::MatrixXd jq(device_.get()->baseJend(state).e().cols(),device_.get()->baseJend(state).e().rows());
      jq = device_.get()->baseJend(state).e(); //Extract J(q) directly from robot

      //Least square solver - [AtA]⁻1AtB

      Eigen::MatrixXd A (6,6);
      A = jq.transpose()*(jq*jq.transpose()).inverse();

      Eigen::VectorXd du(6);
      du(0) = 0.1 - t_tool_base.P().e()[0];
      du(1) = 0 - t_tool_base.P().e()[1];
      du(2) = 0 - t_tool_base.P().e()[2];
      du(3) = 0;  // Should these be set to something if i don't want the tool position to rotate?
      du(4) = 0;
      du(5) = 0;

      ROS_ERROR("What you want!");
      Eigen::VectorXd q(6);
      q = A*du;

      cout << q << endl; // Least square solution - want a vector of solutions.
</code>```

I want a vector of solution - how do I get that?

the Q is related to this https://robotics.stackexchange.com/questions/9672/how-do-i-construct-i-a-transformation-matrix-given-only-x-y-z-of-tool-position

The robot being used is a UR5  - https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_kin_tech_report_1.pdf

答:
Your Jacobian-based approach is great for velocity control, or when the manipulator is close to the original point.  But remember, the Jacobian only gives a first-order approximation to the manipulator's motion.  With the highly nonlinear kinematics of manipulators, accuracy will vary throughout the workspace, and will decrease as the step size between the current pose and desired pose gets larger.    </p>

<p>Closed-form, or analytic, inverse kinematics do not require the Jacobian for computing the joint angles which will achieve a desired pose.   They are based on the trigonometry of the manipulator.   To compute analytic inverse kinematics, you need to take the forward kinematics and solve for each $q$.   This can be challenging for some manipulator types.  It is easier if the wrist is spherical (all three wrist axes intersect at a point).  The good news is that many robotics books contain inverse kinematic solutions for each of the manipulator types.  </p>

Body:
Your Jacobian-based approach is great for velocity control, or when the manipulator is close to the original point.  But remember, the Jacobian only gives a first-order approximation to the manipulator's motion.  With the highly nonlinear kinematics of manipulators, accuracy will vary throughout the workspace, and will decrease as the step size between the current pose and desired pose gets larger.

Closed-form, or analytic, inverse kinematics do not require the Jacobian for computing the joint angles which will achieve a desired pose.   They are based on the trigonometry of the manipulator.   To compute analytic inverse kinematics, you need to take the forward kinematics and solve for each $q$.   This can be challenging for some manipulator types.  It is easier if the wrist is spherical (all three wrist axes intersect at a point).  The good news is that many robotics books contain inverse kinematic solutions for each of the manipulator types.


问:
how to find maximum force of a robot joint
I want to know if there is any equation that calculates the maximum force of a robot joint. The force that we should not exceed.

For example in human leg, if we apply a big external force to the knee, it will break. now how can i find the necessary force that will just make the leg move without breaking the knee.

I have a programme that generates robot morphologies randomly with different sizes, so I have to know the force to not exceed for each joint. I think this depend on weight, mass, inertia of each robot part.

I can not do this by trial and error because I have hundreds different morphologies.

This video shows the behaviour of robot if I apply a big force. It is in Gazebo robotic simulator.

Thanks in advance!

答:
I think you probably need to learn more about simulation before you proceed. Otherwise you will not be able to choose a good one, nor set the parameters of operation which will give valid results.

Regarding the automated design of robot joints;

The allowed joint forces depend on the components and approach used to create the joint.

The recursive Newton Euler approach will give you the bulk joint forces. Those can then be resolved into specific bearing and structural loads specific to your mechanical design.

You will need to create at least one valid mechanical joint design which will have a maximum load that can be measured or, to some extent, predicted. It's best to create a couple generic design approaches with tables of components if you want to do genetic optimization.

Make sure you do a little background reading and understand what has been done before. For example: http://darwin2k.sourceforge.net/


答:
It sounds like all you really care about it is a realistic simulation. For you, this means that you are interested in specifying a force that is "reasonable" for your robot. I say "reasonable" because it's up to you to define, but hopefully I can help you set some guidelines.

Torque, the angular force you apply to a joint, has two general uses: static poses and dynamic motion. A lot of people try to calculate the torque they need by evaluating only the static torque, which is a poor method to use because, once the worst-case position is reached, there is no longer any overhead to accelerate (change the speed of) the joint. That is, once the joint stops at the worst-case position, if you only specified the worst-case static torque, it's not possible to resume motion.

Static torque is easily calculated: $\tau_s = FL\sin{\theta}$. Torque is the force applied, times the distance at which it's applied, times the sine of the angle the arm makes with vertical.

Dynamic torque, which I'd say is arguably the more important of the two, is a little harder to calculate because it requires the moment of inertia of the load.

Dynamic torque is $\tau_d = I\alpha$. Moment of inertia times angular acceleration. It's pretty straightforward, once you have the moment of inertia, but again that can be tricky to calculate.

The easiest way, in my opinion, is to calculate it through the center of mass of the object, then use the parallel axis theorem to shift the axis to that of the joint.

Now, this becomes tricky when you are dealing with multibody simulation because the method I give above only gives the moment of inertia for one link. If you have multiple linkages, then you have to take the joint angle into account when calculating the moments of inertia for all subsequent linkages.

Ultimately, for your problem, you need to choose an acceptable angular acceleration $\alpha$, programmatically calculate the moments of inertia, and then use $\tau = I\alpha$ to determine the maximum forces you want to apply.

Title: how to find maximum force of a robot joint

Body:
I want to know if there is any equation that calculates the maximum force of a robot joint. The force that we should not exceed.

For example in human leg, if we apply a big external force to the knee, it will break. now how can i find the necessary force that will just make the leg move without breaking the knee.

I have a programme that generates robot morphologies randomly with different sizes, so I have to know the force to not exceed for each joint. I think this depend on weight, mass, inertia of each robot part.

I can not do this by trial and error because I have hundreds different morphologies.

This video shows the behaviour of robot if I apply a big force. It is in Gazebo robotic simulator.

Thanks in advance!


答: In my experience, the answer to your last question is "yes." I've had much more luck using odometry instead of dynamic (velocity) prediction. However, I've never used the motion model you describe (from Thrun's book). Instead, I've used the model I described here.</p>

Title: Extended Kalman Filter using odometry motion model

Body:
In the prediction step of EKF localization, linearization must be performed and (as mentioned in Probabilistic Robotics [THRUN,BURGARD,FOX] page 206) the Jacobian matrix when using velocity motion model, defined as

$\begin{bmatrix} x \\ y \\ \theta \end{bmatrix}' = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} + \begin{bmatrix} \frac{\hat{v}_t}{\hat{\omega}_t}(-\text{sin}\theta + \text{sin}(\theta + \hat{\omega}_t{\Delta}t)) \\ \frac{\hat{v}_t}{\hat{\omega}_t}(\text{cos}\theta - \text{cos}(\theta + \hat{\omega}_t{\Delta}t)) \\ \hat{\omega}_t{\Delta}t \end{bmatrix}$

is calculated as

$G_{T}= \begin{bmatrix}
  1 & 0 & \frac{υ_{t}}{ω_{t}}(-cos {μ_{t-1,θ}} + cos(μ_{t-1,θ}+ω_{t}Δ{t})) \\
  0 & 1 & \frac{υ_{t}}{ω_{t}}(-sin {μ_{t-1,θ}} + sin(μ_{t-1,θ}+ω_{t}Δ{t})) \\
  0 & 0 & 1
 \end{bmatrix}$.

Does the same apply when using the odometry motion model (described in the same book, page 133), where robot motion is approximated by a rotation $\hat{\delta}_{rot1}$, a translation $\hat{\delta}$ and a second rotation $\hat{\delta}_{rot2}$ ? The corresponding equations are:

$\begin{bmatrix} x \\ y \\ \theta \end{bmatrix}' = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} + \begin{bmatrix} \hat{\delta}\text{cos}(\theta + \hat{\delta}_{rot1}) \\ \hat{\delta}\text{sin}(\theta + \hat{\delta}_{rot1}) \\ \hat{\delta}_{rot1} + \hat{\delta}_{rot2} \end{bmatrix}$.

In which case the Jacobian is

$G_{T}= \begin{bmatrix}
  1 & 0 & -\hat{\delta} sin(θ + \hat{\delta}_{rot1}) \\
  0 & 1 & -\hat{\delta} cos(θ + \hat{\delta}_{rot1}) \\
  0 & 0 & 1
 \end{bmatrix}$.

Is it a good practise to use odometry motion model instead of velocity for mobile robot localization?


问:
What type of actuator should I use?
I need to find out if There is a way to get at least 60 Hz of linear Motion with at least 5 mm of stroke that I intend to make linear persistence of vision device(not rotating one)It must be small and light as possible. ( maybe 50 mm long and 10-15 mm diameter or around these) (less than 500 grams) The Load will be around 50 grams.  There are voice coils that is very expensive, can I use solenoids for instance or what do you recommend? </p>

<p>Thanks </p>

答:
A few guidelines to help you search:

Max displacement: +/- 5mm.
Frequency: 60Hz = 377 rad/s.

Now, some math:

$$
x = 0.005 \sin{(377 t)} \\
v = (377)(0.005) \cos{(377 t)} \\
a = -(377^2)(0.005) \sin{377 t)} \\
$$

Then, the max values are wherever the particular trig function is equal to 1, so:

$$
x_{\mbox{max}} = 0.005 \mbox{m}\\
v_{\mbox{max}} = 1.9 \mbox{m/s}\\
a_{\mbox{max}} = 710 \mbox{m/s}^2 \approx 72 \mbox{g}\\
$$

Now, $F = ma$, so the peak force you are applying is:

$$
F = ma \\
F = (0.3)(710) \\
F = 213 \mbox{N} \\
$$

Peak power is force times speed.
$$
P = (213)(1.9) \\
P = 405 W \\
$$

Per your guidelines, you want (I'm assuming) a cylinder that's 0.05m long and 0.01m in diameter. This means that the volume is (approximately)

$$
V = Ah \\
V = \pi r^2 h \\
V = \pi (0.005)^2 (0.05) \\
V = 0.00000393 \mbox{m}^3 \\
V = 0.00393 \mbox{liters} \\
V = 3927 \mbox{mm}^3 \\
$$

Now, you can try looking at power density and see that you want 405W of peak power in a space of 0.00393 liters, or a power density of about $405/0.00393 = \boxed{103 \mbox{W/l}}$, or about the same power density as a Ferrari engine.

I'm not saying it's not possible, but good luck.

If you are a PhD student at a university, you should consider asking your committee members to guide you to another grad student for assistance with this.

Typically I would vote to close an open-ended design/shopping question like this, but I figured I'd go through the steps to show that what you want isn't really attainable.

Title: What type of actuator should I use?


问:
How to rotate covariance?
I am working on an EKF and have a question regarding coordinate frame conversion for covariance matrices. Let's say I get some measurement $(x, y, z, roll, pitch, yaw)$ with corresponding 6x6 covariance matrix $C$. This measurement and $C$ are given in some coordinate frame $G_1$. I need to transform the measurement to another coordinate frame, $G_2$. Transforming the measurement itself is trivial, but I would also need to transform its covariance, correct? The translation between $G_1$ and $G_2$ should be irrelevant, but I would still need to rotate it. If I am correct, how would I do this? For the covariances between $x$, $y$, and $z$, my first thought was to simply apply a 3D rotation matrix, but that only works for a 3x3 submatrix within the full 6x6 covariance matrix. Do I need to apply the same rotation to all four blocks? </p>

答:
The MRPT library can do this for you. You need to use a CPose3DPDFGaussian to represent your pose and covariance then use the + operator.

Under the hood it represents your 6DOF covariance as a 7DOF quaternion base covariance, where the math is more straightforward.

Title: How to rotate covariance?


答:
To your first question: "Does the same apply when using the odometry motion model?", the answer is Yes.

The the EKF is pretty much the same thing as the KF, with the addition of the linearization step.  What you are linearizing here is the motion model, whatever model that is.

For your second question: "Is it a good practise to use odometry motion model instead of velocity for mobile robot localization?": I think the answer is 'it depends.'

If you are using a data set that has velocity information and the localization is good enough for your purposes, then the simplicity of that model is probably preferred.  If you are directly controlling the robot and have access to the odometry information, then you're likely to get a better result.


问:
Suitable uC for atonomous robot
I am going to build an autonomous robot with Kalman-filter for localization integrated from Lidar, encoder, IMU and GPS. I will also add obstacle avoidance while moving to the required position.

Is the ATmega32 8-bit suitable for that (or Arduino Mega) or do I have to use avr32, ARM, or PIC32 and which is better?

答:
One could break down your problem to three areas: memory, speed and I/O.

Memory is pretty straightforward, but you have to estimate it yourself. Things to consider: Are you doing a map? Are you saving past state? If yes how long/much? etc.

Speed is a bit different: Assume you are building a quadcopter: If you have high latency in the controller's feedback loops the thing will not even fly. If you have a ground vehicle on the other hand you could simply drive slower.

Consider how you want to structure the program's control flow: Do you poll for new data all the time or do it interrupt-driven?
How fast should your control loop run and what do you need to calculate during it? How many floating point operations do you need? How large is your map for obstacle avoidance?

Lastly what peripherals, sensors, debugging tools etc. do you want to connect and does the uC have enough connections to allow that?

If this is going to be a rather serious project, I would shy away from arduino, because if you need to have more control over the program and write low-level code things are likely to get messy. The learning curve to develop with Atmel Controllers directly is not that high.

With the meager information you provided I feel like the ATmega32 will be suitable, but if you need a fast control loop consider ARM with a floating point unit.

Title: Suitable uC for atonomous robot


问:
Is there a "follow me" Roomba/Create that works like a BEAMBot?
The diagram below shows an old BEAMBot strategy:

Is there code or an example using this method? I would rather avoid OpenCV, ultrasonic, GPS etc. I just want the Roomba wheels to react as I go straight, turn left or right. Finally, I could add a front wheel on a servo and try having the Roomba turn with me.

Also has anybody added big, all terrain wheels to a Roomba to replace the originals?

答:
The Roomba already does something very close to this: when it returns to its charging dock, it aligns itself with an IR signal coming from the top of the dock.  The procedure is more sophisticated, but the basics are the same.

According to this PDF of the iRobot Roomba 500 Open Interface, there is a way to read the "infrared character" from the left, right, and omni sensors.  So you would need a device that could send an IR character (in Roomba's format) to the vehicle, and your code would detect and react to it.


问:
Performing inverse kinematics based on a displacement of the end effector?
I think i have an simple problem, but can't my head around how i should resolve it...

My setup looks like this:

The grey box on end effector is supposed to be an camera, which measures a dx,dy,dz between the object and the camera. These are used to  position the camera such that dz between the object and the camera is equal to 0.5, and dx = dy = 0.

I know that I using inverse kinematics can determine the Q which positions it according the given rotation and position, but what if I only provide it a position only?

How do extract all Q that make dx = dy = 0, and dz = 0.5, while keeping the object in sight at all time?

An example could be if an object was placed just above the base (see second image), it should then find all possible configurations which in this case would consist of the arm rotating around the object, while the camera keeps the object in sight...

Update

I just realized a possible solution would be to create a sphere with the object in centrum  an radius of dz, and then use this sphere to extract all pairs of rotations and position... But how would one come by with such an solution?

答:
You always have both position ad orientation. In your case the orientation can be chosen by you to be constant (e.g. 0, 0, 0), in which case you will be not be able to track the object if it is for example above the camera.

Th camera measures the relative position of the object (relative to the camera). You subtract the distance on the z axis that you want to keep and you add the difference to the current poseition of the robot. Morever you can calculate the angle how much the endefector has to rotate to face the object. Eg. If the camera measures:
Dx = 0,5, dy=0 and dz=0,5 then you would add the dx to the current tcp position (since dz=0,5 which is the distance you want to keep). After that you can calulate the change in orientation needed to face the object by

$RotY = atan2(dx,dz) = atan2(0.5,0.5) = 45deg$

You add this (also calculate RotX and RotZ if not defined as "tool axis") to the current orientation of the end-effector and give it as reference (together withe the changed position) value for the next motion.


问:
Simulated kinect rotation around X [gazebo bug?]
I asked this question on answers.ros.org and gazebo.ros.org but still haven't got any answer. I'm posting my question here with the hope someone can help me.

In our robot, the Kinect can be mounted on the side of the arm, as shown in the screenshot below. When running the simulation in Fuerte, I found this weird behaviour. As you can observe on the image, the point cloud does not match the robot model (we see a partial image of the hand/arm at the bottom left of the screenshot, which should be on the robot model).

As soon as I rotate the kinect against its X axis (so that the kinect is horizontal as you can see on the second screenshot), then the point cloud and robot model are aligned properly.

The kinect xacro and dae are the one from the turtlebot. I'm simply attaching them with a rotation:

```<code><joint name="base_camera_joint" type="fixed">
  <origin  xyz="0.01216 0.1713 0.433"
       rpy="-<span class="math-container">${M_PI/2} $</span>{M_PI/4} -${M_PI/12}" />
  <!-- This -pi/2 in origin rpy is the offending parameter -->

  <parent link="shadowarm_trunk"/>
  <child link="camera_link" />
</joint>
</code>```

The code can be seen on github.

Any help is greatly appreciated!

答:
An answer from David Butterworth which solved my problem! Thanks David.

I found that if you modify the visual/collision mesh to be aligned with your joint origin, then it's okay.
Don't do extra translations/rotations of the mesh from within your URDF, because that is broken.
The orientation of the sensor data should depend on the Gazebo macro, but there should be only translations in there, with 2 joint rotations for the extra pair of camera frames.

My Gazebo macro is based on the PR2 one, but with the visual/collision meshes fixed and re-scaled as per above.
The end result is that any translation/rotation is only done at the head_mount_kinect joint that orientates everything else, and you can successfully pitch the sensor up-down or vertically.

I'm guessing that in your situation, the PointCloud data is actually in the correct place, but because of the bug with the meshes, the visual model is in the wrong place.

ContentLicense: CC BY-SA 4.0


问:
Kuka KR16L-2 robot simulation base and wrist rotation inconsistent with original robot
The issue is regarding simulation of Kuka robot KR16L6-2 in MATLAB using Robotics toolkit by Peter Corke. I wish to simulate  the kinematic before passing a command to real robot for motion.

I have attached the DH-Parameters. Apart from this I have also tried many other combination of orientations but to no useful effect.

The problems is that the robot base rotates counter-clockwise by default for positive increases in Joint1, while the original robot moves in the opposite direction. Similarly for the wrist roll, i.e. Joint 4, the direction of simulation is reversed.

In order to confirm that it's not my mistake only, I searched for similar ready made simulation software. Although it did not include the same KUKA robot, a similar variant (KUKA_KR_5_sixx_R650) was available. Hence, KUKA_KR_5_sixx_R650 had one set of motions for base and wrist in RoKiSim v1.7 for positive increases in joint angle and reverse motion in roboanalyzerv7  .

NOTE: Only the rotation of J1 (base) and J4 (Wrist Roll) are reversed
and I want to recreate the results of RoKiSim v1.7 in Matlab where rotations are similar to the real world robot spec provided by KUKA.

答:
Kuka robots have the axis positive and negative side clearly marked on them. Use these to define the orientations of the Z-axes assigned to the joints. With other words, try to reverse engineer the DH parameters used by Kuka. Only this way can you corelate your DH table with the kuka axis orientations. If the axis moves in the wrong direction it means that your coordonate system is upside down. Flip it and change the sign of the other parameters.

Also keep in mind that there are multiple variants of teh DH parametrization (proximal and distal or modified DH). It can be that kuka uses a different one then the Robotics toolbox.

Other simulation software is not necesarily corelated with the real robot.


问:
Choosing the right Mecanum wheel
I am part of my college robotics team which is preparing for Robocon 2017.

We have used Mecanum wheels in last Robocon competition, but we have faced huge slip and vibration. I have looked for all kinematic and dynamic formulas and all stuff about Mecanum wheels, but still can't get to a conclusion for my problem.

Video of the problem

The robot is around 25 kg and the Mecanum wheel diameter is about 16 cm with 15 rollers (single type). Please help me why it happened like that!?

Also suggest me what to do now - Should I design a new Mecanum wheel or bring it from market?

If I should design, what parameters should I consider, and please help me how to design in CAD software like SolidWorks? And then, shall I give it to 3D printing?

If I should buy directly from market, where should I buy?

答:
The problem appears to be that your drive motors are providing too much torque.

In other words, there isn't enough friction between the wheels and the floor to counteract the force that the motors are exerting on the wheels.  To determine whether this is actually the problem, try adding weight to the vehicle (directly above the wheels, if possible); this should balance the forces by increasing the static friction.

Of course, adding weight to your vehicle is not a good long term solution.  The proper fix would be to configure your motor controllers to limit the torque produced by the motors. This might also be expressed as a current limit, depending on your setup.

Title: Choosing the right Mecanum wheel


答:
I agree with holmeski's suggestion that an accelerometer is the way to go for static projects, but if you need more sophisticated measurements, an accelerometer/gyroscope combo will be necessary.

The MPU-6050 is a chip that contains a 3-axis MEMs accelerometer and gyroscope with a very small footprint. I have personally used the GY-521 breakout board, and I was impressed with the performance given the low price (~$5). Furthermore, there is documentation for the MPU-6050 on the Arduino website, which migh help for your project (http://playground.arduino.cc/Main/MPU-6050).

One caveat to the MPU-6050 is that you will have to put some thought into filtering the measurements. Accelerometers suffer from low frequency noise, while gyroscopes suffer from high frequency noise. When I used the MPU-6050, I used a complimentary filter to combine the measurements from the accelerator and gyroscope into a single measurement for angle, which worked well. In retrospect, it would have been better to implement a Kalman Filter instead. Both will not only enable you to complete your project with the MPU-6050, but you will get to implement software filters, which is a good experience.

Title: Best sensor to determine "up" versus "down"

Body:
I want to start designing an Arduino project and have telemetry readings that indicate tilt or angle of placement. </p>

<p>Would an accelerometer be the best for determining tilt? Are there good tutorials? </p>


问:
Generate transformation matrices for rotating around a object?
How do i compute all transformation matrices which places a robot endeffector at the shell of this sphere, with the end effector pointing toward the object in the center.

I know at all time how far the object is relative to the endeffector, and radius of the sphere is the desired distance i want between the object and endeffector.

I want by using inverse kinematics pan around this object in a sphere shaped trajectory.

Each transformation matrix should contain different positions on the sphere and the rotation should be oriented such that the arm looks at the object.

The position should be relative easy to compute, as i already know the distance to to object, and radius of the sphere.

But the rotation matrix for each position is still a mystery for me.

答:
make sure tha your axes are parallel with the coordinate system in which your robots orientations are expressed.

Caclulate the orientation angles geometrically by taking projections of the vector marked with red.

I do not see the need for any rotation matrices.

EDIT:

Here is a 2D example. The camera moves on the circle and the Z orinetation has to be adjusted. The dx and dy can be calculated form the circle equations and the Z rotation is

$Z_{rot}= atan2(dy,dx)$


答:
Remember that the columns of a rotation matrix are simply unit vectors indicating where each axis points.  Lets say your end-effector is at $p_e = (x_e, y_e, z_e)$, and the center of the circle is at $p_o = (x_o, y_o, z_o)$.  You didn't fully answer my comment about your end-effector axes, so I will assume you want $z_e$ pointed at the object. Here are the steps you should take.

First, some notation.  The transform from the world frame to your desired end-effector is:
$$
T^{world}_{ee} = \begin{bmatrix} R_e & p_e \\ 0 & 1 \end{bmatrix}
$$

Where $R_e$ is the rotation matrix in question.  It is composed of 3 column vectors:

$$
R_e = \begin{bmatrix} R_{x_e} & R_{y_e} & R_{z_e} \end{bmatrix}
$$

The third (Z) column of the rotation matrix is simply $R_{z_e} = \widehat{p_o - p_e}$.

Pick a natural orientation for your camera.  Perhaps $y_e$ down.  So think about a plane created by the world down axis and $R_{z_e}$.  Now $x_e$ must be normal to that plane.  Assuming the world down axis is $down = (0,0,-1)$.  Now $R_{x_e} = \widehat{down \times R_{z_e}}$.  This is the first (X) column of your rotation matrix.  (Note: be careful to avoid degenerate cases, like when the camera is directly above the object ($z_e = z_o$).  You need to test for this case and handle this differently.)

Lastly, the Y axis of your new frame must be the cross product of the other two, so: $R_{y_e} = R_{z_e} \times R_{x_e}$.  This is the second (Y) column of your new rotation matrix.

This gives you a single orientation for each position on your sphere in which the object will be upright in the camera image.

Your question was a little vague on whether this is sufficient, or if you want many orientations for each point on the sphere.  Since the camera can rotate about its focal axis, the arm and camera will still be pointing at the object.  However, the object will be rotated in the image.

There are an infinite number of orientations that satisfy the constrains for each point, but you can get a discrete set by picking some angles from 0 to 360 degrees then simply rotating your end-effector frame about its Z axis by this amount.

$$
T^{world}_{newee} =  T^{world}_{ee} * \begin{bmatrix}
\cos \theta &  -\sin \theta & 0 & 0\\
\sin \theta & \cos \theta & 0 & 0\\
0 & 0 & 1 & 0\\
0 & 0 & 0 & 1\\
\end{bmatrix}
$$

So now $T^{world}_{newee}$ will have the object upside down if you chose $\theta = 180^{\circ}$ for example.

Here is some simple Python code which implements this solution (without the arbitrary rotation), and displays the frames with OpenRave.

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

import numpy as np
from openravepy import *

# set up OpenRave environment
env = Environment()
env.SetViewer('qtcoin')

# center of sphere
p_o = np.array([0.0, 0.0, 0.0])

# radius of sphere
r = 1.0

# down vector
down = np.array([0.0, 0.0, -1.0])

# large coordinate frame at the center of the sphere
originaxes = misc.DrawAxes(env, [1, 0, 0, 0, p_o[0], p_o[1], p_o[2]], dist=1, linewidth=2)

# all other frames
axes = []

for i in range(200):

    # get a random point on the sphere
    p_e = np.random.uniform(-1, 1, 3)
    p_e *= r / np.linalg.norm(p_e)
    p_e += p_o

    # Z column
    R_z = (p_o - p_e)
    R_z /= np.linalg.norm(R_z)

    # X column
    R_x = np.cross(down, R_z)
    R_x /= np.linalg.norm(R_x)

    # Y column
    R_y = np.cross(R_z, R_x)

    # Full transformation matrix
    T = np.array([[ R_x[0], R_y[0], R_z[0], p_e[0] ],
                  [ R_x[1], R_y[1], R_z[1], p_e[1] ],
                  [ R_x[2], R_y[2], R_z[2], p_e[2] ],
                  [      0,      0,      0,      1]])

    # draw it
    axes.append(misc.DrawAxes(env, T, dist=0.2, linewidth=1))
</code>```

This should give you frames all around the sphere like this:

Title: Generate transformation matrices for rotating around a object?

Body:
How do i compute all transformation matrices which places a robot endeffector at the shell of this sphere, with the end effector pointing toward the object in the center.

I know at all time how far the object is relative to the endeffector, and radius of the sphere is the desired distance i want between the object and endeffector.

I want by using inverse kinematics pan around this object in a sphere shaped trajectory.

Each transformation matrix should contain different positions on the sphere and the rotation should be oriented such that the arm looks at the object.

The position should be relative easy to compute, as i already know the distance to to object, and radius of the sphere.

But the rotation matrix for each position is still a mystery for me.

Body:
Remember that the columns of a rotation matrix are simply unit vectors indicating where each axis points.  Lets say your end-effector is at $p_e = (x_e, y_e, z_e)$, and the center of the circle is at $p_o = (x_o, y_o, z_o)$.  You didn't fully answer my comment about your end-effector axes, so I will assume you want $z_e$ pointed at the object. Here are the steps you should take.

First, some notation.  The transform from the world frame to your desired end-effector is:
$$
T^{world}_{ee} = \begin{bmatrix} R_e & p_e \\ 0 & 1 \end{bmatrix}
$$

Where $R_e$ is the rotation matrix in question.  It is composed of 3 column vectors:

$$
R_e = \begin{bmatrix} R_{x_e} & R_{y_e} & R_{z_e} \end{bmatrix}
$$

The third (Z) column of the rotation matrix is simply $R_{z_e} = \widehat{p_o - p_e}$.

Pick a natural orientation for your camera.  Perhaps $y_e$ down.  So think about a plane created by the world down axis and $R_{z_e}$.  Now $x_e$ must be normal to that plane.  Assuming the world down axis is $down = (0,0,-1)$.  Now $R_{x_e} = \widehat{down \times R_{z_e}}$.  This is the first (X) column of your rotation matrix.  (Note: be careful to avoid degenerate cases, like when the camera is directly above the object ($z_e = z_o$).  You need to test for this case and handle this differently.)

Lastly, the Y axis of your new frame must be the cross product of the other two, so: $R_{y_e} = R_{z_e} \times R_{x_e}$.  This is the second (Y) column of your new rotation matrix.

This gives you a single orientation for each position on your sphere in which the object will be upright in the camera image.

Your question was a little vague on whether this is sufficient, or if you want many orientations for each point on the sphere.  Since the camera can rotate about its focal axis, the arm and camera will still be pointing at the object.  However, the object will be rotated in the image.

There are an infinite number of orientations that satisfy the constrains for each point, but you can get a discrete set by picking some angles from 0 to 360 degrees then simply rotating your end-effector frame about its Z axis by this amount.

$$
T^{world}_{newee} =  T^{world}_{ee} * \begin{bmatrix}
\cos \theta &  -\sin \theta & 0 & 0\\
\sin \theta & \cos \theta & 0 & 0\\
0 & 0 & 1 & 0\\
0 & 0 & 0 & 1\\
\end{bmatrix}
$$

So now $T^{world}_{newee}$ will have the object upside down if you chose $\theta = 180^{\circ}$ for example.

Here is some simple Python code which implements this solution (without the arbitrary rotation), and displays the frames with OpenRave.

#! /usr/bin/env python

import numpy as np
from openravepy import *

# set up OpenRave environment
env = Environment()
env.SetViewer('qtcoin')

# center of sphere
p_o = np.array([0.0, 0.0, 0.0])

# radius of sphere
r = 1.0

# down vector
down = np.array([0.0, 0.0, -1.0])

# large coordinate frame at the center of the sphere
originaxes = misc.DrawAxes(env, [1, 0, 0, 0, p_o[0], p_o[1], p_o[2]], dist=1, linewidth=2)

# all other frames
axes = []

for i in range(200):

    # get a random point on the sphere
    p_e = np.random.uniform(-1, 1, 3)
    p_e *= r / np.linalg.norm(p_e)
    p_e += p_o

    # Z column
    R_z = (p_o - p_e)
    R_z /= np.linalg.norm(R_z)

    # X column
    R_x = np.cross(down, R_z)
    R_x /= np.linalg.norm(R_x)

    # Y column
    R_y = np.cross(R_z, R_x)

    # Full transformation matrix
    T = np.array([[ R_x[0], R_y[0], R_z[0], p_e[0] ],
                  [ R_x[1], R_y[1], R_z[1], p_e[1] ],
                  [ R_x[2], R_y[2], R_z[2], p_e[2] ],
                  [      0,      0,      0,      1]])

    # draw it
    axes.append(misc.DrawAxes(env, T, dist=0.2, linewidth=1))

This should give you frames all around the sphere like this:


答:
Currently your problem still has one degree of freedom, namely the orientation of the robot can still roll around the vector. For now I will assume that you would want the top of the robot to keep facing upwards as best as possible.

A rotation matrix can be seen as an second order tensor, which can be constructed by summing multiple dyads of the new and old base vector,

$$
R = \vec{e}_{x1} \vec{e}_{x0} + \vec{e}_{y1} \vec{e}_{y0} + \vec{e}_{z1} \vec{e}_{z0}.
$$

Because each basis should be orthonormal, then taking the dot product of $R$ with either $\vec{e}_{x0}$, $\vec{e}_{y0}$ or $\vec{e}_{z0}$ will result in the base vectors $\vec{e}_{x1}$, $\vec{e}_{y1}$ or $\vec{e}_{z1}$ respectively. Each dyad can be expressed as a matrix by taking the vector direct product.

Now all you have to do is find an expression for the initial and final orthonormal basis. Since you want the robot to face the center of the sphere, you can define $\vec{e}_{x0}$ by the direction the front of the robot is facing, when no rotation is applied, and $\vec{e}_{x1}$ as the unit vector pointing towards the center of the sphere. By using the addition constraint for the roll you can define $\vec{e}_{y0}$ as the direction the top of the robot is facing, when no rotation is applied, and $\vec{e}_{y1}$ by taking the orthogonal component of $\vec{e}_{y0}$ with respect to $\vec{e}_{x1}$,

$$
\vec{e}_{y1} = \frac{\vec{e}_{y0} - \left(\vec{e}_{y0} \cdot \vec{e}_{x1}\right) \vec{e}_{x1}}{\left\|\vec{e}_{y0} - \left(\vec{e}_{y0} \cdot \vec{e}_{x1}\right) \vec{e}_{x1}\right\|}.
$$

The remaining vectors van be found using the fact that each basis should be orthonormal, which can be achieved by simply taking the cross product of the other base vectors. So $\vec{e}_{z0}=\vec{e}_{x0}\times\vec{e}_{y0}$ and $\vec{e}_{z1}=\vec{e}_{x1}\times\vec{e}_{y1}$, you could reverse the order of the vectors in the cross product, but you should do for both the $\vec{e}_{z0}$ and $\vec{e}_{z1}$ because otherwise the resulting "rotation" will also contain a reflection (the robot will be rotated but also a mirror image of itself).


问:
Is it possible to both move and stabilize a two wheeled robot with no gyroscopes?
With two wheeled robot like this one, I have managed to stabilize it while keeping it stationary. This was done using a digital feedback control system by reading the position of the wheels to determine position, and the natural back electromotive force from the wheel motors was used in the feedback loop to determine velocity. It was kept stable with a PID controller, which was designed using a root locus algorithm to keep it stable and modulate the performance parameters (such as percent overshoot, settling time, etc.). I wanted to attempt to keep it stable while simultaneously propelling it forward, but I couldn't figure out how to go about designing a linear controller that could do that. Is it possible to both propel the robot forward and keep it stable using a feedback controller on the wheels, or is a gyroscope necessary?

答: Mathematically, the fact that you now have rotation (mostly) eliminates that parameter as a possible control parameter.  Basically you'd have to redesign your algorithm to accept a large and variable angular velocity component while still using angular velocity in your feedback.  The less noisy this is, the better the probable outcome simply because you're likely going to apply a differential of position to derive another control parameter.  Or rather that is one way.  So it does look like you need another control input, but it need not be an accelerometer.  You could do a horizon, fixed marker location or even tilt sensors.</p>


问:
Getting “rospack package not found error” in ROS
I created a package in catkin workspace and put a publisher.py node inside the src directory of package which worked fine. Then i added another node subscriber.py node and used catkin_make to build. Now when I try to run any of the nodes or find package i am getting above error. Am I missing any step ?

Thanks.

答:
It's hard to tell what's going wrong from just from you describing the error (might be better to paste the message directly..Sounds like a problem with your ROS_PACKAGE_PATH environment variable. I'm assuming you're following this tutorial)? I remember having had this problem before, so some possible things to try:

You may need to just type in source devel/setup.bash into your terminal.

Make sure your bash.rc file is properly set. Refer to this question. (If I remember correctly, if you don't want to type in source devel/setup.bash every time you open up a new terminal you can edit the bash.rc file.)

Check that your CMAKE_PREFIX_PATH is right.

Also, try entering rospack find [package_name] into the terminal to find the path to the package.


答:
Given a desired pose:

$$T^{desired} = \begin{bmatrix}
             s_x&n_x&a_x&P_x\\
             s_y&n_y&a_y&P_y\\
             s_z&n_z&a_z&P_z\\
               0&  0&  0&  1
            \end{bmatrix}$$

we know that this is given as follows:

$$T^{desired} = {}^{0}T_{1}{}^{1}T_{2}{}^{2}T_{3}{}^{3}T_{4}{}^{4}T_{5}{}^{5}T_{6}$$

where each ${}^{j-1}T_{j}$ is the transformation between frame attached to joint j with respect to joint j-1.

Now the idea is to start pre-multiplying the equation with the inverse transformation and try to deduce the joint angles as we go along, as an example let's start by the first joint (i.e. $q_1$):

$${}^{1}T_{0}T^{desired} = {}^{1}T_{2}{}^{2}T_{3}{}^{3}T_{4}{}^{4}T_{5}{}^{5}T_{6}$$

When you do this you find the following:

$$-\cos(q_5) = -a_x \sin(q_1) + a_y \cos(q_1)$$

and

$$-d_4 - d_6\cos(q_5) = -p_x\sin(q_1)+p_y\cos(q1)$$

so by manipulating those two equations you end up with a quadratic equation in $\sin(q_1)$ or $\cos(q_5)$ which gives two solutions, only one  of them satisfy the second equation above which gives the other parameter (i.e. the $\sin(q_1) or \cos(q_1)$) then you get $q_1$ as

$$q_1 = atan2(\sin(q_1),\cos(q_1))$$

then you can get $q_5$ and then proceed in the same manner to get the rest.
If you want I can share with you some document that explain the procedure in more details, and also some matlab m files that help you to symbolically advance through the procedure.

Small remark: I am not using exactly the  Denavit-Hartenberg parameters but a slightly different ones (i.e. Khalil-Kleinfinger) which is also explained in the document.

Title: Implementing an analytic version of an inverse kinematic

Body:
People have recommended me implement an analytic version of inverse Jacobian solver, such that I won't be forced only the least square solution, but would have an local area of solution near to the one I desire.

I can't seem to implement it correctly, I mean how much does it differ from the least square inverse kinematics which I have implemented here?

```<code>Eigen::MatrixXd jq(device_.get()->baseJend(state).e().cols(),device_.get()->baseJend(state).e().rows());
      jq = device_.get()->baseJend(state).e(); //Extract J(q) directly from robot

      //Least square solver - [AtA]⁻1AtB

      Eigen::MatrixXd A (6,6);
      A = jq.transpose()*(jq*jq.transpose()).inverse();

      Eigen::VectorXd du(6);
      du(0) = 0.1 - t_tool_base.P().e()[0];
      du(1) = 0 - t_tool_base.P().e()[1];
      du(2) = 0 - t_tool_base.P().e()[2];
      du(3) = 0;  // Should these be set to something if i don't want the tool position to rotate?
      du(4) = 0;
      du(5) = 0;

      ROS_ERROR("What you want!");
      Eigen::VectorXd q(6);
      q = A*du;

      cout << q << endl; // Least square solution - want a vector of solutions.
</code>```

I want a vector of solution - how do I get that?

the Q is related to this https://robotics.stackexchange.com/questions/9672/how-do-i-construct-i-a-transformation-matrix-given-only-x-y-z-of-tool-position

The robot being used is a UR5  - https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_kin_tech_report_1.pdf


问:
How can I improve ZED Camera precision?
I'm using Stereolabs ZED camera for my computer vision project. I did a small research about several sensors on the market and ultimately we decided to go with the ZED Camera.

However I'm finding that the precision of the camera isn't that great. And the Point Cloud takes too much storage space. Anyone found the same problems? And if so, how did you managed them?

答:
For improved precision try calibrating the camera with the Stereo Camera Calibrator app in MATLAB.

As far as the memory taken up by the point cloud, that really depends on how many points you have. You can always try reducing the resolution of the cameras, downsampling the point cloud itself, and/or using a lower precision data type to store the coordinates.

Title: How can I improve ZED Camera precision?


问:
Do magnets affect IMU values?
Im in the process of making a robot which requires 12 3x10mm cylindric magnets for the construction. They are 30mm from the center of the robot where I plan to have the IMU.

I was thinking about using MPU-6050. Do magnets affect the values? If yes, is there a solution for it? like maybe I could have a shield or something around the IMU?

答:
If permanent magnets are rigidly mounted at a fixed distance from the IMU, they have no effect on the accelerometers and gyros inside the MPU-6050.

You can optionally connect the MPU-6050 to an external magnetometer.
(It's used to cancel out yaw drift).
That magnetometer, if you have one, will be affected by magnets.
In theory you could shield the magnetometer by wrapping it in mu-metal,
but that would also shield the magnetometer from the magnetic field of Earth, making the magnetometer useless -- better to leave out the magnetometer and the shield entirely.

Maybe you will be lucky and the magnets will merely shift the magnetometer values, without making it peg out.
If so, there are various ways of calibrating-out this fixed shift, and the IMU could work as well as it would have worked without those magnets.
(It's not clear to me if the MPU-6050 in particular supports such calibration).

I'm assuming your magnets have some purpose, yes?
Generally people use magnets in ways where the magnetic field in one small region is actually used, and any "stray" magnetic field elsewhere doesn't help.
There are several ways to "concentrate" the magnetic field,
making it stronger where it is actually useful, and making the stray fields weaker.

Counter-intuitively, it is possible to add more magnets to your system in such a way that the total magnetic field, at the magnetometer, from all the magnets is almost perfectly cancelled out, leaving only the Earth's magnetic field.
(This often makes the magnetic field elsewhere stronger).

For example, a Halbach array arranges permanent magnets in a way that makes the field stronger on one side, while cancelling the field to almost zero on the other side.

Title: Do magnets affect IMU values?


问:
Is my servo fried?
I got a new servo a few days back (RC Servo, Futaba FP-S148). I first tested it out with the Sweep sketch on Arduino, powering it with the Arduino 5v and GND pins only. It was working, just fine.

Today I was trying to use it in my robot and I tried powering it with 2 LiPo batteries (Samsung ICR16850 2200mAh, from an old laptop battery) connected in series, giving 8.32v. As soon as I connected my servo, it started rotating randomly, I had not connected it to my Arduino yet. I quickly took it out.

Next, I used a L7805 to get 5.13v regulated supply out of my batteries that I used earlier. When I connected my batteries to the servo, and the servo to the Arduino, uploaded the sketch, the servo started behaving rather strangely, it first did a complete turn and then stopped. Only a humming sound came from the servo. Strange thing is, whenever I connect one of my Multimeter leads to the power cables, the servo immediately turned in the opposite direction only as long as only lead was in contact with either the positive or negative wire.

Otherwise, the servo just gives a humming sound.

Have I fried my servo? Or is it some other issue?

UPDATE 1

I stripped down the servo and checked the motor. It is working fine, seems like this is a gear problem.

答:
Yes it's absolutely fried. Check the tech specs of your servo next time.

Power requirement: 4.8V or 6V (from receiver)
  Power consumption: 6V 8mA (at idle)

Title: Is my servo fried?


问:
KUKA robot - update coordinates
I need to develop something in order to update some coordinates in a KUKA KR C4 robot predefined program.

After some research I've found some ways to do it, but all of them non free.

I had several options, like developing a HMI in the console with 3 buttons, to touch up the 3 coordinates that I have to update for example.

Sending a XML file would work too but I need a RSI connection, and I can't do it without proper software (I guess).

Do you know about something like this? Or a C++ library that allows me to have access the .src/.dat files or to create a new one with the same "body" but with different coordinates?

Summing up, imagine that I have a conveyor that carries boxes and I need to develop a pick and place program. So far so good. But every 100 boxes, the size changes (and I can't predict it). So the operator goes and updates the coordinates, but I want to make sure that he won't change anything else in the program. Any ideas?

答:
If you are working with windows then for KUKA programs you can use the (OrangeEdit), check it in this website, OrangeEdit Beta. It is free and it enables you to view and edit all the .src and .dat files and more.

As for the robot program itself, usually you can save it in a USB key, to do so you just log into the KCP of the robot as administrator (default password is "kuka") then plug your key in the robot CPU, then in the KCP, in the menu bar, choose File->Save As->USB and wait for the saving to finish. You end up with a zip file that you can extract and then access via the OrangeEdit.

Normally there are three main folders, the one you will be interested in is (KRC) inside you will have a file called ($config.dat) where normally all the base frames are defined. Actually people, usually, use these base frames to define zones or areas of work, then in each zone another specific program defines the relative displacement. So, for example, in you application you might find a program called pick_cartoon() where the relative displacement, according to the cartoon dimensions, will be saved in the associated .dat file. So if that is the case then what you can do is to add offset to the part in .src file that does the displacement of frame of work, the offset of course express the difference between the two types of cartoons you have. To do so you need to follow the following steps:

First defines your offsets parameters in the $config.dat.

Then program them in a convenient way in your .src file, you can use a counter or presence sensors to differentiate between the two types of cartoon, in this case you need to use some I/O of the robot and those also you can define in the $config.dat file.

Then you make these parameters accessible by the operators by defining them in the ConfigMon file which you can find under the (\C\) folder in the robot program.

BTW, if you are under Linux you can actually access all the files and open them with any text editor, but it is not as nicer as with OrangeEdit, because that software was created specially for KUKA programs.

Let me know if this helps you, or if you have any further questions.

Title: KUKA robot - update coordinates


答:
I don't know what you mean by "precision" and how do you measure it. The sensing
accuracy will probably go back to the camera calibration precision and the stereo matching algorithm used. If they ship the device "calibrated" then no one knows what happened after the camera was calibrated in factory and before you got it (mechanical shock, temperature swings) - so you better re-calibrate it yourself (with OpenCV, Matlab etc.).

If you are interested in the depth accuracy from stereo vision itself then see the following equation:

dz = (z^2 * de) / (f * b)

where dz is the depth error in meters, z is the depth in meters, de is the disparity error in pixels, f is the focal length of the camera in pixels and b is the camera baseline in meters. The main message of the equation above is that the depth error grows with the square of depth (distance from camera) itself.

You get better depth accuracy the bigger f and b are. Given the ZED camera has both of these relatively small - baseline is "just" 120mm and f will be pretty small given the quite large FOV of 110 degrees in diagonal (f is "FOV inverted", so wider FOV gives smaller f). What is good for you is the high resolution which should mean smaller disparity error which could compensate for the bad f and b values.

Regarding point cloud size - this goes back to the rather high sensor resolution of the ZED camera. High resolution may not really be needed for stereo because 1) the stereo matching takes long time 2) the point cloud is huge 3) the depth accuracy may not be much better since the disparity is usually determined with sub-pixel precision anyways. So see what angular resolution you need for your project - you can calculate how many pixels per angle (horizontal, vertical) your camera (=sensor+lens) has. If that is too much, you can lower the angular resolution (=lower sensor resolution) without harming your computer vision results.

Hope this helps!

Title: How can I improve ZED Camera precision?

Body:
I'm using Stereolabs ZED camera for my computer vision project. I did a small research about several sensors on the market and ultimately we decided to go with the ZED Camera.

However I'm finding that the precision of the camera isn't that great. And the Point Cloud takes too much storage space. Anyone found the same problems? And if so, how did you managed them?


问:
Designing Ackerman's Steering Principle for an autonomous robot
I am working on a high speed autonomous robot (about 6-7 m/s), which does obstacle detection as well as senses traffic lights (I have used Raspberry Pi 3 and Arduino Uno).

For the steering mechanism, I wanted to implement an Ackerman's steering. I've read about the principle and have understood its basics. Now to actually make the design, I am currently using switchboards, sold here in India, they are surprisingly strong, lightweight, waterproof(they are switchboards) and cheap. Now I got 1 big axle and the small axle cut out already, along with the two L-shaped pieces that join the 2 axles together... I'm just now confused as to how to connect the wheels to the axle and how to make them rotate along side it. The site won't let me upload any pics right now, I'll try again ASAP.

I have the switchboard, an electric drill and will to do anything to make this happen ( ;P ). I don't have access to a 3D Printer.

Any help would be greatly appreciated...

P.S- And if you have any suggestions of your own, which might be better for my robot, feel free to share them, I'm just looking for a good steering method for my robot.

答:
The key device for Ackermann steering is the steering knuckle. It has two vertical mount and an arm with a third mount.

The upper and lower control arms attach the vertical mounts to the frame of the vehicle - this lets the wheel move up and down. This may or may not be more complicated than you need, as the control arms are what enable suspension. The strut generally attaches to or replaces the upper control arm.

The arm on the steering knuckle allows a tie rod to be attached. The arm is at an angle to the face of the steering knuckle, and that angle is determined by the wheel base and width of the car. From the Wikipedia page on Ackermann steering:

[Ackermann steering] was achieved by making the linkage not a simple parallelogram, but by making the length of the track rod (the moving link between the hubs) shorter than that of the axle, so that the steering arms of the hubs appeared to "toe out".

If you actually had one solid rod between the two tires, it would bend the first time the suspension moved. Instead, the steering gear box is bolted to the frame, and then tie rods attach the steering knuckle to the gear box.

At each end of the tie rod is a "tie rod end" or rod end, which has a spherical bearing and allows the tie rod to move with suspension travel.

So, tl;dr - Ackermann steering is actually a relatively involved setup, especially if you can't source properly sized components. This is why a lot of small and medium sized robots use differential steering and caster wheels.


问:
Implementation of inverse kinematics solution in c++
I am having some issue with implementing a least square solution of the inverse kinematics problem.

The q configuration I get are rather large, or makes no sense, so I was hoping someone here could help me find my error in my program.

```<code>rw::math::Q pathPlanning::invKin(double dx, double dy , double dz)
{

    rw::kinematics::State state =  this->state;
    rw::math::Transform3D<> t_tool_base =  this->device.get()->baseTend(state);

    cout << t_tool_base.R().e() << endl;
    cout << endl;
    cout << t_tool_base.P().e() << endl;
    cout << endl;

    Eigen::MatrixXd jq(this->device.get()->baseJend(state).e().cols(), this->device.get()->baseJend(state).e().rows());
    jq =  this->device.get()->baseJend(state).e();

    //Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du  <=> dq = A*du
    Eigen::MatrixXd A (6,6);
    //A = jq.transpose()*(jq*jq.transpose()).inverse();
    A = (jq*jq.transpose()).inverse()*jq.transpose();

    std::vector<rw::math::Transform3D<> > out = sphere(dx,dy,dz);

    std::ofstream outfile;
    outfile.open("q_conf.txt", std::ios_base::app);

    for(unsigned int i = 0; i <= out.size() ; ++i )
    {
        rw::math::Vector3D<> dif_p = out[i].P()-t_tool_base.P();

        Eigen::Matrix3d dif = out[i].R().e()- t_tool_base.R().e();
        rw::math::Rotation3D<> dif_r(dif);
        rw::math::RPY<> dif_rot(dif_r);

        Eigen::VectorXd du(6);
        du(0) = dif_p[0];
        du(1) = dif_p[1];
        du(2) = dif_p[2];

        du(3) = dif_rot[0];
        du(4) = dif_rot[1];
        du(5) = dif_rot[2];

        Eigen::VectorXd q(6);
        q = A*du;

        rw::math::Q q_current;
        q_current = this->device->getQ(this->state);
        rw::math::Q dq(q);
        rw::math::Q q_new = q_current+ dq;

        //cout << jq << endl;
        //cout << endl;
        //std::string text = "setQ{"  + to_string(q_new[0])  + ", " + to_string(q_new[1]) + ", " + to_string(q_new[2]) + ", " + to_string(q_new[3]) + ", " + to_string(q_new[4]) + ", " + to_string(q_new[5]) + "}";
        //cout << text << endl;
        //outfile << text << endl;

    }

    rw::math::Q bla(6); //Just used the text file for debugging purposes,  Which why I just return a random Q config.
    return bla;
}

rw::math::Transform3D<> pathPlanning::transform(double obj_x, double obj_y, double obj_z, double sphere_x, double sphere_y ,double sphere_z)
{
    // Z-axis should be oriented towards the object.
    // Rot consist of 3 direction vector [x,y,z] which describes how the axis should be oriented in the world space.
    // Looking at the simulation the z-axis is the camera out. X, and Y describes the orientation of the camera.
    // The vector are only for direction purposes, so they have to be normalized....
    // TODO: case [0  0 -1]... Why is it happening at what can be done to undo it?

    rw::math::Vector3D<> dir_z((obj_x - sphere_x), (obj_y - sphere_y), (obj_z - sphere_z));
    dir_z = normalize(dir_z);
    rw::math::Vector3D<> downPlane(0.0,0.0,-1.0);
    rw::math::Vector3D<> dir_x = cross(downPlane,dir_z);
    dir_x = normalize(dir_x);
    rw::math::Vector3D<> dir_y = cross(dir_z,dir_x);
    dir_y = normalize(dir_y);

    rw::math::Rotation3D<> rot_out (dir_x,dir_y,dir_z);

    rw::math::Vector3D<> pos_out(sphere_x,sphere_y,sphere_z);

    rw::math::Transform3D<> out(pos_out,rot_out);
    return out;
}

std::vector<rw::math::Transform3D<>> pathPlanning::sphere(double dx, double dy, double dz)
{
    double r = 0.50; // Radius of the sphere -  set to 0.50 cm (TODO: has to be checked if that also is accurate)
    cout << "Create a sphere" << endl;

    double current_x = this->device->baseTend(this->state).P()[0];
    double current_y = this->device->baseTend(this->state).P()[1];
    double current_z = this->device->baseTend(this->state).P()[2];

    rw::math::Vector3D<> center(current_x + dx, current_y + dy , current_z + dz);

    // Formula for sphere (x-x0)²+(y-y0)²+(z-z0)²=r²
    // x: x = x_0 + rcos(theta)sin(phi)
    // y: y = y_0 + rsin(theta)sin(phi)
    // z: z = z_0 + rcos(phi)
    // Angle range: 0 <= theta <= 2M_PI ; 0 <= phi <= M_PI

    double obj_x = current_x + dx;
    double obj_y = current_y + dy;
    double obj_z = current_z + dz;

    ofstream positions;
    ofstream rotations_z;
    ofstream rotations_y;
    ofstream rotations_x;
    positions.open ("sphere_positions.csv");
    rotations_z.open("z_dir.csv");
    rotations_y.open("y_dir.csv");
    rotations_x.open("x_dir.csv");
    std::vector<rw::math::Transform3D<>> out;

    int count = 32;
    for(double theta = 0; theta <= 2*M_PI ; theta+=0.1 )
    {

        for(double phi = 0; phi <= M_PI ; phi+=0.1)
        {

            double sphere_x = obj_x + r*cos(theta)*sin(phi);
            double sphere_y = obj_y + r*sin(theta)*sin(phi);
            double sphere_z = obj_z + + r*cos(phi);

            string text = to_string(sphere_x) + " , " + to_string(sphere_y)+ " , " + to_string(sphere_z);
            positions << text << endl;

            rw::math::Transform3D<> transformation_matrix = transform(obj_x,obj_y,obj_z,sphere_x,sphere_y,sphere_z);

            string text2 = to_string(transformation_matrix.R().e()(0,2)) + " , "  + to_string(transformation_matrix.R().e()(1,2)) + " , " + to_string(transformation_matrix.R().e()(2,2));
            string text1 = to_string(transformation_matrix.R().e()(0,1)) + " , "  + to_string(transformation_matrix.R().e()(1,1)) + " , " + to_string(transformation_matrix.R().e()(2,1));
            string text0 = to_string(transformation_matrix.R().e()(0,0)) + " , "  + to_string(transformation_matrix.R().e()(1,0)) + " , " + to_string(transformation_matrix.R().e()(2,0));

            rotations_z << text2 << endl;
            rotations_y << text1 << endl;
            rotations_x << text0 << endl;

            if(count == 32) //TODO: Why...... is this occuring?
            {
                //cout << "Theta: " << theta << " Phi: " << phi << endl;
                //cout << sphere_x << " , " << sphere_y <<" , "<< sphere_z << endl;
                count = 0;
            }
            else
            {
                count++;
            }

            out.push_back(transformation_matrix);
        }
    }

    positions.close();
    rotations_z.close();
    rotations_y.close();
    rotations_x.close();
    cout << endl;
    cout <<"Object at: " << obj_x << "," << obj_y << "," << obj_z << endl;
    cout << "done " << endl;
    return out;
}
</code>```

What am I trying to do, I am trying to orbit a robot endeffector  around an object in the center. The trajectory of the endeffector is an sphere where the endeffector should always point in to the object.  The sphere function should compute all transformation matrices which move the robot arm to the different position on the sphere with a given rotation, and the inverse kinematics should compute all the different Q-states, given an x,y,z which is the actual displacement to the object itself.

I am not quite sure where my error could be at, but I think it might either be at transform function where I generate my desired transformation matrix, or in invKin where I create du, I think I might have made an mistake in creating du(3),  du(4), du(5)

The libraries I've been using is Eigen, robwork (basically all rw::) if anyone want to look syntax through.

Update

Based on @ghanimmukhtar I began checking for singularities for the jacobian.. Which seems in general supringsly low. I computed it for a list of random Q configurations which resulted into this...

```<code>Determinant: -0.0577779
Determinant: -0.0582286
Determinant: 0.0051402
Determinant: -0.0498886
Determinant: 0.0209685
Determinant: 0.00372222
Determinant: 0.047645
Determinant: 0.0442362
Determinant: -0.0799746
Determinant: 0.00194714
Determinant: 0.0228195
Determinant: 0.096449
Determinant: -0.0339612
Determinant: -0.00365521
Determinant: -0.030022
Determinant: 0.021347
Determinant: 0.0413364
Determinant: 0.0041136
Determinant: -0.0151192
Determinant: 0.0682926
Determinant: -0.0657176
Determinant: 0.0915473
Determinant: -0.00516008
Determinant: -0.0394664
Determinant: -0.00469664
Determinant: 0.0494431
Determinant: -0.00156804
Determinant: -0.0402393
Determinant: -0.0141511
Determinant: 0.0203508
Determinant: -0.0368337
Determinant: -0.0313431
Determinant: -0.0566811
Determinant: -0.00766113
Determinant: -0.051767
Determinant: -0.00815555
Determinant: 0.0564639
Determinant: 0.0764514
Determinant: -0.0501299
Determinant: -0.00056537
Determinant: -0.0308103
Determinant: -0.0091592
Determinant: 0.0602148
Determinant: -0.0051255
Determinant: 0.0426342
Determinant: -0.0850566
Determinant: -0.0353419
Determinant: 0.0448761
Determinant: -0.0103023
Determinant: -0.0123843
Determinant: -0.00160566
Determinant: 0.00558663
Determinant: 0.0173488
Determinant: 0.0170783
Determinant: 0.0588363
Determinant: -0.000788464
Determinant: 0.052941
Determinant: 0.064341
Determinant: 0.00084967
Determinant: 0.00716674
Determinant: -0.0978426
Determinant: -0.0585773
Determinant: 0.038732
Determinant: -0.00489957
Determinant: -0.0460029
Determinant: 0.00269656
Determinant: 0.000600619
Determinant: -0.0408527
Determinant: -0.00115296
Determinant: 0.013114
Determinant: 0.0366423
Determinant: 0.0495209
Determinant: -0.042201
Determinant: -0.036663
Determinant: -0.103452
Determinant: -0.0119054
Determinant: 0.0692284
Determinant: -0.00717832
Determinant: 0.00729104
Determinant: 0.0126415
Determinant: -0.00515246
Determinant: -0.0556505
Determinant: 0.000670701
Determinant: -0.0545629
Determinant: 0.00251946
Determinant: 0.0405189
Determinant: 0.010928
Determinant: -0.00101032
Determinant: 0.0308612
Determinant: 0.0536183
Determinant: -0.0439223
Determinant: -0.0113453
Determinant: -0.0193872
Determinant: 0.0660165
Determinant: -0.00184695
Determinant: -0.106904
Determinant: 0.01246
Determinant: -0.00883772
Determinant: 0.0601036
Determinant: 0.0468602
Determinant: 0.0513812
Determinant: -0.000663089
Determinant: -0.00392395
Determinant: 0.0710837
Determinant: 0.0629583
Determinant: -0.0464579
Determinant: 0.0257618
Determinant: -0.0193227
Determinant: 0.00388693
Determinant: -0.02003
Determinant: 0.0191158
Determinant: -0.00159198
Determinant: -0.0702308
Determinant: -0.0242876
Determinant: -0.00934638
Determinant: -0.00221986
Determinant: -0.0268925
Determinant: 0.0596055
Determinant: -0.00925273
Determinant: -0.0167357
Determinant: 0.0596476
Determinant: -0.00515798
Determinant: -0.00324081
Determinant: -0.00321565
Determinant: 0.0669645
Determinant: -0.0342913
Determinant: -0.000342155
Determinant: -0.0104422
Determinant: -0.0410489
Determinant: -0.0246036
Determinant: 0.0208562
Determinant: -0.0692963
Determinant: 0.000839091
Determinant: -0.049308
Determinant: -0.0349338
Determinant: 0.0016057
Determinant: -0.00214381
Determinant: -0.0332965
Determinant: 0.0168007
Determinant: -0.0748581
Determinant: -0.00864737
Determinant: -0.0638044
Determinant: -0.00103911
Determinant: -0.00690918
Determinant: 0.000285789
Determinant: 0.0215414
Determinant: 0.0560827
Determinant: -0.0063201
Determinant: -0.00677609
Determinant: -0.00686829
Determinant: 0.0591599
Determinant: 0.0112705
Determinant: 0.0874784
Determinant: -0.0146124
Determinant: -0.0133718
Determinant: -0.0203801
Determinant: -0.0150386
Determinant: -0.102603
Determinant: -0.077111
Determinant: 0.021146
Determinant: 0.089761
Determinant: -0.0532867
Determinant: -0.0620632
Determinant: -0.0165414
Determinant: -0.0461426
Determinant: 0.00144256
Determinant: 0.00844777
Determinant: 0.0893306
Determinant: -0.0814478
Determinant: -0.0890507
Determinant: -0.0472091
Determinant: 0.0186799
Determinant: -0.00224087
Determinant: -0.0242662
Determinant: -0.00195303
Determinant: 0.014432
Determinant: 0.00185717
Determinant: -0.0354357
Determinant: -0.0427957
Determinant: -0.0380409
Determinant: 0.0627548
Determinant: 0.0397546
Determinant: 0.0570439
Determinant: 0.106265
Determinant: 0.0382001
Determinant: -0.0240826
Determinant: -0.0866264
Determinant: 0.024184
Determinant: 0.0841286
Determinant: -0.0303611
Determinant: -0.0337029
Determinant: -0.0202875
Determinant: 0.0643731
Determinant: -0.0475265
Determinant: -0.00928736
Determinant: -0.00373402
Determinant: 0.0636828
Determinant: 0.0122532
Determinant: 0.0398141
Determinant: -0.0563998
Determinant: -0.0778303
Determinant: 0.0164747
Determinant: 0.0314815
Determinant: 0.0744507
Determinant: -0.0897675
Determinant: 0.0260324
Determinant: -0.0734512
Determinant: 0.000234548
Determinant: -0.0238522
Determinant: -0.0849523
Determinant: 0.0204877
Determinant: -0.0715147
Determinant: 0.0703858
Determinant: -0.0142186
Determinant: -0.101503
Determinant: 0.03966
Determinant: 4.69111e-05
Determinant: 0.0394428
Determinant: 0.0409131
Determinant: 8.90995e-05
Determinant: -0.00841189
Determinant: -0.0671323
Determinant: 0.00805167
Determinant: -0.00292435
Determinant: 0.0507716
Determinant: 0.0493995
Determinant: 0.00629414
Determinant: -0.0428982
Determinant: -0.0446924
Determinant: 0.0776236
Determinant: 0.00440478
Determinant: -0.0463321
Determinant: -0.00247224
Determinant: -0.0199861
Determinant: 0.0267022
Determinant: 0.0184179
Determinant: 0.0104588
Determinant: 0.116535
Determinant: -0.0857382
Determinant: -0.0477216
Determinant: 0.0286968
Determinant: 0.0387932
Determinant: 0.042856
Determinant: -0.0964
Determinant: 0.0320456
Determinant: -0.0676327
Determinant: 0.0156632
Determinant: 0.0548582
Determinant: 0.0394791
Determinant: 0.0863353
Determinant: -0.0568753
Determinant: -0.00953039
Determinant: -0.0534666
Determinant: 0.0506779
Determinant: 0.00521034
Determinant: 0.0353338
Determinant: 0.0845463
Determinant: -0.00847695
Determinant: 0.015726
Determinant: -0.0648035
Determinant: 0.0170917
Determinant: 0.0045193
Determinant: -0.0195397
Determinant: 0.00630076
Determinant: -0.0137401
Determinant: 0.0209229
Determinant: 0.00382077
Determinant: -0.0588661
Determinant: -0.0923883
Determinant: -0.00726003
Determinant: -0.0411533
Determinant: 0.00544489
Determinant: 0.0101791
Determinant: 0.0903306
Determinant: -0.0590416
Determinant: -0.0377112
Determinant: -0.0150455
Determinant: 0.0793066
Determinant: 0.0425759
Determinant: -0.040728
Determinant: -0.0376792
Determinant: -0.0387703
Determinant: -0.0232208
Determinant: 0.0506747
Determinant: -0.0284409
Determinant: 0.000536999
Determinant: -0.0289103
Determinant: -0.00586449
Determinant: -0.0805586
Determinant: 0.0133906
Determinant: -0.00311773
Determinant: 0.0184798
Determinant: -0.00981978
Determinant: -0.0491601
Determinant: 0.0452526
Determinant: 0.00411708
Determinant: -0.0515142
Determinant: 0.0121114
Determinant: 0.00636972
Determinant: -0.0126048
Determinant: -0.0412662
Determinant: 0.00195264
Determinant: -0.0726478
Determinant: 0.0692254
Determinant: -0.0256477
Determinant: 0.0702529
Determinant: -0.0052493
Determinant: 0.0625172
Determinant: 0.00282606
Determinant: 0.0229033
Determinant: 0.0558893
Determinant: 0.0766217
Determinant: -0.00388679
Determinant: -0.0193821
Determinant: -0.00718189
Determinant: -0.0864566
Determinant: 0.0809026
Determinant: -0.0398232
Determinant: -0.00224801
Determinant: 0.0333072
Determinant: -0.0212002
Determinant: 0.00371396
Determinant: 0.0162035
Determinant: -0.0811845
Determinant: 0.0148128
Determinant: 0.0372953
Determinant: 0.00351286
Determinant: -0.00103575
Determinant: 0.0384813
Determinant: 0.00752738
Determinant: -0.0248252
Determinant: -0.106768
Determinant: -0.0192333
Determinant: -0.026543
Determinant: -0.0222608
Determinant: -0.0487862
Determinant: 0.00376402
Determinant: -0.0329469
Determinant: 0.00266775
Determinant: 0.0762491
Determinant: 0.0159609
Determinant: -0.0190175
Determinant: -0.0338969
Determinant: -0.0631867
Determinant: -0.0238901
Determinant: 0.107709
Determinant: -7.74935e-05
Determinant: -0.0468996
Determinant: 0.0462787
Determinant: 0.0387825
Determinant: 0.0753388
Determinant: -0.000279933
Determinant: 0.00638663
Determinant: -0.00458034
Determinant: 0.0185849
Determinant: -0.00543503
Determinant: -0.0520309
Determinant: -0.0234638
Determinant: 0.0593986
Determinant: -0.00036774
Determinant: 0.00960819
Determinant: -0.00685314
Determinant: -0.000176925
Determinant: 0.0207583
Determinant: -0.0337003
Determinant: -0.0534818
Determinant: 0.0142158
Determinant: -0.0728077
Determinant: 0.0246877
Determinant: -0.0660952
Determinant: -0.0466
Determinant: 0.0915457
Determinant: -0.00340539
Determinant: 0.00815076
Determinant: -0.0751806
Determinant: -0.00617677
Determinant: 0.0019761
Determinant: -0.0016673
Determinant: 0.0310364
Determinant: 0.0483121
Determinant: -0.00664964
Determinant: 0.0659273
Determinant: -0.019015
Determinant: 0.0087627
Determinant: 0.0267279
Determinant: 0.0253497
Determinant: 0.00246292
Determinant: -0.0684746
Determinant: -0.0234524
Determinant: -0.0197933
Determinant: 0.0120796
Determinant: -0.0192703
Determinant: 0.0853956
Determinant: 0.0388196
Determinant: -0.0599305
Determinant: -0.0626148
Determinant: 0.0258541
Determinant: -0.0341273
Determinant: 0.0972889
Determinant: -0.0306585
Determinant: 0.0188553
Determinant: 0.00247702
Determinant: -0.00368989
Determinant: -0.0951982
Determinant: 0.0113578
Determinant: 0.000762509
Determinant: -0.0225219
Determinant: 0.0414059
Determinant: -0.0244409
Determinant: -0.0425728
Determinant: 0.04275
Determinant: -0.0413427
Determinant: -0.00556264
Determinant: -0.0894398
Determinant: -0.0193197
Determinant: -0.00788038
Determinant: -0.00455421
Determinant: -0.0788177
Determinant: 0.0415381
Determinant: -0.0346766
Determinant: -0.0748027
Determinant: 0.0087688
Determinant: -0.0968796
Determinant: 0.0683526
Determinant: -0.00996678
Determinant: 0.00955922
Determinant: -0.0914706
Determinant: 0.0728304
Determinant: 0.0541784
Determinant: 0.0457072
Determinant: -0.0299529
Determinant: -0.0096473
Determinant: -0.0142643
Determinant: -0.0684794
Determinant: 0.00281004
Determinant: -0.03252
Determinant: -0.0144637
Determinant: 0.0294154
Determinant: 0.00574353
Determinant: -0.019569
Determinant: 0.00492446
Determinant: -0.0526394
Determinant: -0.000870143
Determinant: -0.0180984
Determinant: -0.0144104
Determinant: 0.0456077
Determinant: -0.0113433
Determinant: 0.00377549
Determinant: -0.0775854
Determinant: -0.0336789
Determinant: -0.0744995
Determinant: -0.0427397
Determinant: 0.0300061
Determinant: -0.0326518
Determinant: -0.0333735
Determinant: -0.0284057
Determinant: -0.00999835
Determinant: -0.0380404
Determinant: 0.00648521
Determinant: 0.0449298
Determinant: 0.0120318
Determinant: -0.0230653
Determinant: -0.00934067
Determinant: -0.0175326
Determinant: -0.0799447
Determinant: 0.0679027
Determinant: -0.00670324
Determinant: -0.0841748
Determinant: 0.0236213
Determinant: 0.0386624
Determinant: -0.0239495
Determinant: 0.076976
Determinant: -0.00997484
Determinant: 0.025157
Determinant: -0.0654046
Determinant: 0.0090564
Determinant: 0.00129045
Determinant: -0.105119
Determinant: 0.0976925
Determinant: -0.105149
Determinant: -0.0465851
Determinant: 0.00237453
Determinant: -0.0456927
Determinant: 0.0328236
Determinant: -0.0914691
Determinant: -0.0157904
Determinant: -0.00170804
Determinant: -0.014797
Determinant: 0.00464912
Determinant: -0.035118
Determinant: -0.0242306
Determinant: 0.0081405
Determinant: 0.0733502
Determinant: -0.0860252
Determinant: -0.0511219
Determinant: -0.0925647
Determinant: 0.0495087
Determinant: -0.0515914
Determinant: -0.044318
Determinant: 0.000900043
Determinant: 0.0632521
Determinant: 0.00957955
Determinant: 0.00598059
Determinant: 0.0179513
Determinant: 0.0952263
</code>```

dx,dy,dz is a is the distance between tcp and an object i want to keep in sight. The sphere is like a safety zone, but is mainly used to compute the orientation of the tool.

答:
It is a bit hard to know the exact cause of the error without having the (RobWork), I tried to install it following the link your provided, but I ran into some problems and I don't have the time to fix them at the moment. But I can point you to some issues that might be the source of your problem:

The transform method you are given in this code doesn't have a return statement, though you have declared it to return a rw::math::Transform3D<>.

If you are near a singularity the inverse kinematics will normally return high values. The singularities depend on the robot structure you are using. You can detect this by seeing the determinant of your jacobian matrix, the closer it is to zero the closer you are to a singularity.

Also I don't understand why you use the line rw::math::Q dq(q), because normally the inverse kinematic should return dq if your desired displacement is small enough (i.e. du) so normally you should be able to add it directly to add q directly to the q_current, so may be it could be of interest to check the raw values for these variables.

Also may be it could be more accurate to use a polynomial interpolater to produce the du and then run the whole inverse kinematic in a while loop with a boos::timer for instance. I have actually developed and inverse kinematic for Crustcrawler arm that does that, but I use joint velocity control directly since the output of the inverse kinematic is actually joints velocity, also I am not binding myself to move in sphere I just give a goal position and duration and the model drive the end effecter nicely to it via joints velocities.

If you have more details please share them, it could help in identifying the issue more effectively. Like how invKin is called? and what exactly are those parameters passed to it (i.e. x, y, z)?


答:
Hard to tell in this exact case.  I looked up the MPU-6050 specs and I am unsure whether it integrates a digital compass to combat gyro drift.  On Sparkfun, it refers to it being a '9 axis fusion algorithm' which implies compass (three axis each for gyro, accel, and magento) but elsewhere it only refers to gyro and accel.

I was doing some related work with a Pololu MinIMU-9 which has all nine axis but requires you to implement the IMU integration logic in code.  What I found with that part was that placing it within 10 cm of the drive motors was causing the magnometer to be very difficult to use.  On one hand you can do a calibration and remove the static magnetic fields from your reading (assuming your IMU routines are coded for this).  On the other hand, I was finding that the strength of the static motor fields was so strong that the comparatively weaker Earth's magnetic fields were down in the 'noise'.  I was having to tune down the flux sensitivity to handle the drive field so my sensitivity to the Earth's field also dropped.

This fix was to ensure the magnometer was sufficiently far away from the drives that their fields were sufficiently smaller than the Earth's fields.  In my case I had to move the IMU sensor about 50 cm away from the motors.

So in summary, 'it depends' :-)  It depends on your sensors, the strength of your local magnetic fields, and whether you can calibrate out the local fields in your IMU logic.

Title: Do magnets affect IMU values?

Body:
Im in the process of making a robot which requires 12 3x10mm cylindric magnets for the construction. They are 30mm from the center of the robot where I plan to have the IMU.

I was thinking about using MPU-6050. Do magnets affect the values? If yes, is there a solution for it? like maybe I could have a shield or something around the IMU?