问:
Center line steering with differential drive
I’ve got 2 motors that drives my unit and I can easily steer it manually by simply adding more speed to one wheel vs the other.

Now I’m automating the rover using image processing to follow a driveway. As the rover travels I calculate the distance in pixels between the rover center and the target (middle of driveway). I then use some factor (1.1) and use that to add speed to the correct wheel.

Kinda works good until it needs to make large corrections. I can only update the speed every .75secs.

After it turns and drives to the middle the wheel speeds are equalized - but the rover is pointing in the direction of the turn. In the best case it then goes past the center before correcting back again and eventually stabilizes.

Original Poster here with some more details of what i've tried so far. Attached is a frame from my imagine processing. As you can see i have 2 green circles along the edges that define the sides of the road and a green circle in the middle that is simply the mid-point between them (TARGET). This isn't a great example because I also have a red circle behind the center which indicates the center of the camera (CENTER). When i am not centered on the driveway the TARGET will be some number of pixels from CENTER - this is my DELTA as indicated in the legends.

So what i have so far is that i take DELTA and multiple it by 1.1 and then add that to the correct wheel to steer it back towards the center. Each time I get a new image frame (every .5 - 1sec) DELTA is smaller and I reduce the rate of the turn - but its still turning. Once I get to the center or past the center I either make both motors the same or i add to the opposite motor since i over corrected.

If things go well this actually works good enough - i get some S turns as it zeros in on the center. Problem is when the image processing doesn't work perfectly and the algorithm needs to make large changes. In this case the DELTA needs to be significant to turn it but then reduce so it doesn't turn beyond the center point.

In a perfect world i would be able to turn a calculated amount depending on how far off i was (size of DELTA), drive straight to the center, then turn in opposite direction a specific amount to center the rover. Problem is i need to do this relative to each frame from the camera as the DELTA gets smaller as i am correcting.

This really feels like i'm trying to invent a new algorithm for something i have a feeling has been solved a long time ago. I am not 100% sure what a PID controller does but I have a feeling it may help here. Or do i need some sort of additional piece of information (feedback?) to indicate my orientation?

I don't need perfect here - just want the rover not to go off the side of the driveway...

答:
Based on the suggestion “look into PID”l controller” I did some more research on that for line following and that’s exactly what I was looking for. Funny in some ways my naive algorithm already uses a P factor to adjust the error. The I and D will help smooth out the amount of overshoot when I hit the center and minimize the “S turns” to stabilize.

Will need to fine tune it but this seems like the standard approach for line following (which is basically what my image processing does). However it is very dependent on the initial position being with the correct orientation and close to the line. If the rover gets too far out of position it may not be recoverable. But I’ll have to solve for that with additional logic.

So the answer (thanks chuck or bill) is applying PiD control algorithm to calculate the error correction to the individual motors.


问:
Denavit Hartenberg Exercises
I'm reading "Robot Modeling and Control" by Spong. It seems to me there are mistakes in the manual solutions provided by the authors. I came across the following two questions

In this answer I think there is no $d_3$. It should be zero. In the following question

In my solution $\alpha_2=-90^\circ$ not $90^\circ$ and $\theta_3 = 0^\circ$ not $-90^\circ$. I would love to hear your feedback. Also, if you are instructor, you may need to double check this.

答:
yes you are right in both models.

but add extending length of prismatic joint as it may be able to extend further
and add theta to revolute joints

add d1+a1 in the place of d1 and so on

hope it helps

ContentLicense: CC BY-SA 4.0

Title: Denavit Hartenberg Exercises


问:
How to avoid oscillations caused by backlash in position control?
I am working with a ball screw mechanism driven by a brushless dc motor with an encoder mounted on the load side. We are using a PID position controller with tuning settings by optimization however the ball screw keeps oscillating around the reference position. We are thinking it is caused by the backlash and we accept the precision reduction caused by it. The only thing we would like to have is a non-oscillating response. Is there any trick/technique or any source that we can get an idea of?

Thanks in advance,

答: Backlash is only an issue when you change directions, because that's when you'll open the space in the gear train. If you're using PID control, consider an overdamped response instead of a typical "critically damped" response that overshoots. An overdamped response should have no overshoot and thus no directional change. You'll have a slower response, but should eliminate the oscillation/chatter.</p>


问:
Aligning the coordinate systems of accelerometer and vehicle
I have an accelerometer located in an arbitrary position on a car. As the accelerometer already has a coordinate system by default, I want to find some method that allows transforming the data I have from the sensor and aligning them with the coordinate system of the car. I have read several papers but I have not found the solution. I know that one of the first steps is to have the vector with the car stopped and then running and obtain a rotation matrix, a hand would be of great help to solve my case.</p>

答:
First let's define a right-handed coordinate system for the car. Let X point to the right, Y straight ahead, and Z straight up. Next we'll measure where the car's axes are in the accelerometer coordinate system. (Assume the accelerometer calibration units/G are the same in its X, Y, Z.)

Measuring car -Z is easy -- that's 1 G, straight down, as long as the car is parked on level ground not too far from sea level. The magnitude of the measurement vector calibrates its units/G.

To find car +Y, accelerate the car straight forward. Capture the accelerometer measurement, and subtract the reading that it showed for -Z. The difference points in the car's +Y direction. Its magnitude isn't important; normalize its magnitude to match the 1 G calibration from -Z.

The car +X direction is the cross product of the normalized (car +Y) and (car +Z). Note the Z sign change.

Stack up those X,Y,Z measurement vectors by rows into a matrix, and you have a way to transform car coordinates to accelerometer readings.

Finally, the inverse of that matrix gives a way to transform what the accelerometer reports to what the car is doing.

What if the road was unavoidably bumpy while driving for the +Y measurement? If the bumps were strictly up and down, I think you can subtract some multiple of the Z measurement to make the net Z projection zero. But I haven't worked through those details. If the car is bouncing in X and/or Y directions, or if you can't accelerate it straight ahead, I guess move the project to Bonneville Salt Flats :-)

I haven't tested this procedure, so I hope it works for you. Good luck!


问:
What control circuit element will control an ESC while doing bench testing?
While doing bench testing of motor and prop combinations what simple device would I use in the circuit to properly control the ESC for the full range of power from 0% to 100% ?
I have seen a few videos where bench testing is demonstrated to measure the thrust and power usage of a brushless motor and propeller. Typically the control of the ESC is done with a very small device attached to the ESC? What is that device? (I find it hard to believe it as simple as a rheostat, or is it?)</p>

答: If your ESC uses servo-like control signals, the you want to send it a servo PWM pulse, generally at 50hz (20ms) with a pulse between 1ms (5% duty cycle) which is full reverse, and 2ms (10% duty cycle) which is full forward.  You can generate that signal with a microcontroller or a cheap servo tester.  Search amazon or aliexpress for 'serve tester' and you will find things like this, https://www.amazon.com/ICQUANZX-Aluminum-Consistency-Controler-Helicopter/dp/B081D5VHVY/ref=sr_1_6</p>


问:
Feed-forward controller with lookup table
does anyone have experience in implementing a feed-forward controller for a hydraulic cylinder?

Often, when a hydraulic cylinder is controlled with PWM values, people (https://drive.google.com/file/d/1w7QMFdTzgVR4rFUxGR5WB2nv9o54hpzD/view?usp=sharing) seem to create a lookup table for the working range of the cylinder (input: PWM -> output: velocity) - see Fig.2 in the paper. Afterward, they invert this stored lookup table so that they have a mapping relationship (input: velocity -> output: PWM) which can be then used for control algorithms.

This somehow sounds very simple... Is there anything else that I am missing in this approach? For example from the attached paper? Or any other approach except PID?

答:
I don't have time to read the paper right now, but yeah generally feed-forward just means providing the signal you expect will give you the output you want. That would mean you have some advance knowledge of how to jockey the system to get the output you want, which is some kind of model. The model can be anything - differential equations, a lookup table, etc. Whatever you want that lets you go, "Okay, I need X input to get Y output."

The beautiful thing about feed-forward control is that you don't need an error term like you would for PID control. Because a PID control only acts on reference error the PID controller can only ever be reactive. Another issue with PID control is that it can be very difficult to adequately tune it to large differences in reference magnitudes or to large variations in reference shapes (ramp vs. step, etc.).

When you do feed-forward control you minimize the control error because your system is proactive - you start taking action now to meet the reference instead of waiting for the reference error to build up large enough to become actionable. Additionally, you minimize the magnitude of the reference error across a variety of scenarios because the feed-forward control does the "bulk" action and the feedback control exists to make up for the modeling errors or disturbance inputs. Getting to a more repeatable reference error means you can better tune your PID controller and that the gains you choose will be appropriate for a wider range of scenarios.


问:
How this DH-parameters is obtained for this manipulator
Take a look at the following picture. I'm wondering how $\theta_3 -90^\circ$ appears in the table. As far as I see from the way axes arranged, it is only $\theta_3$.

答: Usually, this type of offset comes from the fact, that you want to corelate your actual angular values of your joints (motors) with your kinematic model. Most probably the hardware in the configuration chosen in the image has the joint coordinates $(0, 0, 90)$. So adding that -90 offset accounts for discrepancies between the theoretical kinematic model and the actual joint angle value for the respective position of the hardware.


问:
Add image onto box in URDF file
I'm trying to display an image on top of a box in a .urdf file. How could I achieve this?</p>

答:
This ROS Answers response walks you through the steps. My answer is a summary of that post.

To add your image, called a texture, you first create a material file, let's call it box.material and input your image file:

```<code>material YourMaterialName{
    technique
    {
        pass
        {
            texture_unit
            {
                texture your_material_image.jpg
                scale 1 1
            }
        }
    }
}
</code>```

Then, in your URDF file, you add a reference to the material above the link, like so:

```<code><gazebo reference="your_link">
    <material>YourMaterialName</material>
</gazebo>
</code>```

Note that this approach only works for URDF primitives (boxes, cylinders, etc). If you are using a mesh file for your URDF you will need to use a different approach with a more complex material file.

Title: Add image onto box in URDF file

Body:
This ROS Answers response walks you through the steps. My answer is a summary of that post.

To add your image, called a texture, you first create a material file, let's call it box.material and input your image file:

material YourMaterialName{
    technique
    {
        pass
        {
            texture_unit
            {
                texture your_material_image.jpg
                scale 1 1
            }
        }
    }
}

Then, in your URDF file, you add a reference to the material above the link, like so:

<gazebo reference="your_link">
    <material>YourMaterialName</material>
</gazebo>

Note that this approach only works for URDF primitives (boxes, cylinders, etc). If you are using a mesh file for your URDF you will need to use a different approach with a more complex material file.


问:
roslaunch exit code 2 error
I am implementing a simple roslaunch file but I am getting an exit code 2 status I have pasted the exact log below any idea what would cause this and how I can rectify it.

```<code>kshitij@Kshitij:~/catkin_ws$ roslaunch apriltag_ros apriltag.launch
... logging to /home/kshitij/.ros/log/c7672d6c-479d-11ec-bd02-c56b9aa24743/roslaunch-Kshitij-20782.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Kshitij:38583/

SUMMARY
========

PARAMETERS
* /rosdistro: noetic
* /rosversion: 1.15.13

NODES
/
apriltag_ros (apriltag_ros/tagdetector.py)

auto-starting new master
process[master]: started with pid [20790]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to c7672d6c-479d-11ec-bd02-c56b9aa24743
process[rosout-1]: started with pid [20800]
started core service [/rosout]
process[apriltag_ros-2]: started with pid [20803]
usage: tagdetector.py [-h] [-f FAMILIES] [-B N] [-t N] [-x SCALE] [-b SIGMA] [-0] [-1] [-2] [-c]
tagdetector.py: error: unrecognized arguments: __name:=apriltag_ros __log:=/home/kshitij/.ros/log/c7672d6c-479d-11ec-bd02-c56b9aa24743/apriltag_ros-2.log
[apriltag_ros-2] process has died [pid 20803, exit code 2, cmd /home/kshitij/catkin_ws/src/apriltag_ros/scripts/tagdetector.py __name:=apriltag_ros __log:=/home/kshitij/.ros/log/c7672d6c-479d-11ec-bd02-c56b9aa24743/apriltag_ros-2.log].
log file: /home/kshitij/.ros/log/c7672d6c-479d-11ec-bd02-c56b9aa24743/apriltag_ros-2*.log
</code>```

The launch file code is as below

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

<launch>
    <node name = "apriltag_ros" pkg = "apriltag_ros" type = "tagdetector.py"  output="screen" />
</launch>
</code>```

Edit 1: I have added the function that I am using to publish to ROS where the ROS node is initialised and used.

The function I am using to publish the data is as below

```<code>def location_publisher():
    """
    ROS Publisher: Publishes X, Y and Yaw values
    """
    pub = rospy.Publisher('apriltag_pose', tag, queue_size=10)
    rospy.init_node('apriltag_ros', anonymous=False)
    msg = tag()
    msg.location.x = z # z in camera frame of reference is the distance from the tag i.e. x in general frame of reference
    msg.location.y = y
    msg.location.theta = yaw
    msg.status.data = status_tag
    msg.tagid.data = tag_id
    rospy.loginfo(msg)
    pub.publish(msg)

# Detect AprilTag
parser = ArgumentParser(description='Detect AprilTags from video stream.')
apriltagbase.add_arguments(parser)
options = parser.parse_args()

detector = apriltagbase.Detector(options, searchpath=apriltagbase._get_dll_path())

while(video.isOpened()):

    check,frame = video.read()

    if not check:
        break

    # overlay box on AprilTag format of detect_tags can be viewd in apriltag.py line 590.
    result,overlay = apriltagbase.detect_tags(frame,
                                        detector,
                                        camera_params=(565.348501, 565.653872, 326.910261, 226.544390),
                                        tag_size=0.1688,
                                        vizualization=3,
                                        verbose=3,
                                        annotation=True
                                        )
    cv2.imshow('April Tag', overlay)

</code>```

Link to the same question on the ROS Forum: https://answers.ros.org/question/391124/roslaunch-exit-code-2-error/

答:
The script I have added uses parses arguments defined in apriltagbase.

```<code>apriltagbase.add_arguments(parser)
options = parser.parse_args()
</code>```

argparse fails for all arguments which are not added here. So need to replace it by

```<code>options, unknown = parser.parse_known_args()
</code>```

to prevent failing due to arguments added automatically by roslaunch.

Title: roslaunch exit code 2 error

Body:
The script I have added uses parses arguments defined in apriltagbase.

apriltagbase.add_arguments(parser)
options = parser.parse_args()

argparse fails for all arguments which are not added here. So need to replace it by

options, unknown = parser.parse_known_args()

to prevent failing due to arguments added automatically by roslaunch.


问:
control of a rod attached to a motor
I have a 76 cm, aluminum rod, attached to the shaft of a 12v DC banebot motor. The rod is hanging vertically initially, and rotates in the vertical plane. The task is to control the rod to make it stop at a particular angle. I am using a quadrature encoder alongside the motors to measure the angle. Apart from this, I am also using a cytron 10A motor driver along with an Arduino UNO.

What makes this problem tricky IMO, is the fact that we have to offset the effect of gravity. Had the rod been just rotating in the horizontal plane, we simply could've controlled the angular speed to be = $kp(\theta-\theta_{ref})$, so that as $\theta$ approaches $\theta_{ref}$, the motor keeps slowing down. (Ofcourse, we could've added I and D terms too for better performance.)

But the same code will not work in the vertical plane, as we have a continuous torque due to gravity as well. The equation of the system looks something like

$$\ddot{\theta}= \dfrac{6g}{l}\sin \theta + \dfrac{\tau_m}{I}$$
$$\dot{\theta}= \int \dfrac{6g}{l}\sin \theta dt + \Omega$$

Where I think $\Omega$ is the motor's contribution of the motor output to the angular speed, which is something we can control.So if we set $\Omega$ to be something like $kp(\theta-\theta_{ref})-\int \dfrac{6g}{l}\sin \theta dt$, Then I think as $\theta$ approaches $\theta_{ref}$, $\dot{\theta}$ approaches zero. But I am not sure if that will drive $\ddot{\theta}$ to zero too...

Also, How exactly will we implement the integration step? Something simply like I+=6*g*sin(\theta)*dt/l in a loop?

答:
Wouldn't we expect vanilla PID control, once it's properly tuned, to "just work"? When the arm is at rest, constant torque due to gravity will be opposed by constant torque from the motor (or friction in the gear train). Dynamically, it's nonlinear and all, maybe difficult to tune, "tricky" as you say. But PID can always —eventually— "achieve zero steady-state error in the presence of constant disturbances", right?. When you go to maximize performance, you'll probably want to add a compensating nonlinearity that effectively tells the motor to try harder when $g \sin \theta$ is larger.

For your second question, integration: Numeric integration has a long and rich history, since the 18th century at least. On wikipedia, you could read articles on "Numerical integration", "Runge–Kutta methods", "Numerical methods for ordinary differential equations", and "Predictor–corrector method". If you go down that rabbit hole, beware: much of the historical work has been to get the most out of analytic formulations, integrating some expression that can be evaluated anywhere in the region of integration. A lot of that turns out to be irrelevant when all we have is a time series of measured error numbers with no formula in sight.

But you can probably skip all that: do a web search for "time series integral for PID". Common practice seems to be to just maintain a running total of the errors, trusting that the controller will correct for integration error just like it corrects position errors. You do have to worry about "integral windup", though (check wikipedia).

Title: control of a rod attached to a motor


问:
Spiral path generation to cover a 2D area
In my project, a robotic arm with an end effector of radius R needs to cover a 2D area in an optimal route (least time, smooth path). I am trying to achieve that by generating spiral covering routes.

I am currently a bit lost. I have a 2D area shaped arbitrarily. How can I implement the route generation algorithm to traverse it spirally (see photo)? I am kinda starting this project and I need a robotic arm to traverse this area spirally. Any suggestions would be helpful.

The spacing of the spiral turns has to be uniform and adjustable.

The generated path would be similar to that generated in CNC routing software. (Figure 2)

I tried implementing a very basic Matlab implementation but the result is coarse (See Figure 3).

答:
Here's the approach I would take. Let's start with the skeleton, then add the meat and the hair.

Suppose the outer contour has an identifiable tangent everywhere. Suppose the robot arm will travel generally clockwise, as in your sketch with the red path. It will keep its "left hand" on the border. That means that the circle center always lies on a normal to the tangent, R away from the intersection of the tangent and the normal (i.e., from the border).

The arm (that is, the center of the end effector) will travel in a path approximately parallel to the tangent, incrementally adjusting its course left or right according to how the tangent changes direction. As the center traces out its path, the "right hand" traces out a path, too. The "right hand" is diametrically opposite to the "left hand", 2R from the border. Everything between the "left hand" and the "right hand" will have been covered, or milled, or routed, or whatever.

When the arm gets back to where it started, that's one circuit, or layer. The path traversed by the "right hand" becomes the new border, and the whole thing iterates once more. It terminates when the circle is trapped, such that it would intersect the border no matter what direction it moves.

What about corners, where the tangent is undefined? I hope the border is at least piecewise continuous. If the corner is not pathological, the arm's path for one piece will intersect with the path for the next piece, so the arm just starts following the second path where the paths intersect. If it's too pathological for that work, I'll wave my hands about differential geometry or Lebesgue measure or something, and send you off to math.stackexchange.com :-)

How does it represent the "right hand" path during traversal? If you have some analytic representation for the border, like Bezier splines or something, there might be an analytic transformation or something. It would be simpler and more general, though, (but probably use more memory) to "draw" into a big bitmap, the way a paint program's brush would. I hope someone has found a better way.

How does it identify a tangent in a bitmap? It will sample a neighborhood of pixels, and locally fit a line or some other, more general curve. Even the best fit will only be an approximation, but then the bitmap itself is an approximation. The size of the neighborhood and the acceptable degree of approximation will serve to smooth the computed motion. Those smoothing parameters also give the flexibility to fix the coarse zig-zaggy right-angle "problem".

How does it get from one layer to the next? Probably the same way it got to the starting point, whatever that was.

One loop inside another is not a continuous spiral path. That's true. In fact, forbidding overlap, it probably won't even be able to finish the center. The problem statement has to relax one or more of the constraints of

uniform path spacing,

cover everything, exactly once,

don't go outside the border,

one continuous spiral.

To make a single spiral path, we could do a gradual linear interpolation from one path to the next, with pathlength/circumference or something as the parameter—like the "grooves" on a phonograph record. There are details for how to get it started and ended, but then record players actually work.


问:
Inverse kinematics for redundant robot
I'm currently doing some in-depth analysis based on this article.
Starting from a general inverse kinematics solution, the article gives an appropriate formula to compute a solution that takes into account more than one priority-base tasks.

Now, I'm currently stuck on the very first step to follow the thread: computing the inverse kinematics solution. Simulating a Kuka LWR4+, I'm doing this in MATLAB, but it seems I'm neglecting something: the solution I get for just a single task does not follow the simple trajectory I've considered.
Since I'm also using the Robotics Toolbox, I've tried to use the IK solver that comes with it, and it does give me an acceptable solution, so I know there is (at least) one. But for studying and learning purposes I do have to come up with my code.

Below is a reduced (for simplicity) snippet of my code.

```<code>clc; clear all; close all;

syms t
syms q [7 1]

%% Numerical Data [cm]
l0 = 11; l1 = 20; l2 = 20; l3 = 20; l4 = 20; l5 = 19; offset = 7.8; rho = 10;

%% Denavit-Hartemberg and Jacobian
%                a   alpha   d       theta    joint_type
denavit = @(q)([[0,  pi/2,   0,      q(1),       "R"]
                [0,  -pi/2,  0,      q(2),       "R"]
                [0,  -pi/2,  l2+l3,  q(3),       "R"]
                [0,  pi/2,   0,      q(4),       "R"]
                [0,  pi/2,   l4+l5,  q(5),       "R"]
                [0,  -pi/2,  0,      q(6),       "R"]
                [0,  0,      offset, q(7),       "R"]]);

Jac = @(q, ind)(JacobFromDH(denavit(q), ind));

J = @(q)Jac(q,0); % J4 = @(q)Jac(q, 4);

Tos = HomX(0, [0,0,l0+l1]);
ForKine = @(q) Tos*DHFKine(denavit(q));

%% Task
t_task = 0:pi/10:3*pi;

% Desired EE motion
despos = @(t) [20 - 10*sqrt(3)*sin(t), 20*cos(t), 65 + 10*sin(t),0.5*t,0,sqrt(3)/2*t];
destwist = diff(despos,t);

traj_fig = figure2('Name', 'task trajectory');
grid
hold on
for k = t_task
    app = despos(k);
    plot3(app(1),app(2),app(3),'x--','Color','blue')
end
view(45,45)

%% Inverse kinematics algorithm
qSotNorm = sot(J(q), ...
               destwist',...,
               ForKine(q), ...
               despos(t));

%% Plot solution
f_dot = figure2('Name', 'qSotNorm');
for j=[1:size(qSotNorm,1)]
    subplot(size(qSotNorm,1), 1, j)
    plot(squeeze(qSotNorm(j,end,:)))
end

%% Robot design
kuka = rigidBodyTree('Dataformat', 'column');

dhparams = [0  pi/2   0         qSotNorm(1,end,1);
            0  -pi/2  0         qSotNorm(2,end,1);
            0  -pi/2  l2+l3     qSotNorm(3,end,1);
            0  pi/2   0         qSotNorm(4,end,1);
            0  pi/2   l4+l5     qSotNorm(5,end,1);
            0  -pi/2  0         qSotNorm(6,end,1);
            0  0      offset    qSotNorm(7,end,1)];

bodyNames = {'b1','b2','b3','b4','b5', 'b6','b7'};
parentNames = {'base','b1','b2','b3','b4', 'b5','b6'};
jointNames = {'j1','j2','j3','j4','j5', 'j6','j7'};
jointTypes = {'revolute','revolute','revolute','revolute','revolute', 'revolute','revolute'};

for k = 1:7
    % Create a rigidBody object with a unique name
    kukaBodies(k) = rigidBody(bodyNames{k});
    % Create a rigidBodyJoint object and give it a unique name
    kukaBodies(k).Joint = rigidBodyJoint(jointNames{k}, jointTypes{k});
    % Use setFixedTransform to specify the body-to-body transformation using DH parameters
    setFixedTransform(kukaBodies(k).Joint, dhparams(k,:), 'dh');
    % Attach the body joint to the robot
    addBody(kuka, kukaBodies(k), parentNames{k});
end

showdetails(kuka)

%% Integration
ris = cumtrapz(0.1, qSotNorm, 3);

fris = figure2('Name', 'with integration');
for j=1:size(ris,3)
    show(kuka, ris(:,end,j))
    view(45,45)
    drawnow
    pause(0.1)
end

hold on
for k = t_task
    app = despos(k);
    plot3(app(1),app(2),app(3),'o-')
end

eu_fig = figure2('Name', 'trying euler');
[ts, qs_eu] = ForwardEuler_sot(qSotNorm, 0, zeros(7,1), 0.1);
for j=[1:size(qs_eu,2)]
    show(kuka, qs_eu(:,j))
    view(45,45)
    drawnow
    pause(0.1)
end

hold on
for k = t_task
    app = despos(k);
    plot3(app(1),app(2),app(3),'o-')
end

%% Plot solution
f_norm = figure2('Name', 'integrated values with cumtrapz');
for j=[1:size(ris,1)]
    subplot(size(ris,1), 1, j)
    plot(squeeze(ris(j,end,:)))
end
</code>```
```<code>function qsol = sot(J, xdot, FK, pos_t, kind, lambda)

    syms t
    total_time = 2*pi;
    step = 0.1;
    n_steps = ceil(total_time/step);
    n_joints = size(J, 2);
    n_tasks = size(xdot,2);
    syms q [n_joints 1]

    % +1 because the 1-index column of all pages will be qdot0(t)=0
    qdot = zeros(n_joints, n_tasks+1, n_steps);
    % Resulting in a (n_joints*1 x n_joints*1 x 1*n_tasks) matrix
    PA = repmat( eye(n_joints), 1, 1, n_tasks+1); % checked: OK to be an Identity
    % Extends twist vector to use the same index as the other matrices
    twists = cat(2,zeros(size(xdot,1),1), xdot);

    js = cat(3, zeros(size(J,1),size(J,2)),J);

    for i=[1:n_tasks] % i=2 <-> task 1
        current_task = i+1;
        prev_task = i;

        proj_prev = double(PA(:, :, prev_task)); % Projector of J_i-1

        for curr_time_step = [1:n_steps]
            timestep = curr_time_step * step;
            if curr_time_step <= 1
                prev_step = curr_time_step;
            else
                prev_step = curr_time_step-1;
            end

            % Jacobian: current task, previous timestep approximation
            jac = double(subs(js(:,:,current_task), q, qdot(:, current_task, prev_step)));

            % Desired twist: current task, current timestep
            twist_des = double(subs(twists(:, current_task), t, timestep));

            % Previous task final solution
            q_prev = qdot(:, prev_task, end);

            num_FK = double(subs(FK, q, qdot(:, current_task, prev_step)));

            posdes = double(subs(pos_t, t, timestep));
            err_pos = posdes(1:3)' - num_FK(1:3, 4);

            quat_att = MatToQuat(num_FK(1:3,1:3));
            quat_des = MatToQuat(eye(3)); % move out of this function
            n_des = quat_des(1);
            eps_des = quat_des(2:4);
            n_att = quat_att(1);
            eps_att = quat_des(2:4);
            % Can be computed via quaternions directly, too
            err_or = (n_att*eps_des' - n_des*eps_att' - Skew(eps_des)*eps_att');

            errors = [err_pos; err_or];

            k_pos = 50*eye(3);
            k_or = 50*eye(3);
            gains = [[k_pos, zeros(3,3)];
                     [zeros(3,3), k_or]];

            q_clik = pinv(jac) * (twist_des + gains * errors);
            q_app = q_clik; % so this is the starting point

            % Compute next timestep solution, for each task i
            q_next = q_app;% + pinv(jac * proj_prev) * (twist_des - jac * q_prev);

            % i-th task
            qdot(:, current_task, curr_time_step) = q_next;
        end

        proj_next = proj_prev - pinv(jac * proj_prev) * jac * proj_prev;
        PA(:, :, current_task) = proj_next;
    end

    qsol = qdot(:,2:end,:);
end
</code>```

As you can see I'm using some code to perform DH-base Forward Kinematics and Jacobian computations; I've checked and those are good.

Does someone have any suggestions, even not code-related? I'm kinda confused, like I'm missing some core stuff...

Thanks!

答:
Just to help out someone in the future, here's what I've done.

The problem as treated above is ill-approached. I was trying to numerically compute the derivative terms hoping that I could integrate them later.

Turns out, the right way to approach this problem is to build your own function that computes a derivative function and then use the ODE family to integrate.
So, back to this case, for the inverse kinematics I've coded like the following

```<code>    function qdot = clik(tt, q, J_sym, twist_sym)
        % initialization
        n = size(J_sym,2);
        qdot = zeros(n,1);

        jac = J_sym(q);                 % 6xn
        proj_jac = pinv(jac);              % nx6
        twist_des = twist_sym(tt)';     % 6x1

        % actual function: qdot = J^(-1) * xdot_des
        for i=[1:n]
            qdot(i,1) = proj_jac(i,:) * twist_des;
        end
    end
</code>```

and then used

```<code>[t_clik, y_clik] = ode15s(@(t,y) clik(t,y,J,destwist), [t0 tf], qinit);
</code>```

where destwist and J are the function handles to compute the desired twist at the end effector and the manipulator jacobian, while qinit is the initial condition.

Title: Inverse kinematics for redundant robot

Body:
Just to help out someone in the future, here's what I've done.

The problem as treated above is ill-approached. I was trying to numerically compute the derivative terms hoping that I could integrate them later.

Turns out, the right way to approach this problem is to build your own function that computes a derivative function and then use the ODE family to integrate.
So, back to this case, for the inverse kinematics I've coded like the following

    function qdot = clik(tt, q, J_sym, twist_sym)
        % initialization
        n = size(J_sym,2);
        qdot = zeros(n,1);

        jac = J_sym(q);                 % 6xn
        proj_jac = pinv(jac);              % nx6
        twist_des = twist_sym(tt)';     % 6x1

        % actual function: qdot = J^(-1) * xdot_des
        for i=[1:n]
            qdot(i,1) = proj_jac(i,:) * twist_des;
        end
    end

and then used

[t_clik, y_clik] = ode15s(@(t,y) clik(t,y,J,destwist), [t0 tf], qinit);

where destwist and J are the function handles to compute the desired twist at the end effector and the manipulator jacobian, while qinit is the initial condition.


答:
The control of a pendulum subject to gravity is a quite standard control problem to which you may apply different techniques such as PID or LQR.

Unless you perform current control, you usually don't have direct access to the torque $\tau_m$ generated by the motor. A more common setting foresees voltage control instead. In this context, the dynamic equation you have to address is slightly more complex as it needs to comprise the electromechanical behavior of the motor per se.

That said, from a control standpoint, it's essential to understand what is your control variable (CV), which is the output of your controller, and what is the process variable (PV), which is the variable you want to control such that it will eventually assume/track the desired setpoint value (SP).

Here, it's clearly:

CV = motor input voltage $u$

PV = angular position of the rod $\theta$

SP = the desired position $\theta_d$

Your PID is, therefore:
$$
\begin{equation}
e = \theta_d - \theta \\
u = K_p \cdot e + K_d \cdot \dot{e} + K_i \cdot \int{e}
\end{equation}.
$$

The integral part is fundamental in this scenario. In fact, assume that $K_i = 0$ and that you're already on the setpoint in a stationary condition. It follows that $e = 0$ and $\dot{e} = 0$; thus, $u = 0$, meaning you're not delivering voltage to the motor at all and the rod will start falling because of gravity. To sum up, the integral part is the only contribution capable of compensating for the effect of gravity as it can deliver non-null output when the input $\left(e,\dot{e}\right)$ is zero thanks to its internal reservoir.

In theory, you could also provide a feed-forward term $u_{FF}$ to better help the controller:
$$
u = K_p \cdot e + K_d \cdot \dot{e} + K_i \cdot \int{e} + u_{FF}.
$$

The term $u_{FF}$ can be calculated starting from the gravity and then reflecting it to the side of motor voltage. The calculation is often imprecise; that's why an integral part is always required.

Integration

Usually, we make use of the Tustin method for implementing the integral part in a discrete system:

$$
y_k = y_{k-1} + T_s \cdot \frac{e_k + e_{k-1}}{2}.
$$

The output $y$ of the integral at the instant $k$ is a function of two subsequent samples of the input signal $e$, whereas $T_s$ is the sample time.

The Tustin (a.k.a. trapezoidal) method provides smoother outcomes with respect to the more common forward or backward Euler formulas.

A somewhat related Q&A: https://robotics.stackexchange.com/a/19658/6941.

Title: control of a rod attached to a motor

Body:
I have a 76 cm, aluminum rod, attached to the shaft of a 12v DC banebot motor. The rod is hanging vertically initially, and rotates in the vertical plane. The task is to control the rod to make it stop at a particular angle. I am using a quadrature encoder alongside the motors to measure the angle. Apart from this, I am also using a cytron 10A motor driver along with an Arduino UNO.

What makes this problem tricky IMO, is the fact that we have to offset the effect of gravity. Had the rod been just rotating in the horizontal plane, we simply could've controlled the angular speed to be = $kp(\theta-\theta_{ref})$, so that as $\theta$ approaches $\theta_{ref}$, the motor keeps slowing down. (Ofcourse, we could've added I and D terms too for better performance.)

But the same code will not work in the vertical plane, as we have a continuous torque due to gravity as well. The equation of the system looks something like

$$\ddot{\theta}= \dfrac{6g}{l}\sin \theta + \dfrac{\tau_m}{I}$$
$$\dot{\theta}= \int \dfrac{6g}{l}\sin \theta dt + \Omega$$

Where I think $\Omega$ is the motor's contribution of the motor output to the angular speed, which is something we can control.So if we set $\Omega$ to be something like $kp(\theta-\theta_{ref})-\int \dfrac{6g}{l}\sin \theta dt$, Then I think as $\theta$ approaches $\theta_{ref}$, $\dot{\theta}$ approaches zero. But I am not sure if that will drive $\ddot{\theta}$ to zero too...

Also, How exactly will we implement the integration step? Something simply like I+=6*g*sin(\theta)*dt/l in a loop?


问:
How to derive the inverse kinematic equations for this simple 6-DOF robot?
For the simple 6-DOF shown, we have to solve for the inverse kinematic equations and its resulting figures.

But I'm sorry, I really don't have any idea where to begin. Can someone please help?

What should I do to solve this?

P.s. That's all the information our teacher gave us.

答:
You would first solve for the center of the wrist.
There are examples on this website and textbooks how to solve for the center of the wrist. The wrist 3d pose ignores the last joint value because you are traveling along the Z rotation axis of the EE.
https://ieeexplore.ieee.org/ielx7/6287639/6514899/08630999.pdf

You can then solve for the joint value of theta 1 since it points the arm to the wrist center. (you should be able to figure this out).

Then, once you have t1 and the wrist center, you run into an issue.

It should be observed that you have 3 joints all on the same plane. (2,3,4) This means that there is a kinematic redundancy in this robot arm. I suggest you create your own solver to handle the planar kinematic redundancy. (The stretching out of the wrist). There are many ways you can do this, so you will just have to experiment.
I suggest starting with the simplest design possible.

This solver would have to take into account the magnitude distance from the 3d location of the center of joint 2 all the way out to the 3d position of joint 5 (wrist center).
This will allow you to solve joints 2,3,4.
This can be done by assuming the last link points directly to the wrist center (from joint 2). You can then solve for the other 2 joints (both solutions) through the inverse of the cosine law.

Then, once you have the joints 1,2,3,4 solved, you need to solve for t5 and t6.
If you do the FK given t1-t4, you know the 3d position of joint 4. Since you know the 3d position of joint 4,5,6, you can then solve for t5 using the 3d triangle shape. (sss form) Then, once you have t1-5, you can solve for t6 by subbing "0" into t6 with t1-5 and then doing an atan2() call.

This may not make a lot of sense, but you will have to do some research anyways.


问:
Kinematic Bicycle Model theta units
I don't understand the $\theta$ units. The bicycle model defines $\theta$ as:
$$\theta=\theta +\frac{v\cdot\tan\delta}{L}\cdot$$

Where $v$ is the vehicle velocity, $L$ is the wheelbase distance, and $\delta$ is the desired steering angle.

Say I'm driving at $30m/s$ in a vehicle with $L=2.3$, want to steer $50\deg$, and the $\Delta t=0.1$, then
$$\theta=0+\frac{30\cdot\tan50\deg}{2.3}\cdot0.1=1.554$$

But what are the units of $\theta$? Radians or Degrees?

Since we need $\theta$ to calculate $x$ for example, with $x=x+v\cos\theta$, when $\theta$ in degrees $x=30\cos 1.554\deg=29.9889$ (slightly changed) or $x=30\cos 1.554\text{ rad}=0.503$ (significantly changed)

The $\theta$ unit is very important but I can't find any reference for it. I guess it should be degrees. Am I right?

答:
No, $\theta$ has to be in radians. If you follow through the analysis in, say, this guy, which was the first hit in a web search for "bicycle model kinematics", you see that $\omega = \dot\theta$ is the rate of rotation of the reference point (rear axle, in his section 2.1) around the instantaneous center of rotation, with radius R. Since he uses $\omega = v/R$, it's clear that it all has to be radians.

BTW, if I were peddling my bicycle with my $v$ exceeding the automobile speed limit on most roads where I would feel safe riding a bike, I might not be steering that hard. (It might be interesting to compute how many g's the centripetal acceleration is, at $30m/s$ and $\delta=50^\circ$). For your numeric sanity check, maybe try something more sedate like $v=4 m/s$ and $\delta = 2^\circ$.

Title: Kinematic Bicycle Model theta units


问:
Inverse kinematic solution with constraints on Rotation for Homogeneous Matrix
I have a 4 degree of freedom robot arm with 3 revolute joints and one prismatic joint as shown in the image:

I can give just Target Point position to the Inverse kinematic chain and find a solution for the 4 DOF robot. Which moves 4 joints to touch the Target Point (Red Dot). However I also want certain condition for the rotation of the last Joint. I don't want to find a solution where the orientation of the last joint rotate along Z axis. In other words orientation of the end effector is important for me.

I am given a H.M to the inverse kinematic chain for the robot show above. My H.M is something like this (Target_x, Target_y, Target_z represents the red dot position):

```<code>[X X X Target_x]
[X X X Target_y]
[X X X Target_z]
[0 0 0 1       ]
</code>```

As you can see Rotation in H.M, not set (I wrote Xs) and solution is found only for the position. But I would like to set a orientation, so that end-effector would not rotate over its own Z axis. In other words rotation in Yaw should be zero. What kind of a rotation matrix should I set inside the H.M. to achieve this?

```<code>                  [X_? X_? X_?]
   Rotation =     [X_? X_? X_?]
                  [X_? X_? X_?]
</code>```

P.S: I am using IKPY inverse kinematic python library which allows me to find inverse kinematic solution by Position, Orientation, or Position and Orientation together. I don't think it is directly related with the question it self, but I wanted to point out.

答:
Honestly, I would just treat this arm as a 2-dof arm whose end effector approach angle and distance from desired 2d point lives on a circle with a radius that matches the length of the last link (the prismatic joint), and not actually on the EE tip.
Then, the approach angle is calculated given the fact that the location of joint 3 and the EE tip are known.

In other words, assume the length of the prismatic joint is "0" and solve the arm.

This isn't a super difficult arm to solve IK for, it is just that there are 4 parameters that you can play with on a planar arm, but there are a maximum of 3 dof to describer the EE tip frame. You have to apply an extra constraint somewhere, and I would do that at the prismatic joint. I say that because the longer the link, the more torque exerted on the joints across the entire arm.

Does this make sense?


问:
Power Model for humanoids
I am in the process of creating a Power Prediction Model for the Hubo Robot.
The Robot has 38 Degrees of Freedom and has a computer some sensors and motor boards. The motors are powered through Motor Boards. All these boards are powered through a main power board that exists at the robots chest.

My model should be able to predict the power for any trajectory of the robot. Say for instance if the robot raises its hand from 0 degrees to 180 degrees my model should be able to predict the power.

Heres an idea I came across. My idea was to equate the electrical torque to the mechanical torque of each joint.

For instance if the Right arm pitch moves from 0 to 180 degrees I can do as follows ?
$mgsin(\theta)= Kt*I$

However, I am not getting a proper prediction and the current value is way off than what we can read from a software installed in the robot. I know there are losses but even then its off. I was wondering if there are any other approaches or a fault in my approach.

And after I do this I can add all the joint currents for a specific trajectory and then give a estimate for total power consumption.

答: What you want to do is figure out what are the motor torque requirements to move the robot in the desired trajectory. The torques required will then translate to current draw required by motors. I suggest you look at the jacobian matrix of each serial arm to calculate from the Force+Torque of the tip into the motor torques at each joint. This is a humanoid, however, so you will have to be careful how you define the jacobian matrices of the legs and the arms.</p>

ContentLicense: CC BY-SA 3.0

Title: Power Model for humanoids


问:
Do electromagnets affect magnetometer calibration?
I am designing a landing pad for a drone that will have electromagnets built into the pad. My drone has a pixhawk 2 with PX4. I'm worried that the electromagnets in the pad will throw off the drone's IMU/magnetometer calibrations and affect it during flight. Does anyone have any insight into if magnets can have a delayed effect on a magnetometer/IMU like this? I obviously won't be flying while on the pad, so am only worried about its calibrations once in flight. Thanks!</p>

答:
Without knowing the specifics of the sensitivity of your magnetometer and electromagnet, it is impossible to give a concrete yes or no answer.

With that, in general any source of magnetic field is likely to cause some disturbance in a magnetometer measurement.  This includes internal sources, such as the electronics and motors on the drone, or external sources, such as any magnets in the environment.

The calibration for a magnetometer can really only account for the internal disturbance sources and cannot account for any time-varying aspects of the internal or external disturbances.

My advice would be to perform your magnetometer calibration with it installed on the drone, but away from the platform.  This calibration should remain correct until something on the drone is changed.

You may see disturbances when the drone is on the platform, but there should be no need to re-calibrate in this case.


答:
To compare trajectories a metric is usually the energy.  Indeed this is the integral of the power over the trajectory.

There is usually two parts to compute it.

The first one is to consider the full model of the robot that is provided by the corresponding URDF file. Using the Recursive Newton-Euler algorithm that you can find on the Pinocchio library or RBDL, you can compute the torque $\tau$ for each joint for a given trajectory. The input are the joint position, the joint speed and the joint acceleration. Once you have the torque you can compute the mechanical energy.

The second part implies that you know some specific information from your motors and your power electronics. You need the motor resistances and the electric motor torque constant to approximate the electric energy. The energy used by the power electronics can be either neglected or computed from the current that has to be delivered to the motor.

You can find a paper on benchmarking here and with a specific example on a humanoid robot more here.

Title: Power Model for humanoids

Body:
I am in the process of creating a Power Prediction Model for the Hubo Robot.
The Robot has 38 Degrees of Freedom and has a computer some sensors and motor boards. The motors are powered through Motor Boards. All these boards are powered through a main power board that exists at the robots chest.

My model should be able to predict the power for any trajectory of the robot. Say for instance if the robot raises its hand from 0 degrees to 180 degrees my model should be able to predict the power.

Heres an idea I came across. My idea was to equate the electrical torque to the mechanical torque of each joint.

For instance if the Right arm pitch moves from 0 to 180 degrees I can do as follows ?
$mgsin(\theta)= Kt*I$

However, I am not getting a proper prediction and the current value is way off than what we can read from a software installed in the robot. I know there are losses but even then its off. I was wondering if there are any other approaches or a fault in my approach.

And after I do this I can add all the joint currents for a specific trajectory and then give a estimate for total power consumption.


问:
How secure is ROS2?
How secure is the data transmission between nodes? is safer to use in industrial applications?

I need to replace the RS485/modbus communication in an automobile industry

答:
Rather than trying to explain the nuances of ROS 2 security, I think its better to mention that ROS 2 uses the DDS standard for its security paradigm. If you're working on something important like automotive, you should probably look into the actual specification itself, DDS Security (https://www.omg.org/spec/DDS-SECURITY/1.1/About-DDS-SECURITY/).

It includes the big things you think of like encryption, authentication, access control, and logging. Whether the provided DDS security is right for your application is largely up to your specific requirements and industry norms, but DDS does provide good security for what its meant for.

Title: How secure is ROS2?


问:
Underwater Glider Obstacle Avoidance simulator
I am currently taking part in a project where a team is building an autonomous underwater glider with obstacle avoidance features.

However, since I do not have a background in Robotics (I am in Computer Science), I am having a very hard time knowing what to do.

So far, I have read multiple papers on obstacle avoidance algorithms, physics simulators, and underwater glider simulators but I am frozen in panic at the amount of information lying in front of me. There are so many simulators and I have no idea which one to learn, no idea how to use them, no idea how to personalize and configure them to my need.

My question is: I want to be able to do these things:

be able to modify the shape of the underwater glider. The hydrodynamics should depend on the shape of the underwater.

be able to simulate inputs from the sensors: I will be using single beam forward-looking sonars for obstacle avoidance. The point of the simulation is to allow me to test different configurations or arrangements of sonars.

be able to simulate actuators, so propellers, wings, etc.

parameterize some properties of the water and the environment, so gravity or water turbidity

be able to write navigation and obstacle avoidance policy in a programming language such as Python or C++

render the whole scene in acceptable fidelity

What would be an approach chosen by an expert? I am currently tempted to write my simulator from scratch as this would be the most straightforward way but I would be wasting a lot of time.

答:
A lot of your question is written as a shopping question which is not a good match for this site. We want questions which are practical and can have a concrete answer. As opposed to open ended questions which are focused on opinion and subjective.

However to that end I'll answer your more generic but concrete question at the end. How would an expert approach this?

This problem requires a classic comparative analysis. You should identify what you potential options and enumerate them. You then want to identify your requirements. You can then evaluate each option against you requirements. You can estimate the levels of effort to bring anything not meeting your requirements up to that level for each option.

As an aside to your thoughts about just building your own. Do not underestimate the level of work required. Depending on what level of fidelity you want in your system this could be a multi year project where you need consultation with people whose specialty is modeling and controlling underwater vehicles. This is from actively participating on a project with MBARI doing most of the things you're listing.

This is one of a large community active open source projects in this area. There was a recent birds of a feather discussion about maritime robotics which might help you see a sampling of what people are currently doing in the space and what you might be able build off of.


问:
gazebo world with roof
Does anybody know of any readily accessible gazebo worlds with a roof?

I have several environments without a roof, and one with a roof (that is too high up for the required purpose), i could do with another. The first floor of any building will do just fine.

Background - I'm testing a 3D mapping package, with a velodyne LiDAR. In the real world the roof of an environment gets compacted down as an obstacle, and I need an environment to tune the parameters to prevent this happening / check that that doesn't happen with this setup.

Any help would be greatly appreciated.

答:
There are several world files with enclosed spaces in the Ignition Fuel website:

https://app.ignitionrobotics.org/fuel/worlds

The Edifice Demo world is indoors, though perhaps the roof is too high

https://app.ignitionrobotics.org/OpenRobotics/fuel/worlds/Edifice%20demo

Just about all of the sub-T worlds are in tunnels, which have a roof.

https://app.ignitionrobotics.org/OpenRobotics/worlds/Finals%20Systems%20Preliminary%20Round%202

https://app.ignitionrobotics.org/OpenRobotics/fuel/worlds/Finals%20Prize%20Round%20World%2004

Title: gazebo world with roof


问:
Where to start for the software side of Robotics?
I am a Computer Science student entering my last year of college. I'm pretty sure Robotics is what I want to eventually be doing based on my interests in AI and embedded systems. I've seen a lot of topics that covers Robotics such as: control theory, signal processing, kinematics, dynamics, 3D simulators, physics engines, AI, Big Data with machine learning. I'm hoping someone can point me in the right direction as to what I should be attempting to study in my interests of Robotics. I am not sure what other topics I have not mentioned that would be relevant. I would like to deal with the software side of Robotics, both AI and none AI.

My other question is about machine learning. I've seen researchers applying machine learning (deep learning/unsupervised learning specifically) to robotics but how do they do this? Is information and data transferred from the internals of the robot to an external computer that does the data processing? Machine learning requires a lot of data to predict. Is this the only way machine learning can be used in robotics (through an external computer)?

I hope someone can touch on some of the things I've mentioned, Thank you.

答:
To answer your first question: if you are looking merely into the software side of robotics, you might consider what aspect of software interests you the most considering the list you posted. One thing to remember in this regard when making a choice is to think on what platforms you could test those hypotheses you would generate while studying and how to set up an experimental verification. Generally speaking, a C language is the most common.

On your second question: machine learning is a vast subject. Personally, the global aspect of learning--leveraging, say, neural networks--is a good experience in itself. Understanding the state-of-the-art in NN programming, as well as following examples is a great start to both your questions.

If you have the interest to program, I would suggest keying into Google "neural networks c#" and visiting CodeProject for examples. C# has the shortest learning curve, I think, and has the most to offer you simply of the volume of people using it.


问:
How to find the nonlinear equations of motion of a seesaw-cart electromechanical system?
So what I have here is a single pendulum with a cart system. The pendulum is composed of a set of rigidly connected parts integrated in a seesaw-like structure which can freely pivot around the axis on its bottom. The weight attached to the vertical rod of the pendulum is an additional element to the pendulum.

What I need to do is to derive equations of motion and state-space representation of this system and put it all into MATLAB to model this system. I am just getting to learn Newton's and Langrange's method and I have no way of knowing if my solution would remotely be correct.

The two figure below represent the described model, with the second being a more simplified model of the seesaw model.

The system's constant parameters necessary to model the dynamics of the system are as follows:

a height of center of mass of the pendulum, excluding the weight B (i.e., point A in the figure = center of mass of the light gray
shaded parts),

b position of the weight B on the vertical rod )the weight is modelled a point concentrated mass at point B),

d height of interconnection points of ropes which connect multiple pendulums (this is irrelevant to the given problem since I want to
model the system for a single pendulum),

c height of the center of mass of the cart (point C in the figure),

h height of the rack for cart propulsion,

J moment of inertia of the pendulum, excluding the weight B,

m1 mass of the pendulum, excluding the weight B,

m2 mass of the weight in point B,

m3 mass of the cart.

The cart is driven by a DC motor.

Any help with this I would greatly appreciate.

答: Took some time but this right here might be the solution to the problem. I did this by hand so I hope the handwriting is not a problem. Thanks to Chuck for the help! :)


问:
What is Wrong With My Stanley Controller for Car Steering Control
Hope everyone is doing well.

I am just trying out Microsoft Airsim for few days and few hours ago I wanted to implement a Path Tracking Algorithm for an Autonomous Car. For the lateral control, I wanted to use Stanley Controller. Here is the basic idea of the Stanley Controller:

Here is the Stanley Controller part of my script:

```<code>##############################
#  LATERAL POSITION CONTROL  #
##############################

# STANLEY GAIN:
K = 0.78

# READING CAR POSITION AND ORIENTATION FROM THE SIMULATOR:
pos = car_state.kinematics_estimated.position
orientation = car_state.kinematics_estimated.orientation

x = pos.x_val
y = pos.y_val
yaw = orientation.z_val

if v < 0.5 and v > -0.5:
    index += 1
else:
    arr = np.zeros(ref.shape[0], dtype=np.float32)

    for i in range(ref.shape[0]):
        arr[i] = np.sqrt((x - ref[i][0])**2 + (y - ref[i][1])**2)
    index = np.argmin(arr)

#print(index)

# I APPROXIMATED HERE, NOT SURE IF IT IS REALLY CORRECT:
xc = x + (1.5 * np.cos(yaw))
yc = y + (1.5 * np.sin(yaw))

# SLOPE OF THE LINE CONTAINING TWO CONSECUTIVE WAYPOINTS:
ref_path_x = ref[index + 1][0] - ref[index][0]
ref_path_y = ref[index + 1][1] - ref[index][1]

# TO AVOID ZERO BY ZERO DIVISION:
if ref_path_x == 0:
    ref_path_x = 0.000001

slope = ref_path_y / ref_path_x

# X INSTERSECTION OF THE LINE:
# Constant = n:

constant = ref[index + 1][1] - (slope * ref[index + 1][0])

# CONTANTS OF THE LINE EQUATION:
a = -1 * slope
b = 1
c = -1 * constant

# CROSS TRACK ERROR CALCULATION (AS MENTIONED ON THE SCREENSHOT):

e_cross = ((a * xc) + (b * yc) + c) / np.sqrt(a**2 + b**2)

# HEADING ERROR CALCULATION (AS MENTIONED ON THE SCREENSHOT):
e_heading = np.arctan2(-a, b) - yaw

#e_heading = (e_heading + np.pi) % (2 * np.pi) - np.pi

# if e_heading > np.pi:
#     e_heading -= 2 * np.pi
# if e_heading < - np.pi:
#     e_heading += 2 * np.pi
# if e_heading > 0:
#     e_cross = abs(e_cross)
# else:
#     e_cross = - abs(e_cross)

# CROSS-TRACK STEERING (AS MENTIONED ON THE SCREENSHOT):
steer_cross = np.arctan(K * e_cross / v + 0.000001)

# TOTAL STEERING (AS MENTIONED ON THE SCREENSHOT):
steering_out = e_heading + steer_cross

#steering_out = (steering_out + np.pi) % (2 * np.pi) - np.pi
# if steering_out > np.pi:
#     steering_out -= 2 * np.pi
# if steering_out < - np.pi:
#     steering_out += 2 * np.pi
</code>```

However, when I run the simulation, the car starts normally but then towards the turn (before the reference steering), it steers around 20 - 30 degrees abnormally, causing collision. I tried different gains but received the same result. I suspect that there is a mistake in my code. Can you please direct me towards a solution?

答:
While matching up your code against the Stanford paper I did some rewriting, which I pasted below. The handling of ref and arr were pretty confusing, so I introduced somewhat more transparent terms. Also I tried to follow the paper's nomenclature although I chose names for some anonymous quantities.

It's generally a "code smell" when you find yourself worrying about dividing by zero when it's just an artifact of the coordinate system that something has infinite slope. Like if you could rotate everything by $45^o$ the problem would go away (or move somewhere else!) A related issue is doing atan of a quotient, which is better done with atan2.

```<code># from elsewhere
v = <speed of car>
k = <tracking control coefficient>

# location of car
xc, yc = <location of car>

# orientation of car aka yaw
theta_c = <orientation of car>

# previous and next waypoints
x1, y1 = <previous waypoint>
x2, y2 = <next waypoint>

# cross track error: distance of car xc,yc from the line defined
# by the two waypoints.
# https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
e_numerator = (x2-x1) * (y1-yc) - (x1-xc) * (y2-y1)
e_denominator = sqrt((x2-x1)**2 + (y2-y1)**2)
e = e_numerator / e_denominator

# direction of correct track
theta_track = atan2(y2-y1, x2-x1)

# heading error
psi = theta_track - theta_c

# cross track steering correction
theta_xc = atan2(v, k * e)

# steering
delta = psi + theta_xc
</code>```

Body:
While matching up your code against the Stanford paper I did some rewriting, which I pasted below. The handling of ref and arr were pretty confusing, so I introduced somewhat more transparent terms. Also I tried to follow the paper's nomenclature although I chose names for some anonymous quantities.

It's generally a "code smell" when you find yourself worrying about dividing by zero when it's just an artifact of the coordinate system that something has infinite slope. Like if you could rotate everything by $45^o$ the problem would go away (or move somewhere else!) A related issue is doing atan of a quotient, which is better done with atan2.

# from elsewhere
v = <speed of car>
k = <tracking control coefficient>

# location of car
xc, yc = <location of car>

# orientation of car aka yaw
theta_c = <orientation of car>

# previous and next waypoints
x1, y1 = <previous waypoint>
x2, y2 = <next waypoint>

# cross track error: distance of car xc,yc from the line defined
# by the two waypoints.
# https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
e_numerator = (x2-x1) * (y1-yc) - (x1-xc) * (y2-y1)
e_denominator = sqrt((x2-x1)**2 + (y2-y1)**2)
e = e_numerator / e_denominator

# direction of correct track
theta_track = atan2(y2-y1, x2-x1)

# heading error
psi = theta_track - theta_c

# cross track steering correction
theta_xc = atan2(v, k * e)

# steering
delta = psi + theta_xc


问:
Difference between motion-only bundle adjustment and pose-graph optimization
I've been trying to figure out the difference between motion-only bundle adjustment and pose-graph optimization, when talking about systems that only use cameras to estimate motion between frames, but have so far had no luck finding a solid explanation explaining the differences between these two concepts.

Motion-only BA optimizes over camera poses, and treats triangulated landmark positions as being constant, where the error function is the reprojection error between triangulated 3D landmarks and their corresponding 2D feature points on the camera/image plane (e.g. a nice explanation of motion-only BA can be found on page 4 of the ORB-SLAM2 paper).

However, pose-graph optimization seems extremely similar, where again camera poses are optimized and triangulated landmark positions are treated as being constant during optimization. But, instead of having a reprojection-based error function, the error function for pose-graph optimization instead seems to be the difference between relative poses (e.g. a nice explanation of pose-graph optimization can be found on page 9 of the tutorial paper on visual odometry by Fraundorfer and Scaramuzza).

However, for vision-based systems it seems like the difference between relative poses has to be obtained using some type of reprojection-based error function (assuming the use of sparse features, and not dense tracking). As such, are motion-only BA and pose-graph optimization two names for the same thing, or are there more nuanced differences between the two?

答:
From the Appendix section of the ORB SLAM paper:

... In pose optimization, or motion-only BA, (see section V) all points are fixed
and only the camera pose is optimized.

So yes, they are the same, I assume it's only the context that makes the use of one term preferable over the other.


问:
"modern contact dynamics" - Why are spring-damper contact models not being used anymore for dynamic simulations in robotics?
While researching different physics engines for dynamic simulations in robotics I found the following statement on the MoJoCo documentation website:

Physics engines have traditionally separated in two categories. Robotics and biomechanics engines (MATLAB Robotics Toolbox, SD/FAST, OpenSim) use efficient and accurate recursive algorithms in generalized or joint coordinates. However they either leave out contact dynamics, or rely on the earlier spring-damper approach which has fallen out of favor for good reason. Gaming engines (ODE, Bullet, PhysX, Havoc) use the modern approach where contact forces are found by solving an optimization problem at each time step.

Link: http://www.mujoco.org/book/index.html

What is the "good reason" for spring-damper not commonly being used anymore to model contacts in dynamic simulations?

答:
For the record, here is the answer from the author himself in one of his articles:

The difficulty with traditional spring-damper models is that, if the contact-space inertia is too large the contact will be springy and result in large penetration and subsequent oscillation, while if the inertia is too small the dynamics will be stiff and very difficult to integrate numerically. Since the contact-space inertia is configuration-dependent while the spring-damper coefficients are fixed, this problem may seem unavoidable. However, if we have access to the diagonal of the ? matrix we can tune the spring-dampers online, and for example make sure that we always have critically-damped springs at all contacts.

From MuJoCo: A physics engine for model-based control


问:
Quadcopter PID tuning
In continuation of the question I asked here: Quadcopter instability with simple takeoff in autonomous mode
  ...I'd like to ask a few questions about implementing a basic PID for a quadrotor controlled by an APM 2.6 module. (I'm using a frame from 3DRobotics)

I've stripped down the entire control system to just two PID blocks, one for controlling roll and another for controlling pitch (yaw and everything else... I'd think about them later).

I'm testing this setup on a rig which consists of a freely rotating beam, wherein I've tied down two of the arms of the quadrotor. The other two are free to move. So, I'm actually testing one degree of freedom (roll or pitch) at a time.

Check the image below: here A, B marks the freely rotating beam on which the setup is mounted.

With careful tuning of P and D parameters, I've managed to attain a sustained flight of about 30 seconds.

But by 'sustained', I simple mean a test where the drone ain't toppling over to one side. Rock steady flight is still no where in sight, and more than 30 secs of flight also looks quite difficult. It wobbles from the beginning. By the time it reaches 20 - 25 seconds, it starts tilting to one side. Within 30 secs, it has tilted to one side by an unacceptable margin. Soon enough, I find it resting upside down

As for the PID code itself, I'm calculating the proportional error from a 'complimentary filter' of gyro + accelerometer data. The integral term is set to zero. The P term comes to about 0.39 and the D term is at 0.0012. (I'm not using the Arduino PID library on purpose, just want to get one of my own PIDs implemented here.)

Check this video, if you want to see how it works.

http://www.youtube.com/watch?v=LpsNBL8ydBA&feature=youtu.be
[Yeh, the setup is pretty ancient! I agree. :)]

Please let me know what could I possibly do to improve stability at this stage.

@Ian: Of the many tests I did with my setup, I did plot graphs for some of the tests using the reading from the serial monitor. Here is a sample reading of Roll vs 'Motor1 & Motor2 - PWM input' (the two motors controlling the roll):

As for the input/output:

Input: Roll and pitch values (in degrees), as obtained by a combination of accelerometer + gyro

Output: PWM values for the motors, delivered using the Servo library's motor.write() function

Resolution

I resolved the problem. Here's how:

The crux of the issue lied in the way I implemented the Arduino program. I was using the write() function to update the servo angles, which happens to accept only integer steps in the argument (or somehow responds only to integer input, 100 and 100.2 produces the same result). I changed it to writeMicroseconds() and that made the copter considerably steadier.

I was adding up RPM on one motor while keeping the other at a steady value. I changed this to increase RPM in one motor while decreasing the opposing motor. That kinda keeps the total horizontal thrust unchanged, which might help me when I'm trying to get vertical altitude hold on this thing.

I was pushing up the RPM to the max limit, which is why the quadcopter kept losing control at full throttle. There was no room for the RPM to increase when it sensed a tilt.

I observed that one of the motor was inherently weaker than the other one, I do not know why. I hardcoded an offset into that motors PWM input.

Thanks for all the support.

Source Code:

If you're interested, here's the source code of my bare-bones PID implementation: PID Source Code

Please feel free to test it in your hardware. Any contributions to the project would be welcome.

答:
I'd start by reading over this question: What are good strategies for tuning PID loops?

If I had to guess, I'd say you have a problem in the way your complimentary filter is constructed.  With the quadcopter motors off, you should tilt the frame back and forth and see if the roll / pitch values that are reported are actually accurate.

To me, it looks like there is a delay between the input from the accelerometer and the output of your filter -- the oscillation could be explained by late reactions to the input data.  The eventual turning over looks like a possible integration error that accumulates over time -- in other words, while your quadcopter is on its side it actually thinks it's hovering levelly.

(Update) Regarding your graph, the fact that your motor speeds continue to increase (instead of staying balanced) means you have an error somewhere.  Perhaps your Integral term is growing unbounded, and you should specify a reasonable maximum for it.

Title: Quadcopter PID tuning


问:
DH Parameters of a 2 DOF RP Robot

How can I determine the DH parameters of the robot shown in the figure ? As i know, the z axis should show the rotation axis for rotary joints and direction of extension for prismatic joints, so the x0 axis and z1 axis overlap. When I try to obtain a robot model with DH parameters I obtain the following model on MATLAB. I don't know how can I obtain model where the prismatic joint extends on the x0 axis.

答:
Using the DH convention in Robot Modeling and Control by Spong, the DH table is

$$
T_2^0 =
\begin{bmatrix}
c_{\theta_1} & 0 & -s_{\theta_1} & 0 \\
s_{\theta_1} & 0 & c_{\theta_1} & 0 \\
0 & -1 & 0 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 1+d_1 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Title: DH Parameters of a 2 DOF RP Robot


问:
How to fix 'no module named actionlib_tutorials.msg error when going through the actionlib ros tutorials
I'm going through the actionlib tutorials and I'm getting this error with the example python script:

```<code>Traceback (most recent call last):
File "/home/leonard/catkin_ws/src/actionlib_tutorials/simple_action_servers/fibonacci_server.py", line 7, in <module>
import actionlib_tutorials.msg
ImportError: No module named actionlib_tutorials.msg
</code>```

This is very confusing because actionlib_tutorials does have an 'msg' file.

I am using ros melodic in singularity

答: Make sure you have followed the section on creating the action messages in the C++ tutorial before doing the Python tutorial. You especially need to do the catkin_make step at the end of that section or the Python module for the messages won't be created.


问:
Invalid <param> tag: Cannot load command parameter [robot_description]:
I am using the waypoint_nav package. When I launched the file named outdoor_waypoint_nav_sim.launch, I got the following error:</p>
```<code>RLException: while processing /home/user/catkin_ws/src/waypoint_nav/outdoor_waypoint_nav/launch/include/gazebo.launch:
while processing /home/user/catkin_ws/src/waypoint_nav/husky_simulator/husky_gazebo/launch/husky_empty_world.launch:
while processing /home/user/catkin_ws/src/waypoint_nav/husky_simulator/husky_gazebo/launch/spawn_husky.launch:
Invalid <param> tag: Cannot load command parameter [robot_description]: no such command [['/opt/ros/noetic/share/xacro/xacro.py', '/home/user/catkin_ws/src/waypoint_nav/husky_customization/husky_custom_gazebo/urdf/custom_description.gazebo.xacro', 'laser_enabled:=true', 'ur5_enabled:=false', 'kinect_enabled:=false']].

Param xml is <param name="robot_description" command="<span class="math-container">$(find xacro)/xacro.py '$</span>(arg husky_gazebo_description)'     laser_enabled:=<span class="math-container">$(arg laser_enabled)     ur5_enabled:=$</span>(arg ur5_enabled)     kinect_enabled:=$(arg kinect_enabled)     "/>
The traceback for the exception was written to the log file
</code>```

答:
It looks like the robot description is not getting specified. Looking at the error, it's complaining when this attempts to execute:

https://github.com/nickcharron/waypoint_nav/blob/4c8bb70bd376e776c821fc659f492c55b89a342b/husky_simulator/husky_gazebo/launch/spawn_husky.launch#L35

I'm assuming it's because husky_gazebo_description is not specified. It's expecting that to be set in an environment variable called HUSKY_GAZEBO_DESCRIPTION:

https://github.com/nickcharron/waypoint_nav/blob/4c8bb70bd376e776c821fc659f492c55b89a342b/husky_simulator/husky_gazebo/launch/spawn_husky.launch#L32

There are two env hooks that seem to set that variable (used GH search):

https://github.com/nickcharron/waypoint_nav/blob/4c8bb70bd376e776c821fc659f492c55b89a342b/husky_simulator/husky_gazebo/env-hooks/50.husky_gazebo.sh
https://github.com/nickcharron/waypoint_nav/blob/4c8bb70bd376e776c821fc659f492c55b89a342b/husky_customization/husky_custom_gazebo/env-hooks/60.husky_custom_gazebo.sh

So then without knowing more, I am wondering if you perhaps just didn't build your workspace? Or perhaps you didn't source the workspace once it was built? I also note that you are using ROS noetic; perhaps the package hasn't been updated to support it yet. In any case, the package author suggests that you can e-mail him directly in the README.


问:
How much accuracy could I get position tracking with a 3-axis accelerometer and gyro sensor, and compass, and how would I do it?
Given a 12' x 12' field (4m x 4m), a reasonably cheap 3-axis gyro sensor and accelerometer, and compass, I plan to design a device capable of tracking its position to sub-centimeter accuracy for a minute of motion or so.

The device has a holonomic drive system, capable of moving any direction at a maximum of about 8mph (3.6m/s), with a maximum acceleration of about 2g's. However, there are some simplifying constraints. For one, the field is nearly flat. The floor is made of a tough foam, so there is slight sinking, but the floor is flat except for a ramp of known angle (to a few degrees). The device will, excepting collisions, not be rising above the floor.

Accuracy is preferred over simplicity, so any mathematics required on the software side to improve the system would be welcomed.

Before I definitively choose accelerometers as the method of position tracking, though, I would like some idea of how much accuracy I could get, and the best ways of doing it.

答:
The mechanics of your vehicle are not extremely relevant here; I will assume that the motion your vehicle induces on the sensors will be within their specifications.

Entire volumes have been written on "sensor fusion", which is the act of combining measurements from multiple sensors (e.g. your gyro, accelerometer, and compass).  Doing this 100% accurately is impossible, so the goal is always to put some known bound on the error that can accumulate with time.  You will have to research that for yourself, as the best option depends greatly on your (and your CPU's) mathematical abilities.  Here is a document that discusses the development of a sensor fusion system with gyroscope, accelerometer, and compass so you get an idea of what's involved.

In the short term, one thing you can do is begin to characterize each of your sensors -- what is their precision and accuracy versus ground truth?  What sort of delay do you get from their measurements?  This characterization will help you more accurately combine their measurements later.


问:
How to calculate Euler Angles from gyroscope output?
I am using a tri-axis accelerometer and tri-axis gyroscope to measure the linear acceleration of a body. I need to get the orientation of the body in euler form in order to rotate the accelerometer readings from the body frame into the earth frame. Please help I'm so stuck</p>

答:
Using the Gyroscope

First thing to note is that the gyroscope is reporting angular rates with respect to the sensor. So if the sensor is rotating with some rate, the data it's outputting will be in the frame of the rotating sensor, NOT with respect to the fixed global frame. In other words, the X-Y-Z coordinate axes of the sensor are spinning with the sensor, whereas the global X-Y-Z axes remain fixed.

So the first task (before integrating), is to convert the angular rates from the sensor-body frame to angular rates in the global frame.

Sections 9.1, 9.2, and 9.3 in this OCW pdf lays out how to do this pretty well: https://ocw.mit.edu/courses/mechanical-engineering/2-017j-design-of-electromechanical-robotic-systems-fall-2009/course-text/MIT2_017JF09_ch09.pdf

Once you have the angular rates in the global frame, you can then integrate and accumulate the angles as mentioned in other answers.

Using the Accelerometer

Pitch and roll Euler angles in the global frame can also be calculated with the accelerometer.

When the sensor is sitting at rest, it will sense the force due to gravity in the negative Z direction as 9.8 m/s^2. Using this knowledge, we can find how much the sensor has pitched or rolled, by calculating what component of the gravity vector has moved from the negative Z axis, to other axes.

Equations that you can use:

Pitch Angle $= \arctan(\frac{A_y}{\sqrt{A_x^2+A_z^2}})$

Roll Angle $= \arctan(\frac{A_x}{\sqrt{A_y^2+A_z^2}})$

Note that this won't give exactly correct results when the sensor is being accelerated by an external force (when you are pushing or rotating it), because there are other unknown forces besides gravity acting on the sensor.


问:
stepper or servo torque in one direction only
What I want to achieve is this. I have a rod, which I want to move only in the direction of its axis, but move it (ideally) without resistance, meaning that If i pull the rod, I want it to follow (possibly aided by a stepper?) and if I push the rod, I want it to move back (retract) without resistance. But I want to be able dynamically to push it or pull it up to a point and be able to lock it after that. Example: I want to push the rod for 12.5cm but after that I want it to prevent me from pushing more.

So, I would be able to push for max 12.5cm and not after that, but also be able to pull it freely (up to some points let's say 30 cm). Maybe a stepper could do that? Hold torque in one direction (once it reaches 12.5cm) but rotate freely in the other direction (pulling). I'm not even sure a torque sensor would help, since I'm not applying torque, or that even a stepper is the right thing to use.

What I mean by dynamically, is that those distances are not fixed and can change programmatically (arduino/PI doesn't matter). It's not an absolute requirement that the rod follows automatically aided by a motor (=no resistance), but would be ideal. The most important is that I want it to lock once it has reaches certain distances either while I pull or I push it (mechanically with my hand).

答:
The sketch shows the basics of a simple design. The rod, sketched in red, can slide left and right. It is kept axial by sliding through holes in the black guides. A washer, purple, is fixed to the rod.

The rod also passes through a hole in a movable stop, blue. Controlled by a microcontroller, the stepper motor turns the lead screw, which moves the nut left and right, which adjusts the position of the stop.

The rod can be pushed freely to the right, until the washer hits the stop. When it's at the stop, the rod can still be pulled to the left.

If it is required to be able to pull out the rod entirely, just change the geometry. Instead of the current guide-washer-stop-guide, you could get rid of the washer and have the end of the rod hit the stop, as guide-guide-stop.


问:
Strategies for tuning with low sample rate
I’m designing an arduino robot that’s objective is to find and drive towards a light using a photo resistor mounted on a servo. I want to write my program with PID control to sweep the photo-resistor while moving.

Currently my logic flow is:

powerMotors(0)

Continuously sweep servo 180

Continuously collect photoresistor

If photoresitor>photoresistorMax

photoresistorMax = photoresistor

Angle to light = servo position

Error = 90 - angle to light

powerMotors(Error*Kp)

So at the moment it is adjusted with P control. It wobbles and does not always make it precisely. I believe a major problem with it is that the sampling time essentially is limited by the time to sweep 180°.

Is there any adjustments you could recommend to improve sampling time and overall increase accuracy of my system?

Thanks

答:
I don't have full a solution for your problem, but I expect that this will improve performance...

I'll suggest that you decrease speed as the Absolute value of the Error increases.

something like...

speed = maxSpeed - abs(Error)

you would also need to ensure that speed never goes to zero, something like this...

speed = max(speed,minSpeed)


问:
HW limit servo motor range
I use MG90S servos(180 degree) for my robotic applications. In few places, all I need is a 30 to 90 degree range of the motor. The motors use a 5K pot for control. Can I change this pot to have limited range in my servo?

I understand we can code for appropriate angles, but I want the servos to operate only for a limited range (say, only 45 degrees), even if coded incorrect. So, what should be the hw change to acheive this?

答:
I thought of a few options (just ideas — I haven’t tried the first two):

Modify internal hardware stops. As @jsotola said, the MG90S design as supplied is controlled by mechanical stops. This article shows a procedure for removing the physical stop to allow continuous motion. The intent of the article is different to yours but gives good insight of the internal workings. It also shows the physical form of the potentiometer used in that particular version. It might be possible to add an additional physical stop to further reduce the motion. Sounds like hard work.

Modify the servo internal potentiometer. For example by inserting an in series resistor to achieve the target resistance after a smaller arc of rotation. On its own this wouldn’t prevent overrun of your intended stop in the case of programming error. Sounds pretty fiddly too.

Add external hardware stops. Instead of limiting the internal motion of the servo physically limit the motion of whatever it’s attached to by introducing physical stops external to the servo. Sounds like easier work.

Set software limits (i.e, only drive the PWM in the suitable range of motion). You mentioned that this is not desirable, but you didn’t mention why. If you tell us more about the actual problem you’re trying to solve, more help may be possible. This sounds easiest to me.

Change to a servo with the desired arc of movement. I would think you’d still need option 4 as well.

Option 1 and 3 will stall the servo, sink more current, and introduce physical shock as the servo hits the end of travel and tries to keep going, unless combined with option 4.

See also https://robotics.stackexchange.com/a/1619/27817

Title: HW limit servo motor range


问:
I can't get motors to turn with Raspberry pi
I want to turn some motors using my raspberry pi. I am able to turn an LED on and off using the 3.3V GPIO pin. For the motors, I tried using a L293D chip as per the instructions on this link.

What happened is that the very first time I set the circuit up for one motor, it worked perfectly. But then, I moved the pi a little and the motor has since refused to work. I even bought a new pi and still no luck with the circuit. I then bought a L298N board that fits smugly on top of the GPIO pins of the pi and followed the instructions on the this video

Still no luck, the motor just won't run with either pi. I am using four AA batteries to power the motor and a connecting the pi to a power supply from the wall. What could possibly be the problem here?

答:
I know that this is an old post but I found it when searching to understand why my motor wasn't working. I finally found the solution to my problem so I thought that I'd post it in case anyone else runs into it in the future.  I lost hours to this issue.

I'd run a very similar program on an arduino and it worked fine - I couldn't work out why it wasn't working on my pi.  I had coincidentally used the same tutorial as the author but I didn't even get the motor to run once.

My problem was that I was running the pi from a USB connected to my mac and it wasn't powerful enough to run the pi for the motor, despite having a separate 9v battery for the motor.  When I changed to a mains connected power supply the motor ran fine.


答:
To improve sampling time, you can usually avoid the full $180^o$ sweep, resorting to it only when the bot has no idea of where the light is. Otherwise the world has continuity you can use: the light is probably near where it was the last time you located it, and it's probably about as bright as it was then. So you can probably make a pretty good guess where to look.

For instance, if the bot just turned a little to the right, and the sensed light dropped off, the servo could look a little to the left and see if the light gets brighter again. It can stop the scan when it looks bright enough -- no need to hunt past the max every time just to make sure.

With all the "probably" and "assume" and "guess" of this approach, it will pay to be a little conservative about how to tell when it's confused. One sign is when you expect the light to be at one side but when you turn in that direction the detected level doesn't rise again. Even then, it could be smart about whether to turn toward full-left or full-right to begin the scan, and maybe even stop early if it sees the light bright enough again.

If it's necessary to cast it in terms of PID, I'd have to think about this harder.

Title: Strategies for tuning with low sample rate

Body:
I’m designing an arduino robot that’s objective is to find and drive towards a light using a photo resistor mounted on a servo. I want to write my program with PID control to sweep the photo-resistor while moving.

Currently my logic flow is:

powerMotors(0)

Continuously sweep servo 180

Continuously collect photoresistor

If photoresitor>photoresistorMax

photoresistorMax = photoresistor

Angle to light = servo position

Error = 90 - angle to light

powerMotors(Error*Kp)

So at the moment it is adjusted with P control. It wobbles and does not always make it precisely. I believe a major problem with it is that the sampling time essentially is limited by the time to sweep 180°.

Is there any adjustments you could recommend to improve sampling time and overall increase accuracy of my system?

Thanks


问:
What thread to use for a load-bearing telescoping lift
I'm trying to find a thread that has good fatigue resistance and strength for a telescoping lift. The lift will lift a weight of around 2-3 kg. I do not intend to spool it as I read that thread with good fatigue resistance are usually not good for spooling, although if there is a kind of thread that is also good with spooling then please let me know. I also want the thread to be relatively lightweight, although that is not a big priority.

I have been looking at steel thread wires but I was wondering if there are any alternatives. Thanks!

答: Look into fishing line. It rests quite comfortably on spools, can be quite strong, is applied where weight is a concern, and is readily available (although I don't know anything about its stretch). A web search for "fishing line strength" turned up many hits offering many ways of selecting which type of line (you just have to ignore the what-type-of-fish parts!)</p>


问:
How to obtain the Jacobian matrix for this 2Planar without differentiation
Take a look at the below picture, I would like to derive the Jacobian matrix without  differentiation. In this Modern Robotics book, the screw theory is used. I've derived the forward kinematic using PoE formula which stated here:

$$
\begin{align}
M &=\begin{bmatrix} 1&0&0& L_1+L_2\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix} \\
\mathcal{S}_2 &= [0,0,1,L_1,0,0]^T \\
\mathcal{S}_1 &= [0,0,1,0,0,0]^T \\
e^{[\mathcal{S}_2]\theta_2} &= \begin{bmatrix} c_{\theta_2} &-s_{\theta_2} &0&-L_1(c_{\theta_2}-1) \\s_{\theta_2} &c_{\theta_2} &0&-L_1s_{\theta_2}\\0&0&1&0\\0&0&0&1 \end{bmatrix} \\
e^{[\mathcal{S}_1]\theta_1} &= \begin{bmatrix} c_{\theta_1} &-s_{\theta_1} &0&0 \\s_{\theta_1} &c_{\theta_1} &0&0\\0&0&1&0\\0&0&0&1 \end{bmatrix} \\
T_2^0 &= e^{[\mathcal{S}_1]\theta_1} e^{[\mathcal{S}_2]\theta_2}M \\
&= \begin{bmatrix} c_{\theta_1+\theta_2}&-s_{\theta_1+\theta_2}&0&L_2c_{\theta_1+\theta_2}+L_1c_{\theta_1} \\
s_{\theta_1+\theta_2}&c_{\theta_1+\theta_2}&0&L_2s_{\theta_1+\theta_2}+L_1s_{\theta_1} \\0&0&1&0\\0&0&0&1\end{bmatrix}
\end{align}
$$
In the book, the authors differentiate the last column of $T_2^0$ to obtain this:
$$
\begin{align}
\dot{x} &= -L_1\dot{\theta}_1 s_{\theta_1} - L_2(\dot{\theta}_1+\dot{\theta}_2)s_{\theta_1+\theta_2} \\
\dot{y} &= L_1\dot{\theta}_1 c_{\theta_1} + L_2(\dot{\theta}_1+\dot{\theta}_2)c_{\theta_1+\theta_2}
\end{align}
$$
Or more compactly as
$$
\begin{align}
\begin{bmatrix} \dot{x}\\\dot{y}\end{bmatrix} &=
\begin{bmatrix} (-L_1s_{\theta_1}-L_2s_{\theta_1+\theta_2})&(-L_2s_{\theta_1+\theta_2})\\(L_1c_{\theta_1}+L_2c_{\theta_1+\theta_2})&(L_2c_{\theta_1+\theta_2})\end{bmatrix}
\begin{bmatrix} \dot{\theta}_1\\\dot{\theta}_2\end{bmatrix} \\
\dot{\mathbb{x}} &= [J_1(\theta) \ J_2(\theta)]\dot{\theta} \\
\dot{\mathbb{x}} &= J(\theta) \dot{\theta}
\end{align}
$$
where $J(\theta)$ the Jacobian matrix. The authors claim the Jacobian matrix can be obtained without differentiation using this formula:

I couldn't reach to the same result with the above formula. For the first column, $J_{1s}=\mathcal{S}_1$ which is not the same result obtained by differentiation. Any suggestions?

答:
For this case shouldn't the "space Jacobian" $J_s$ be $6 \times 2$ while the "Jacobian" $J$ is $2 \times 2$?

It seems like the "space Jacobian" operates on the joint velocities to deliver the end-effector space twist, while the "Jacobian" operates on the joint velocities to deliver the end-effector velocity in the space-fixed Cartesian basis.


问:
System for determining occupied seats in an auditorium
I need an app that can do live monitoring of whether each seat in an auditorium is occupied,  so visitors can load the app and see where to sit.

The auditorium has a relatively flat ceiling 4m high, and the seats are .5m wide.
The hardware cost per seat needs to be $5.

I'm looking for all solutions.  Web cams, preasure sensors, sonars, lasers, arduino, pi, intel edison, anything.

Obviously there cannot be wires that people could trip over.  Sensors on the ceiling could have wired networking.  Sensors on the seat or floor would need to have wireless communication.  sensors on the ceiling would need to consider occlusion by people sitting in the seats (think, if there is an empty spot between 2 people, can the sensor see it as empty)

In the end, the data needs to be collected as a simple list of which chairs are occupied/open

Possible solutions:

rasberry pi's on the ceiling every 8 seats with a camera.

pressure sensors under chair legs wired to pi's gpio

Drones flying around the auditorium :)

Any ideas?

Update (more constraints):

auditorium size is 400 seats

Installation costs should average 10 chairs per hour(400/10 = 40 hours)

as the picture shows, chairs are cushioned

regular maintenance should take no longer than 30 min. per 2-hour event(eg, batteries)

hardware should last 100 sessions

for auditorium cleaning, it should be possible to "disconnect" and "reconnect" the chairs with 4 hours of labor.

答:
OK, so the hardware cost per seat is $5. But what is the total cost per seat? What's the total cost for the whole auditorium? In short, what's your budget? Who will maintain this system? What other constraints do you have?

A simple pressure switch can be made for 50 cents, but wiring it might cost $100 per seat, so you meet the hardware cost constraint, but may fail elsewhere.

That said, with what little you've told us, the simple solution is to put a pressure plate on each seat, with sensor wires going down into the floor. Then all those sensors can be read by a single computer with a lot of input channels. That could easily be built for a cost of $200 + $5/seat, but I suspect that the installation costs will kill you.

If you want wireless communication, now you have to deal with how to power the thing. Who will make sure all those batteries are working every day, or will they all be changed out at a fixed time?

If you use one each of those cheap wireless transmitters and receivers in each seat and a small microcontroller to implement a poll/response mechanism, a battery, sensor and an enclosure, then you might just scrape by for $5/seat in volume.