问:
Robotic Arm Inverse Kinematic Algorithm
I have currently built a large 6 axis Robotic arm that uses a bunch of potentiometers to control the angles. I would like to move into Autonomous control of the robotic arm. I would like to tell the arm "Hey, move to this position" and it figures out the angles of all the joints (or the "pose") needed to reach that position. Here is my current plan:

I would like to generate a table of data. This table will essentially contain every position that the robot arm can take in a sphere. It will also contain a matrix which holds the angles each joint needs to have to reach this position. I would like to generate this table using forward kinematics. Essentially, I will input a matrix of angles, see which position the robot arm ends up in, and then store that result. Once data has been gathered for every possible position, my dataset is complete.

With this dataset, all the robot arm has to do is look up the end position given to it, and then read the angles off of it. This makes it very easy to solve the inverse kinematics problems. All the angles were already generated through forward kinematics, and now the robot arm is just reading off the values form the table.

My question now is: How do I generate this dataset? My robot arm has custom arm lengths, and custom angle constraint for each of the joints. I'm assuming MATLAB would be a useful tool for this? I want to generate the dataset autonomously, and just have it in a .CSV file if possible.

I have the idea, but not sure about how to implement. Any guiding lights would be appreciated. The dataset generation would require so much computational power, so not sure if it is even possible to generate this file.

答:
You've got six axes, and you're using potentiometers, so analog outputs there, but let's assume you're reading them with an 8-bit encoder, which is really not enough resolution to do anything precisely. If a joint has 360 degrees of motion, you're reading that into an encoder that can do 255 increments, and you get about 1.41 degrees per count.

Anyways, as unreasonably low-resolution as that is, going your proposed route you would:

Set the first five axes to zero,

Run the sixth joint through 256 positions, then

Increment the fifth joint by one, then

Run the sixth joint through 256 positions, then

Keep going until the fifth joint has run through 256 positions, then

Increment the fourth joint by one, etc.

You wind up with the sixth joint doing 256 positions for each of the 256 positions in the fifth, etc.

Assuming each joint is using 8-bit encoders, then you want:

```<code>(first)*(second)*(third)*(fourth)*(fifth)*(sixth)
(256)  *(256)   *(256)  *(256)   *(256)  *(256)
= (2^8)*(2^8)   *(2^8)  *(2^8)   *(2^8)  *(2^8)
= (2^(8*6)) = 2^48
</code>```

You're saying you want 2^(48) individual combinations, which is 281,474,976,710,656 possible combinations. That's just the number of combinations, too, not the actual amount of data you'd need.

You could use something like a uint64_t to index into that data, and just use each byte as a new axis and ignore the upper two bytes, but now you need six arrays, one each for x/y/z and roll/pitch/yaw. If your output positions for a particular pose were also given as byte resolutions, then you'd need at a minimum 6*(2^48) or over a petabyte of storage for all your possible output poses.

Now, because you can't hold a petabyte of data in memory at once, you'll need to scan (stream) through your file until you get a match.

Can it be done? Yeah technically I guess, but it's going to be super impractical. This is also assuming that you're only going for 8-bit resolution on the joints; anything higher will have a tremendous impact on the space requirements. 10-bit encoders would have 2^60 combinations, which is more than 1,000,000,000,000,000,000.

OwnerDisplayName: Marko Jurisic

Title: Robotic Arm Inverse Kinematic Algorithm

Body:
You've got six axes, and you're using potentiometers, so analog outputs there, but let's assume you're reading them with an 8-bit encoder, which is really not enough resolution to do anything precisely. If a joint has 360 degrees of motion, you're reading that into an encoder that can do 255 increments, and you get about 1.41 degrees per count.

Anyways, as unreasonably low-resolution as that is, going your proposed route you would:

Set the first five axes to zero,

Run the sixth joint through 256 positions, then

Increment the fifth joint by one, then

Run the sixth joint through 256 positions, then

Keep going until the fifth joint has run through 256 positions, then

Increment the fourth joint by one, etc.

You wind up with the sixth joint doing 256 positions for each of the 256 positions in the fifth, etc.

Assuming each joint is using 8-bit encoders, then you want:

(first)*(second)*(third)*(fourth)*(fifth)*(sixth)
(256)  *(256)   *(256)  *(256)   *(256)  *(256)
= (2^8)*(2^8)   *(2^8)  *(2^8)   *(2^8)  *(2^8)
= (2^(8*6)) = 2^48

You're saying you want 2^(48) individual combinations, which is 281,474,976,710,656 possible combinations. That's just the number of combinations, too, not the actual amount of data you'd need.

You could use something like a uint64_t to index into that data, and just use each byte as a new axis and ignore the upper two bytes, but now you need six arrays, one each for x/y/z and roll/pitch/yaw. If your output positions for a particular pose were also given as byte resolutions, then you'd need at a minimum 6*(2^48) or over a petabyte of storage for all your possible output poses.

Now, because you can't hold a petabyte of data in memory at once, you'll need to scan (stream) through your file until you get a match.

Can it be done? Yeah technically I guess, but it's going to be super impractical. This is also assuming that you're only going for 8-bit resolution on the joints; anything higher will have a tremendous impact on the space requirements. 10-bit encoders would have 2^60 combinations, which is more than 1,000,000,000,000,000,000.


问:
Matrix Algebra vs Trigonometry for Inverse Kinematics
What method is “better” for solving inverse kinematics for a robot arm: matrix algebra or trigonometry?

To define “better”:

efficiency/performance vs memory usage

generality (how well can it be adapted to different arm configurations)

I know trigonometry/geometric methods get very complicated when solving for more that a few DOF so matrix algebra would probably be the best solution there.

But for 2-3 DOF robot arms which method is preferred?

Update: As others have pointed out and more research has showed me, using matrix algebra compared to trigonometry for increase kinematics doesn’t affect performance. They are just different methods of setting up and solving the inverse kinematic equations. Once you solve the IK equations using either of these methods, it’s implemented the same way in the code. I think I prefer the matrix method for solving IK because it’s a little more general that trigonometry, it scales to more joints more easily, and I’m already using matrices for forward kinematics so it provides a better symmetry.

答:
I believe it is standard to use "Matrix Algebra" i.e. matrix representation for solving kinematics-related problems (more generally, solving system of equations) rather than "Trigonometry".
Matrix representation is compact and more efficient computationally speaking.</p>

ContentLicense: CC BY-SA 4.0


问:
What network protocol/control units are motor-based robots controlled with?
I know most vehicles are controlled via CAN (Controlled Area Network) protocol through an ECU (Electronic Control Unit) which send/receives data using this protocol and then sends/receives Amps/Volts to appropriate sensors throughout the vehicle.

I was wondering what is the standard network protocol to control motor-driven robots that is analogous to CAN/ECU controlling vehicles.

答:
In modern industrial motion control, the communication between the controller (motion controller, robot controller, CNC contoller) is typically done with:

Profinet

CanOpen

Sercos

EtherCAT


问:
Explanation of the epipolar constraint in stereo imaging
I am watching this video, in which the epipolar constraint is defined as

$x_l \cdot (x_l \times t)=0$

It means that the vector $x_l$ that passes through the observed point and the left camera origin is perpendicular to the normal of the epipolar plane, defined by that same vector, and the epipolar line.

What I fail to understand is, why is this a constraint at all?

Doesn't it hold for ANY two vectors $v$ and $w$ that $v \cdot (v \times w) = 0$?

答:
Doesn't it hold for ANY two vectors v and w that v⋅(v×w)=0?
-> Yes

but you should think about the later part of the lecture where this obvious equation is converted into an essential matrix equation.

By substituting one xl to xr you can convert the equation so that two cameras and a point are tied by this constraint.


问:
The Optimal Reduction Ratio
Hello everybody. For my robot design, I would like to utilize the most optimal gear ratio that I can use. For that I want to utilize the torque of my motor which is a linear function of rpm. This task proved to be difficult as I have no idea to turn my acceleration function which is dependent on the rpm to a time variable function. Any help is appreciated. The image attached is the torque vs rpm curve. Rpm of the motor is capped at 6400 ish.

答:
I think there is information missing to analyze the problem.
If that's an rpm-torque graph the fast answer is to search the largest combination of torque*rpm... And for the reduction ratio, 1:1 is the most efficient if you don't have any constraints .

But optimal will always depend on the application. And if you want motor efficiency you need to look at nominal values (for continuous use) of rpm and torque. Here you will have the best performance.

Hope that this also helps How high of a gear ratio can a motor have?

Title: The Optimal Reduction Ratio


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

答:
In my opinion, they are meaninglessly different. On the backside of all these names BA, motion only BA, pose-graph optimization, batch optimization, what they do is simply optimize the device trajectory (6dof x N).

The difference between BA and pose-graph optimization used to be whether they optimize the structure part. But these days, people marginalize the structure part in BA and introduce reprojection constraints to the pose graph which is more like batch optimization rather than graph optimization. I am not even sure if the option only BA has a structure part in the state.

I would put all of them into a pose optimization problem with different constraints.


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

答:
roslaunch expects you to be launching ROS nodes. And it's passing standard ROS nodes arguments such as the full name and log path. You're seeing the errors in it rejecting arguments __name: __log:

If you want to make this work you should use a script which is robust to receiving these extra arguments. Or wrap it in a script which will discard the extra arguments.

As a simple solution rospy.init_node will process those arguments. And you can access the stripped ones via rospy.myargs http://wiki.ros.org/rospy/Overview/Initialization%20and%20Shutdown

rosrun is a much lower level function that just executes the file from a different path

Title: roslaunch exit code 2 error


问:
Inverse Kinematic for 3DoF chain with 2 links constrained together
I'm trying to make at Python script that calculates the inverse kinematics for at 3DoF planar robot.
I've got the forwards kinematic working, but i am stuck at the inverse kinematics.

The problem is that q2 and q3 is actuated together by one hydraulic piston.
with maximum angles being:
q2 = 49 degrees
q3 = 57 degrees

q2 and q3 only "bends" downwards.
The fact that q3 is directly constrained by q2 makes it quite hard.

L1: 3.3m, L2: 4.7m, L3: 0.386m, L4: 0.352m

I've tried a few thing but the "best" working one, is only working in a narrow XYZ coordinate, and is off.

I've attached the currently "working" code.
Ignore Throw and *0.675 when calculating q4

```<code>def invKinNew(xyz, throw):
    d1 = 3.3 # Rotation height
    a1 = 4.706 # Arm length
    a2 = 0.386 # Flap length
    a3 = 0.352
    q3max=49
    q4max=57

    # Calculate oc
    xc = xyz[0]
    yc = xyz[1]
    zc = xyz[2]

    if throw > 0.386 and throw < 25:
        a3 = throw
    elif throw == 0:
        a3 = 0.386
    else:
        raise Exception("Throw outside border")

    # Calculate q1 which is the rotation and defined by X and Y
    q1 = math.atan2(yc, xc)

    # z can be negative sinde point can be under first joint height.
    s = (zc - d1)

    #Calculate q2 and q3. The 2 joints responsible for Z height distance away from origo
    r2 = xc**2 + yc**2

    D = ( r2 + s**2 - a1**2 - a2**2 - a3**2)/(2*a1*a2*a3)

    q4 = math.atan2(-math.sqrt(1-D**2), D)*0.675
    q3 = (q3max * q4)/q4max
    q2 = math.atan2(s, math.sqrt(r2)) - math.atan2(a2*math.sin(q3) + a3*math.sin(q4), a1 + a2*math.cos(q3)+ a3*math.cos(q4))

    return math.degrees(q1), math.degrees(q2), math.degrees(q3), math.degrees(q4)
</code>```

I've attached som test figure showing the behaviour

答:
I don't have the time to think this all the way through, but my gut reaction here is that you don't really have two degrees of freedom with those two joints because they're actuated together.

It's not clear to me what exactly you mean when you say, "q2 and q3 is actuated together by one hydraulic piston" - is there literally just one piston? If so then you should drop q2 and q3 and just use one joint for the hydraulic piston, where your q would be the linear extension of that piston. You'll wind up with some equations where your output link L4 is a function of the piston extension and the lengths of L2 and L3, but that's expected.

The math gets a bit more complex in a way, but that's what you need to do to satisfy the constraint.

In a DH parameter setup, instead of having one joint that has strictly a rotation component or strictly a translation component, I think you'll wind up with a "compound" joint where you have rotations and a translation that are some function of your joint position.

Hopefully this "hint" is enough to provoke some better answers :P


问:
Holonomic constraints on an upright wheel
A wheel moving in free space has the six degrees of freedom of a rigid body. If we constrain it to be upright on a plane and to roll without slipping, how many holonomic and nonholonomic constraints is the wheel subject to?

This is a question from NWUs's course on Foundations of Robot Motion,

According to my understanding,
the wheel has 2 non-holonomic constraints:

wheel is rolling and not slipping and

the wheel cannot slide along its axis of rotation.

but I don't understand how many holonomic constraints it would have.

Isn't, the wheel not able to slide along its axis of rotation also a holonomic constraint?

I hope someone can help me with the number of holonomic constraints on the wheel.

答:
Holonomic constraints are ones that are based on the position only. And restrict the system to an envelope.

You've identified one stated constraint that is non-holonmic and broken it down into two components as they have constraints related to the velocity.

The remaining constraints in your list are then holonomic.

The wheel is upright

The wheel is on the plane

You've above decided that not sliding is a non-holonomic constraint. Why are you questioning that? Because it's related to the previous position of the system you're restricting the change in position from one state to the next, thus it's related to the derivative of the position not the value of the position.

If you create a state space representation of the wheel and write the constraints out you may find it more revealing that way such that you can reveal the structure of the constraints and apply the definition of a holonomic constraint to each one.

There's also a good discussion of the difference here: https://physics.stackexchange.com/questions/409951/what-are-holonomic-and-non-holonomic-constraints


问:
Programming an ESC to have reverse mode
How do you program an ESC to have a reverse mode? We're looking to control an ESC from a servo board (for a robotics project).

Assuming that the input will be between 0 and 255, we're looking for 127 as off, 255 as fully forward and 0 as full reverse, so how do we achieve that?

答:
While my answer does not address reprogramming ESCs, which is the actual question asked, it does turn the motor the opposite direction. And it was hard for me to find a good answer on the web. Google searching can end up here, so I'm posting what I think is helpful.

For my ESCs, I have three wires coming out, and to reverse the direction of the motor, all I have to do is swap any two wires that go from the ESC to the motor. It worked for me. YMMV.

ContentLicense: CC BY-SA 3.0

Title: Programming an ESC to have reverse mode


问:
How to calculate probability of particle survival for particle filter?
I'm trying to figure out a way that I can calculate the probability that a particle will survive the re-sampling step in the particle filter algorithm.

For the simple case of multinomial re-sampling, I can assume that we can model the process like a Binomial distribution if we only care about one sample/particle.

So if the particle has a weight of w that is also the probability that it will get selected in a step of the re-sampling. So we use 1 - P(k, p, n) where P is the Binomial distribution, k is 0 (we did not select the particle in all our tries), p is equal to w and n is equal to M, the number of particles.

What is the case though in the systematic re-sampling, where the probability of a particle being selected is proportional but not equal to its weight?

答:
Survival rate for the case of multinomial resampling and the case of $w \geq \frac{1}{n}$ has been covered well by the accepted answer.

However, I didn't find the case of $w < \frac{1}{n}$ intuitive enough for myself, so I will share my own intuitive understanding about it, even though it might not be as formal. Forgive me for the lack of pictures.

How I visualize systematic resampling - Casino roulette:
First, I visualize systematic resampling similarly to a casino roulette over which we spread our cumulative distribution, starting from 0 and draw samples from the start of each roulette field, after spinning the roulette by the randomly sampled value $r_0 \in [0,1/n]$. The variable $n$ here is the number of particles.

A bit more formally: we split the cumulative distribution into $n$ bins of size $1/n$ each, with the first bin starting from $0$. Then we shift the position of those bins by the sampled random number $r_0$, sampling a weight from the beginning of each bin.

Survival (or no survival) of a weight $w$:
If $0 \leq w \leq \frac{1}{n}$, then the probability of $w$ surviving depends on the value of the random variable $r_0$. In order for $w$ to not survive, we need to generate an $r_0$ so that $w$ falls between two of our equally spaced indices that define our bins after shifting the position of the bins by $r_0$. In terms of casino roulettes, we should spin the roulette so that after the spin $w$ falls within a single roulette field and not between them.

We can visualize this $r_0$ offset sampling procedure as having a single $1/n$ sized bin from which we sample the starting position of all bins after the spin (each roulette field) at the same time, with $w$ not surviving if we sample an offset that results in no starting position being covered by $w$ for any of the $n$ bins.

That is equivalent to the probability of $r_0$ having a value in $[0, 1/n]$ but not falling on the part of the space covered by $w$ in any of the $1/n$ sized bins ($w$ can actually be present in 2 bins at most). More formally:

\begin{equation}
    prob(\bar{w}) = \frac{1/n - w}{1/n} = 1 - nw
\end{equation}

where $prob(\bar{w})$ is the probability of $w$ not surviving. Hence, the probability of surviving is equal to $nw$.


答:
In general, when teaching my students how to do IK my first recommendation is always to find a variable (length/angle/value/etc) that is only a function of one joint so that they have one equation with one unknown. They can then look for another variable with is a function of one joint, or that is a function of two joints where one is the newly-solved-for joint variable.

In this case, the distance from joint 1 to the end of the arm is only a function of the hydraulic piston driving joints 2 and 3, so you should be able to formulate an equation that only has the length of the piston as an unknown if given the desired end-effector position (since the location of joint 1 is fixed).

Once you have solved for the hydraulic piston length, you also get the joint 2 and 3 positions (based on your initial description), which will allow you to then solve for the angle of joint 1 based on treating everything after that joint as a rigid body with a known length.

Title: Inverse Kinematic for 3DoF chain with 2 links constrained together

Body:
I'm trying to make at Python script that calculates the inverse kinematics for at 3DoF planar robot.
I've got the forwards kinematic working, but i am stuck at the inverse kinematics.

The problem is that q2 and q3 is actuated together by one hydraulic piston.
with maximum angles being:
q2 = 49 degrees
q3 = 57 degrees

q2 and q3 only "bends" downwards.
The fact that q3 is directly constrained by q2 makes it quite hard.

L1: 3.3m, L2: 4.7m, L3: 0.386m, L4: 0.352m

I've tried a few thing but the "best" working one, is only working in a narrow XYZ coordinate, and is off.

I've attached the currently "working" code.
Ignore Throw and *0.675 when calculating q4

```<code>def invKinNew(xyz, throw):
    d1 = 3.3 # Rotation height
    a1 = 4.706 # Arm length
    a2 = 0.386 # Flap length
    a3 = 0.352
    q3max=49
    q4max=57

    # Calculate oc
    xc = xyz[0]
    yc = xyz[1]
    zc = xyz[2]

    if throw > 0.386 and throw < 25:
        a3 = throw
    elif throw == 0:
        a3 = 0.386
    else:
        raise Exception("Throw outside border")

    # Calculate q1 which is the rotation and defined by X and Y
    q1 = math.atan2(yc, xc)

    # z can be negative sinde point can be under first joint height.
    s = (zc - d1)

    #Calculate q2 and q3. The 2 joints responsible for Z height distance away from origo
    r2 = xc**2 + yc**2

    D = ( r2 + s**2 - a1**2 - a2**2 - a3**2)/(2*a1*a2*a3)

    q4 = math.atan2(-math.sqrt(1-D**2), D)*0.675
    q3 = (q3max * q4)/q4max
    q2 = math.atan2(s, math.sqrt(r2)) - math.atan2(a2*math.sin(q3) + a3*math.sin(q4), a1 + a2*math.cos(q3)+ a3*math.cos(q4))

    return math.degrees(q1), math.degrees(q2), math.degrees(q3), math.degrees(q4)
</code>```

I've attached som test figure showing the behaviour


问:
Thinking of getting a Roomba, which model is the most hackable?
I'm potentially getting a Roomba vacuum robot as a gift, and I was wondering which model might be the best to go with? I would like it to clean, but I'm by nature a tinkerer and will definitely be trying to play around. I'm a software developer by trade, but I play around with IoT devices and RPi's, as well as extensive Arduino programming in college. I found this article, and I'm wondering if there are only certain models that would allow for this kind of mod: https://hackaday.com/2021/03/09/roomba-gets-alexa-support-with-an-esp8266-stowaway/

If this is the wrong place, please let me know. Thanks!

答:
Unfortunately, the latest generation of Roombas (i.e. the "i", "j", and "s" series) don't include the "hacker port".  So they are not really hackable at all.  (But if all you want is Alexa integration, they do have that already).

Only certain models of the 600 series have the serial port or the Open Interface hardware that make it "hackable".  This feature is being phased out, and I'm not sure you can guarantee a hackable version if you buy one online.

So your best bet is to just get a dedicated hackable Create 2.  If you really want it to vacuum, you can purchase the parts separately to make it a functional vacuum.  However, you won't be able to use the internal payload area for your electronics.

But if you are willing to wait, I'd highly recommend getting the Create 3.  It is a much more modern robotics platform.  Again, I think you probably can turn Create 3 into a working vacuum, but it is probably harder.

Disclaimer: I work at iRobot where I am developing the next generation of consumer robots. However, my postings on this site are my own and don't necessarily represent iRobot's positions, strategies, or opinions.


问:
ROS: How to slow down in order to wait for my results
I am doing a very complex calculation inside a node using some other libraries.
The node is...

doing the calculation in an endless loop

it is calculating from the last calculation to ROS::Timer::Now()

Example:
```<code>while(true){
    do_some_expensive_calculation_and_publish(from=last_calculation_point, to=ROS::Time::Now());
    last_calculation_point = ROS::Time::Now();
}
</code>```

Unfortunately ROS time is running way faster than I can do the calculation. Therefore I would need to slow down ROS/Gazebo somehow in order to get the result of the calculation before the next one should begin. Otherwise it takes longer an longer to get the calculation results since the time since the last calculation increases.

How to do that properly?

答:
Since you're using gazebo already and it should then be generating simulated time you can ask gazebo to run slower than realtime.

In the physics section of your world you can ajdust the maximum realtime factor to tell gazebo to run slower than realtime using the real_time_factor parameter.

There's a tutorial on the physics parameters here which you may find helpful.

If you're running the gazebo gui you can adjust it in realtime to experiment before embedding it into your world sdf file.

As a side note this will get you working for this use case, but if you're looking to make this run outside of simulation where you can't slow down time. You may want to look at adjusting your algorithm to allow it to skip forward in time by dropping information to keep it current instead of running on older and older data.


问:
How to stabilize a quadcopter
Today was my quadcopter's first "flight". I'm running megapirate on a Crius AIOP v2 with a Turnigy Talon v2 frame.

I only touched the throttle stick on my remote, nothing else. When I felt the quadcopter was about to take off, I pushed the throttle just a little bit more, and the quadcopter oscillated 2 or 3 times and the just flipped over, landing on the propellers.

So, I broke 2 props, my frame feels a bit loose, I'll probably have to tighten the screws (I hope...). How can I tune the software so it will stabilize nicely after take off?

Edit :

I don't know if it was true oscillation or just random air flows making it unstable. I made some more tests yesterday and it was quite OK (even if I crashed a few times). This time, it was really oscillating but it was quite windy outside and the quadcopter managed to stabilize after all. So i'll probably have to tune my PIDs and find a way to do it without crashing.

Edit 2 : After some PID tuning, I managed to stabilize my quadcopter pretty well but it's still oscillating just a little bit. I guess I'll have to slightly change the values to get a perfect stabilization.

答:
Something you can do WRONG to very easily unstabilize a quadcopter is to put the wrong propeller on the wrong motor. There are both pushers and pullers, and depending on the configuration you choose, you need the right type. Its possible you had two of them swapped. When they broke, you got the new ones on properly.

This page really helped me. This one has some good info too.

Title: How to stabilize a quadcopter


答:
1- You can slow the execution time using sleep() from ros::Rate class. Also, Gazebo allows you do so using real_time_factor.

2- While Gazebo and ROS allow you to slow down the execution time, but it seems to me you're missing a good feature in ROS which is "ROS Action Protocol". The protocol is one-to-one, and reliable communication mode between nodes which differs from services in the sense that the client “doesn’t block” waiting on the server to respond. Actions are a form of asynchronous communication in ROS. Let's say you have two nodes. One node runs heavy computational process (i.e. big fibonacci number) and the second node is interested in the current/final result, feedback about the current process, or stopping the process at any particular time without blocking the client. This protocol is perfect for this scenario. Think about it as running each node with multi-threading.

Title: ROS: How to slow down in order to wait for my results

Body:
I am doing a very complex calculation inside a node using some other libraries.
The node is...

doing the calculation in an endless loop

it is calculating from the last calculation to ROS::Timer::Now()

Example:
```<code>while(true){
    do_some_expensive_calculation_and_publish(from=last_calculation_point, to=ROS::Time::Now());
    last_calculation_point = ROS::Time::Now();
}
</code>```

Unfortunately ROS time is running way faster than I can do the calculation. Therefore I would need to slow down ROS/Gazebo somehow in order to get the result of the calculation before the next one should begin. Otherwise it takes longer an longer to get the calculation results since the time since the last calculation increases.

How to do that properly?


问:
simple ROS script terminating without displaying errors
I am using ROS melodic on ubuntu 18.04, turtlebot 2.

I tried to create a simple ROS program, that tries to make the bot follow a specified path. My gazebo world looks like this:

The task is to try to make the bot follow the white path. The approach is to use the bot's camera to keep clicking photos using of its environment,and save it as "image.jpg", at a particular rate. This job is done by a program known as take_photo_mod.py. Now, there is another program "goforward_mod.py" that does two things: Processes image.jpg using openCV functions , and finds out the angle which the bot should move with the vertical to be aligned with the path.  It's angular velocity is controlled by this angle. If the angle (error) is zero, then the bot simply moves forward at a velocity 0.2 units (and zero angular velocity) If there's some error, then the angular velocity is set using that error, and linear velocity is set to zero.

The openCV aspect is working flawlessly and I have tested it numerous times. However, whenever I run the entire program (including the goforward aspect) through the terminal, the program immediately ends, and gives no error. I tried to remove the openCV aspect from the code, and made the "error" variable, a constant, and I set the angular velocity by simply using a P-controller. This code should simply make the turtlebot  rotate around the z axis but again, the script simply terminates without showing any errors.

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

import rospy
import cv2
import numpy as np
import math as m
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist

class GoForward():

    def _init_(self):
        rospy.init_node('GoForward', anonymous=False)
        rospy.loginfo("To stop TurtleBot CTRL + C")
        rospy.on_shutdown(self.shutdown)
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
        r = rospy.Rate(10)
        move_cmd = Twist()
        w=0
        vx=0
        while not rospy.is_shutdown():
            #frame=cv2.imread("image.jpg",1)
            #error=extract(frame)
            error=-0.47

            if(error==0):
                vx=0.2
                w=0
            else:
                vx=0
                kp=0.15
                w=kp*error

            move_cmd.linear.x = vx
            move_cmd.angular.z = -w  #-sign because we are controlling angular velocity in -z direction (clockwise),if the error is positive, and vice versa
            self.cmd_vel.publish(move_cmd)
            r.sleep()

    def shutdown(self):
        rospy.loginfo("Stop TurtleBot")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

print(error)

if __name__ == '__main__':
    try:
        GoForward()
    except:
        rospy.loginfo("GoForward node terminated.")
</code>```

Neither "To stop TurtleBot CTRL + C", nor "GoForward node terminated." is printed on the terminal. The terminal simply displays the value of error , and then the program finishes. It appears as if GoForward() is not executing at all.....what can be the causes for this?

答:
I took at look at your example and you're correctly noticing that it is not executing. The problem is that python expects the constructor will invoke __init__ while you are defining _init_ Note the different number of underscores. Thus non of your setup code is being executed. I fixed that and saw more of the outputs that you expected since the system was then successfully started.

More details on __init__

PS Thanks for the compact code example. I was able to reproduce your problem quickly to help you.


问:
Fiducials in conjuction with AMCL
I'm having some trouble getting fiducials (using aruco_detect) to work alongside amcl. The goal here is to use normal amcl localization when a fiducial is not present, and to switch to localization using fiducials when the camera sees a fiducial. After having a lot of trouble trying to use fiducial_slam for this, I decided to create my own node that subscribes to /fiducial_transforms (published by aruco_detect) and publishes to /initialpose (subscribed to by amcl).

While the subscribing and publishing works well with both aruco and amcl, I have been running into problems with the trigonometry involved in obtaining the robot's position and orientation. Since the robots position is relative to a 2D map (already created), I converted the rotation quaternions into euler angles and only considered the yaw value (using tf.transformations.euler_from_quaternion). To reconstruct the quaternion for publishing, I use tf.transformations.quaternion_from_euler, passing in (0, 0, new_yaw). After playing around with my calculation of new_yaw (including passing in -new_yaw, pi-new_yaw, pi/2-new_yaw, etc.), I was unable to un-invert the robot's rotation (a rotation of 30 degrees of the robot always corresponds to a rotation of negative 30 degrees in the quaternion).

My current assessment of the problem is that there is some disconnect between the axes of the 2D map and the response from aruco_detect, which for some reason may cause the rotation to always happen in a certain direction (though this does not explain why publishing the quaternions for (0, 0, new_yaw) and (0, 0, -new_yaw) have the same outcome, so I am still unsure). In addition, even though we are assuming for now that the fiducial is at the origin (facing in the quaternion direction (0, 0, 1, 0)), we would like to incorporate known values for the fiducial's position and rotation to obtain the robot's true pose.

答:
I think that you're correct in your assessment that "there is some disconnect between the axes of the 2D map and the response from aruco_detect." Reading the documentation for the topic /fiducial_transforms you'll see that those transforms are relative to the camera, "The transforms showing the pose of the fiducials relative to the camera."

As such you likely want to transform the data into the base_link or base_footprint coordinate frame before you do your logic to reproject the vehicle into the map coordinates.

Title: Fiducials in conjuction with AMCL


问:
What is meant by the phrase "movement positional jitter"?
I was searching online for the meaning of this statement but the closest I came across is that jitter here refers to some sort of "deviation".

Therefore, if we say that the movement positional jitter of a robotic arm gripper is within $5\%$ to minimize wear on the arm then what is meant by this statement?

答:
We can think of "jitter" as a fairly quick, fairly small perturbation upon the intended controlled behavior. A semi-quantitative design guideline (like "within 5%") needs a numerator and denominator. An arm that is supposed to move 20 cm allowing 1 cm wobble (5% of the distance) sounds pretty sloppy. A controller that overshoots by 5% might be acceptable if it settles down to an accurate, repeatable position. It's hard to be more specific with something so general.

Many things could present as jitter, with different causes, hence different ways to mitigate them. Here are a few.

underdamped servoing

A servo tries to move the load until the position sensor (feedback) corresponds to the command. Hobby servos, in particular, are tuned to manipulate some typical moment of inertia. A load with greater inertia will need more power to get it moving, and will overshoot. With large enough inertia, it can even oscillate without ever settling down. A useful phrase for googling is "PID control". To mitigate, there needs to be a better match between the load and the PID tuning, either by changing the design or by changing the tuning (if possible).

friction and backlash

Suppose the drive chain has noticeable static friction. To start things moving, the controller has to send more and more power to the motor until the thing finally gets unstuck. But then there's more instantaneous power than is needed to maintain the motion, so the system overshoots and then overcorrects. This nonlinear phenomenon is different from underdamped servoing in a linear system, although it looks similar. Mitigation measures include lubrication and beefier but tighter control.

system not stiff enough (bouncy)

Suppose the system is moving, and the motor abruptly stops. A system that isn't stiff enough will bounce around the endpoint. An example is in https://www.youtube.com/watch?v=HNpWsKflcMg at timestamp 3:51. (I had the sound off and skipped past the, uh, entertainer.) Other points, perhaps not simply bounce, are at 4:53 and 5:39. Mitigation measures include stiffening, reduce moment of inertia, and more sophisticated control.

underpowered power supply (voltage sag)

Hobby servos, for one instance, misbehave in all sorts of creative ways when the motors draw more current from the power supply than it can comfortably deliver, leading to voltage sag. Mitigation: have more current available, or smaller motors.

electrical noise, poor grounding

Noise in a sensor or command signal will effectively lie to the controller about what it's supposed to do, or corrupt the feedback as to what it's actually doing. The controller can never settle down, instead constantly doing the herky-jerky. I can't be too specific about mitigation, because there are so many ways things can go wrong electrically. Try not to have motor current sharing conductors with small signals, and otherwise carefully review grounding. Capacitors and shielding help sometimes.


问:
KK2.0 Quad Stablility
I'm running a kk2.0 + 4 20A Multistar ESCs + 4 EMax GF 2215-20 motors + 4 Slow Fly Props

After about a foot off the ground, the entire quadcopter starts wobbling like crazy (no auto-level). Any ideas?

I'll add some video if needed.

答:
Sounds like ground effects. When a plane or helicopter is close to the surface, the aerodynamics change. This distance is usually considered to be the same as wingspan, or rotor diameter for a helicopter. The lift/drag ratios change, the thrust efficiency changes, and the balance can be affected because you are moving on or off a bubble of pressurized air. Also the turbulence can go nuts, placing high demands on the control systems. Slower or unoptimized systems are going to struggle through this zone.
This effect is also dependent on the surface you are above, with the effect maximized on smooth hard surfaces. If the craft has the same behavior at the same height when hovering over grass or very bumpy surfaces that it does off a smooth piece of plywood on concrete, then the problem is something else. If the transition occurs at a lower height on grass, than you'll have found the problem.
Check out: http://www.skybrary.aero/index.php/Ground_Effect#The_Extent_of_Ground_Effect
Here is a forum where they had this problem, and were able to reduce it via control settings.
http://forums.openpilot.org/topic/7596-quad-unstable-in-ground-effect/
and if you want a formal discussion:
http://www.seas.upenn.edu/~dmel/ISER2012_powers.pdf</p>

Title: KK2.0 Quad Stablility


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

答:
I think this paper will help you.
Analytical Inverse Kinematics and Self-motion Application for 7-DOF Redundant Manipulator.</p>

Title: Inverse kinematics for redundant robot


问:
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).

答:
Your question is a little vague.  So I will try to cover all the bases.

The simplest way to achieve non-backdriveability is mechanically.  Using a worm gear will make it such that only the motor will be able to actuate the joint.  But any external forces will not be able to move the joint.  Of course, there are many other types of gearing that will accomplish the same thing.

If you only want your joint to move in one direction, a ratcheting mechanism is easy to accomplish mechanically.  But this will apply to both actuation from the robot's motors as well as external forces.

If you want the robot (arm I assume) to hold its position on its own, but move freely when there is an external force on it, the only way to accomplish this is with some form of sensing.  The simplest and most direct would be some type of force/torque sensor on the end-effector.  This is pretty easy to implement.  When the sensor detects a force, simply move the end-effector in the direction of the force.  Of course the downside is that pushing elsewhere on the arm has no effect.

If you want to be able to push on the arm anywhere not just on the sensor, you need sensing in each joint.  Adding some series elastic actuation will
allow you to detect forces in the direction of the joint.  This can be as simple as a spring and displacement sensor (or metal beam and strain gauge).

The fanciest method is probably the most difficult to achieve.  This involves sensing the motor currents of the motors, and using that to determine if there is an external force.  See also gravity compensation.  This is tricky for a number of reasons:

large amounts of gearing decreases the forces seen by the motor

it requires a bunch of external circuitry that needs to be high enough resolution.

it requires a very good model of the motor.  (both electrically and thermally)

the small rise in motor currents due to external forces are usually orders of magnitude smaller than the the currents required just to move the arm around on its own

hysteresis is a big problem.  detecting forces in the direction of motion are quite different than forces opposing motion.


问:
How can I use the pen Node on my custom robot in webots?
I am starting to attempt a new project about a mechanical arm using a pen to draw random things as I move the robot's arm.

so to do that Webots already has a node named "Pen"
(Webots doc,
and this is
an example project by cyberbotics)
which I think I can use for this project, But I don't know what is the proper way of attaching this node to my custom robot or whether it is possible even to get me started for my project. let alone about the procedures for doing so.

edit:

warning: if you happen to know a tutorial or a platform that includes all the procedures, if you share it, it would be more convenient and less time-consuming. if there isn't any continue to read the demanded details which are included below to solve the problem together, thanks for your patience.

what I have done so far

here's a scene of a trial error of mine to make this node work.

found the pen node in the base nodes.

Although it seems to be fairly easily attached, it is not working.

these are the other attributes that might cause issues.

according to Webots doc:

"In order to be paintable, an object should be made up of a Solid node containing a Shape with a valid Geometry and an ImageTexture. The painted layer is applied over the texture without modifying it."

I added another node from Solid and added a plane Geometry to its Shape, as you can see in the image below,

so another thing that should be added to the project tree is the ImageTexture node which I tried adding from the appearance attribute just a basic Appearance value, and then adding the ImageTexture to the Texture.

but it doesn't seem to be working still.

if you see any issues with my procedure or you know how to add this Node to the project please share some screenshots so we can follow the steps.

thank you for your guidance!

答:
firstly I have to say I'll make a youtube tutorial video on how to do this complete procedure for future students wondering about this problem.

Answer

with more tweaking on the pen node and adjusting the parameters, I found the problem to be more related to the angle or rotation of the pen node. if you want to draw something on the ground or paper which you have prepared, you should have the green arrow (x-axis in my case) of the pen face upwards or against the paper in the opposite direction.

you can see the parameters changed in the following image:

the translation attribute has to have some distance even small from the paper to make the ink draw or have physical contact with the ImageTexture node.

Some other changes

here's a more unrelated attribute but necessary to mention, I created a white 256x256 picture in the paint app and saved as /project/worlds/textures/whitepaper.png and linked to the ImageTexture>url.

Final results


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

答:
This is a really complex example to start with if you're just getting into deriving equations of motion; I would think any course would have you do lots of examples with much simpler systems and build towards working on something like this.

That said, when I do work like this in Matlab I try to start with positional equations. You've got drawings there that show things like $c$, $h$, $\phi$, etc. Which of those are time-varying, and which aren't?

Perturb the system and make your positional equations from there, like the second drawing. Look at examples for compound pendulums and see how they've setup their positional equations. Remember that ultimately you're trying to go for kinetic and potential energies, so you'll need to get positions to all the centers of mass.

When you create the equations in Matlab, use the symbolic toolbox. You can define constants with the sym command, like

```<code>sym b;
</code>```

and time-varying parameters by putting (t) after the variable, like

```<code>sym s(t);
</code>```

then, when you write your equations, you can just use the variables b and s and Matlab understands that s varies as a function of time and b does not. This is important because, once you get your position equations completed, you can take derivatives with the diff command. You can just do diff(myEqn) and Matlab understands to take the first derivative with respect to t, or you can be explicit about it like diff(myEqn, t). There's are a few other specifics you can read about in the documentation, but those are the basics for diff.

The Euler-Lagrange equation has the "q" terms, which is just the "variable in question" at the time, so you repeat the process for each variable and wind up with your system of equations. I think you would use $s$ as a q once and then use $\phi$ as a q once, which should end you up with some set of equations for $\ddot{s}$ and $\ddot{\phi}$, which you'd use for your equations of motion. If you're going to put an actuator on $b$ or something to change the bob weight then you'd want to use that, too.

Ultimately you need to get the Lagrangian to start with, like

$$
L = K - U \\
$$

and you can do that with $U = mgh$ and $K = (1/2)mv^2$. You'll get your velocity $v$ term by taking the time derivative of your position equation, with diff.

Then, when you get $L$, you can find the Euler-Lagrange equation, like

$$
\frac{\partial L}{\partial q} L(t) - \frac{d}{dt}\frac{\partial L}{\partial \dot{q}} L(t) = 0 \\
$$

So, once you define what $L$ is, you need to again go through each time for a different "q" variable ($s$, $\phi$, etc.) and get your Euler-Lagrange equation. Again, you can kind of leverage Matlab's diff function for this but it becomes more difficult because Matlab does a pretty poor job at handling partial derivatives of derivatives.

What I do is to use the subs command to pull my derivatives out and replace them with something easier to reference, like

```<code>syms sDot;
L2 = subs(L, diff(s,t), sDot);
</code>```

That would give you L2 as a function of sDot instead of diff(s, t). I do this because again the partial derivative is kind of a pain to do. What you're going for is taking the derivative of L with respect to $\dot{s}$, but what that looks like in Matlab would be:

```<code>dLdqDot = diff(L, diff(s,t));
</code>```

and it's been long enough that I can't remember exactly what the issue was, but I remember Matlab not being able to intelligently handle that. By swapping out your diff(s, t) with a singular variable like sDot you can instead do something like:

```<code>dLdqDot = diff(L, sDot);
</code>```

and get better results. Once you're done with that, you'll need to subs back in your version as a function of time so you can take the time derivative of that, the $d/dt$ of the $d/dt(\partial L / \partial \dot{q})$ term.

```<code>dLdqDot_t = subs(dLdqDot, sDot, diff(s, t));
</code>```

Then you get your right hand side:

```<code>eulerLagrange_RHS = diff(dLdqDot_t, t);
</code>```

and then finally one more round of subs to get your variables back out to be sortable again:

```<code>syms sDdot;
el_RHS = subs(eulerLagrange_RHS, diff(s, t, t), sDdot);
el_RHS = subs(el_RHS, diff(s, t), sDot);
</code>```

and it's important to note that you need to do the second derivative FIRST or you'll wind up with something like diff(sDot, t) instead of diff(s, t, t) or sDdot. After you do the second derivative you can do the first derivative.

Then, once you're done with the (d/dt)(dL/dqDot) then you can work on doing the same for the dL/dq as the left-hand side, and then you can setup your equation:

```<code>equationOfMotion(1) = el_LHS - el_RHS == 0;
</code>```

and then you can solve(equationOfMotion(1), sDdot) and get your equation of motion for $\ddot{s}$. Then you do it all over again for $\phi$. And yes, it's super, super tedious, but again you should have had at least one full semester and probably two full semsters of doing homework basically like this at least once a week, classes like statics, dynamics, system dynamics, etc.

This is all more about how to wrangle Matlab than the specifics about your problem, but do your work in a script in Matlab and you can make all the pretty plots you want along the way to check your work.

I would strongly, strongly recommend you parameterize your variables at the end and use a variety of plot commands to visualize what you're working with, to recreate the scene through various values of $\phi$, etc., to make sure you haven't gone wrong somewhere.

If you try working your way through this and have more issues, please come back and show your work with the problem as far as you've gotten and a description of why you think it's wrong or what has you stuck.

Also a disclaimer - I don't actually have access to a copy of Matlab any more or any of my prior work (changed jobs wooo) so this is all from memory; you'll probably find some mistakes in the syntax or something but the steps should be generally accurate.

Body:
This is a really complex example to start with if you're just getting into deriving equations of motion; I would think any course would have you do lots of examples with much simpler systems and build towards working on something like this.

That said, when I do work like this in Matlab I try to start with positional equations. You've got drawings there that show things like $c$, $h$, $\phi$, etc. Which of those are time-varying, and which aren't?

Perturb the system and make your positional equations from there, like the second drawing. Look at examples for compound pendulums and see how they've setup their positional equations. Remember that ultimately you're trying to go for kinetic and potential energies, so you'll need to get positions to all the centers of mass.

When you create the equations in Matlab, use the symbolic toolbox. You can define constants with the sym command, like

sym b;

and time-varying parameters by putting (t) after the variable, like

sym s(t);

then, when you write your equations, you can just use the variables b and s and Matlab understands that s varies as a function of time and b does not. This is important because, once you get your position equations completed, you can take derivatives with the diff command. You can just do diff(myEqn) and Matlab understands to take the first derivative with respect to t, or you can be explicit about it like diff(myEqn, t). There's are a few other specifics you can read about in the documentation, but those are the basics for diff.

The Euler-Lagrange equation has the "q" terms, which is just the "variable in question" at the time, so you repeat the process for each variable and wind up with your system of equations. I think you would use $s$ as a q once and then use $\phi$ as a q once, which should end you up with some set of equations for $\ddot{s}$ and $\ddot{\phi}$, which you'd use for your equations of motion. If you're going to put an actuator on $b$ or something to change the bob weight then you'd want to use that, too.

Ultimately you need to get the Lagrangian to start with, like

$$
L = K - U \\
$$

and you can do that with $U = mgh$ and $K = (1/2)mv^2$. You'll get your velocity $v$ term by taking the time derivative of your position equation, with diff.

Then, when you get $L$, you can find the Euler-Lagrange equation, like

$$
\frac{\partial L}{\partial q} L(t) - \frac{d}{dt}\frac{\partial L}{\partial \dot{q}} L(t) = 0 \\
$$

So, once you define what $L$ is, you need to again go through each time for a different "q" variable ($s$, $\phi$, etc.) and get your Euler-Lagrange equation. Again, you can kind of leverage Matlab's diff function for this but it becomes more difficult because Matlab does a pretty poor job at handling partial derivatives of derivatives.

What I do is to use the subs command to pull my derivatives out and replace them with something easier to reference, like

syms sDot;
L2 = subs(L, diff(s,t), sDot);

That would give you L2 as a function of sDot instead of diff(s, t). I do this because again the partial derivative is kind of a pain to do. What you're going for is taking the derivative of L with respect to $\dot{s}$, but what that looks like in Matlab would be:

dLdqDot = diff(L, diff(s,t));

and it's been long enough that I can't remember exactly what the issue was, but I remember Matlab not being able to intelligently handle that. By swapping out your diff(s, t) with a singular variable like sDot you can instead do something like:

dLdqDot = diff(L, sDot);

and get better results. Once you're done with that, you'll need to subs back in your version as a function of time so you can take the time derivative of that, the $d/dt$ of the $d/dt(\partial L / \partial \dot{q})$ term.

dLdqDot_t = subs(dLdqDot, sDot, diff(s, t));

Then you get your right hand side:

eulerLagrange_RHS = diff(dLdqDot_t, t);

and then finally one more round of subs to get your variables back out to be sortable again:

syms sDdot;
el_RHS = subs(eulerLagrange_RHS, diff(s, t, t), sDdot);
el_RHS = subs(el_RHS, diff(s, t), sDot);

and it's important to note that you need to do the second derivative FIRST or you'll wind up with something like diff(sDot, t) instead of diff(s, t, t) or sDdot. After you do the second derivative you can do the first derivative.

Then, once you're done with the (d/dt)(dL/dqDot) then you can work on doing the same for the dL/dq as the left-hand side, and then you can setup your equation:

equationOfMotion(1) = el_LHS - el_RHS == 0;

and then you can solve(equationOfMotion(1), sDdot) and get your equation of motion for $\ddot{s}$. Then you do it all over again for $\phi$. And yes, it's super, super tedious, but again you should have had at least one full semester and probably two full semsters of doing homework basically like this at least once a week, classes like statics, dynamics, system dynamics, etc.

This is all more about how to wrangle Matlab than the specifics about your problem, but do your work in a script in Matlab and you can make all the pretty plots you want along the way to check your work.

I would strongly, strongly recommend you parameterize your variables at the end and use a variety of plot commands to visualize what you're working with, to recreate the scene through various values of $\phi$, etc., to make sure you haven't gone wrong somewhere.

If you try working your way through this and have more issues, please come back and show your work with the problem as far as you've gotten and a description of why you think it's wrong or what has you stuck.

Also a disclaimer - I don't actually have access to a copy of Matlab any more or any of my prior work (changed jobs wooo) so this is all from memory; you'll probably find some mistakes in the syntax or something but the steps should be generally accurate.


问:
Move a Three Wheel Omni Robot
I understand this isn't for programming but it's easy to read. I have referenced This for a guide on how to move a 3 wheel omni robot but I have encountered a few problems.

```<code>speed = 100
heading = 45
a1 = 90
a2 = 210
a3 = 330

o1 = a1 - heading
m1 = speed * math.sin(o1)

o2 = a2 - heading
m2 = speed * math.sin(o2)

o3 = a3 - heading
m3 = speed * math.sin(o3)
</code>```

The heading is the angle from X clockwise so 90 being directly up. When I input a heading of 45 it is expected that m1 = 100, m2 = 0 and m3 = -100 yet I get m1 = 85, m2 = 100 and m3 = 77 what could be the problem here?

答:
I think this is a degrees/radians issue, but I'm not sure how you're expecting the numbers you've got. Consider your inputs:

```<code>speed = 100
heading = 45
a1 = 90
o1 = a1 - heading
m1 = speed * math.sin(o1)
</code>```

Then I would expect o1 = 90 - 45, or o1 = 45, and then I would expect m1 = 100 * sin(45). Here's where it's not clear. Typically I would expect a trig function like sine, cosine, tangent, etc. to take the input in radians unless the document explicitly states otherwise.

Let's assume it is in degrees, though. In that case, the sine of 45 degrees is 0.7071, and so I would expect (in degrees) m1 = 100 * 0.7071 or m1 = 70.71. You stated you're expecting it to be 100, so I'm not sure where that's coming from.

However, what if the math.sin is treating your input as being in radians? Well, 45 radians is a TON - multiply by 360 and divide by $2\pi$ to get 45rad = 2578.4deg. What is the sine of 2,578.4 degrees? 0.852. So, if you're passing in 45 radians, then you wind up with m1 = 100 * 0.852 or m1 = 85.2. If you're casting to int or something then you wind up with m1 = 85, which is what you got.

For m2, you're taking 210-45 and then treating that as radians, but 165rad = 9454deg. Then, sine of 9,454 degrees is 0.998, and you wind up with m2 = 99.8, which rounds to 100.

Try converting to radians first and double-check your expected values and hopefully that gets everything straight for you :)

Title: Move a Three Wheel Omni Robot

Body:
I think this is a degrees/radians issue, but I'm not sure how you're expecting the numbers you've got. Consider your inputs:

speed = 100
heading = 45
a1 = 90
o1 = a1 - heading
m1 = speed * math.sin(o1)

Then I would expect o1 = 90 - 45, or o1 = 45, and then I would expect m1 = 100 * sin(45). Here's where it's not clear. Typically I would expect a trig function like sine, cosine, tangent, etc. to take the input in radians unless the document explicitly states otherwise.

Let's assume it is in degrees, though. In that case, the sine of 45 degrees is 0.7071, and so I would expect (in degrees) m1 = 100 * 0.7071 or m1 = 70.71. You stated you're expecting it to be 100, so I'm not sure where that's coming from.

However, what if the math.sin is treating your input as being in radians? Well, 45 radians is a TON - multiply by 360 and divide by $2\pi$ to get 45rad = 2578.4deg. What is the sine of 2,578.4 degrees? 0.852. So, if you're passing in 45 radians, then you wind up with m1 = 100 * 0.852 or m1 = 85.2. If you're casting to int or something then you wind up with m1 = 85, which is what you got.

For m2, you're taking 210-45 and then treating that as radians, but 165rad = 9454deg. Then, sine of 9,454 degrees is 0.998, and you wind up with m2 = 99.8, which rounds to 100.

Try converting to radians first and double-check your expected values and hopefully that gets everything straight for you :)


答: Make sure you have set PIDs for your frame correctly. Here is a video which explains behavior of quadrotor under various PID settings which will help you tune gains for your frame. Also Make sure your frame is well balanced with CG at center of board. ESCs must to be calibrated so that all 4 motors perform equally. Unbalanced propellers can cause significant vibration hence   make sure propeller are well balanced. Always fly quadrotor to height of more than 1m and then check behavior so that ground effects are negligible. </p>

Title: KK2.0 Quad Stablility

Body:
I'm running a kk2.0 + 4 20A Multistar ESCs + 4 EMax GF 2215-20 motors + 4 Slow Fly Props

After about a foot off the ground, the entire quadcopter starts wobbling like crazy (no auto-level). Any ideas?

I'll add some video if needed.


问:
How does baseline work with forward motion in Monocular Visual Odometry
In my computer vision class we are studying visual odometry. In this we calculate the Fundamental matrix using 2 frames from forward motion using the concept of stereo-vision. But in stereo vision the two cameras are side-by-side or at least not one after the another.

tldr,
In VO we need a good enough baseline to reduce depth uncertainty. But how does that work when the frames are taken one after the another like in a moving car?

答:
You are correct in your understanding that monocular VO will have an ambiguity in the scale of the reconstructed trajectory and scene.

The easiest way to fix this ambiguity is to incorporate more information via additional sensors.  For example, a GPS measurement could give an absolute reference that can be used to scale the VO solution correctly. A barometer could give a better estimate of altitude changes, etc.

Alternatively, you could use the motion model of the platform to figure out the correct scale factor.
In the case of a vehicle, the camera is mounted at a fixed distance and angle above the ground. In this case, your algorithm could be something along the lines of:

Compute your fundamental matrix (F) from two sequential frames

Compute the 3d points and rotation/translation between the sequential frames.

At this point, you will only have the points and translation up to scale, because there has been an unknown translation between the frames.

Use a sample/consensus algorithm, such as RANSAC, to find a ground plane in your computed 3d points

Other heuristics may be used to accelerate this (positions of points in the frame, etc)

Using your known physical configuration, compute the translation scale factor that makes the plane match.

For other platforms, you may be able to take advantage of the motion model to perform a similar "trick".


问:
Choose the structure of the dynamic model for the generalized equations of motion of the robot
I ask the robotics specialists for help as I am a little confused.

I have two subsystems which are shown in the picture below:

One of them is a system of stable and controlled differential equations of motion for robot.

Second is a block that implements a Forward Kinematic $f(\theta)$ from a signal entering its input.

$\theta,\Phi,\phi$ - position of drives, positions of end-effector from dynamic model and Forward Kinematic data, $u(t)$ - control signal.

I came across this article and a diagram from it.

Hybrid Open-Loop Closed-Loop Control of Coupled Human-Robot Balance During Assisted Stance Transition with Extra Robotic Legs

Task: choose the structure of the mathematical model in such a way that $\phi=\Phi$. In the above diagram, I am confused by the fact that a block of direct kinematics is used to estimate the angular position of the robot, and, generally speaking, this mechanical coordinate should be available through the equations of motion.

答:
The system in the paper is supposed to control an exoskeleton which will mimic the motions of the person inside, except bearing the weight of the backpack (and some of the person's weight, too). There is no traditional control input for servoing. Instead, to detect the person's intent, it seems to rely on how the person inside incrementally perturbs the posture.

The forward-kinematics part of the system computes what it thinks the system posture should be, given what it knows of the control inputs to the motors. If the measured posture differs from that reference posture, that must be because the person is trying to move, so the overall control should accommodate.

The paper's innovation seems to be to distinguish (a) person-caused perturbations from (b) the errors that servo controls typically try to eliminate. That sounds sort of magical, and the paper itself is not a model of transparent communication. I hope my interpretation here adds clarity, not confusion, but sifting through the details of the math is your avocation, not mine!


问:
What dependencies do I need for USB programing in python with pyUSB?
I am trying to get the usb.find command to work properly in a python script I'm writing on Angstrom for the Beagleboard.

Here is my code:

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

import usb.core
import usb.util
import usb.backend.libusb01 as libusb

PYUSB_DEBUG_LEVEL = 'debug'
# find our device
# Bus 002 Device 006: ID 1208:0815
#  idVendor           0x1208
#  idProduct          0x0815
# dev = usb.core.find(idVendor=0xfffe, idProduct=0x0001)
# iManufacturer           1 TOROBOT.com

dev = usb.core.find(idVendor=0x1208, idProduct=0x0815,
backend=libusb.get_backend() )
</code>```

I don't know what's missing, but here is what I do know.
When I don't specify the backend, no backend is found.  When I do specify the backend usb.backend.libusb01 I get the following error:

```<code>root@beagleboard:~/servo# ./pyServo.py
Traceback (most recent call last):
  File "./pyServo.py", line 17, in <module>
    dev = usb.core.find(idVendor=0x1208, idProduct=0x0815, backend=libusb.get_backend() )
  File "/usr/lib/python2.6/site-packages/usb/core.py", line 854, in find
    return _interop._next(device_iter(k, v))
  File "/usr/lib/python2.6/site-packages/usb/_interop.py", line 60, in _next
    return next(iter)
  File "/usr/lib/python2.6/site-packages/usb/core.py", line 821, in device_iter
    for dev in backend.enumerate_devices():
  File "/usr/lib/python2.6/site-packages/usb/backend/libusb01.py", line 390, in enumerate_devices
    _check(_lib.usb_find_busses())
  File "/usr/lib/python2.6/ctypes/__init__.py", line 366, in __getattr__
    func = self.__getitem__(name)
  File "/usr/lib/python2.6/ctypes/__init__.py", line 371, in __getitem__
    func = self._FuncPtr((name_or_ordinal, self))
AttributeError: python: undefined symbol: usb_find_busses
</code>```

What am I missing so that this will work properly?

Thank you.

答:
At the time of writing, OP is still missing some info so this is only my guess.

TLDR; My guess of a cause of the problem is you didn't have libusb version 0.1

As in unix.stackexchange.com#q428927, there seems to be API breakage b/w 0.1 and 1.0 of libusb, and the symbol in the error you got, usb_find_busses, was removed in 1.0.

However, in your code you're specifying to use 0.1, so API breakage shouldn't matter here (esp. knowing pyusb is capable of handling libusb 0.1).

The only situation I can think of for why you still see this error is that pyusb expects libusb 0.1 to be available on your environment but it is unavailable somehow.


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

答:
Edited after a clarifying comment: this was a pretty good answer, except it assumed the question was about an RC car!

Gravity is your friend here. It always points straight down, exactly 1 G.
Orient your car with its +X axis straight down, and capture the accelerometer reading, all three of its axes. That gives one row of your matrix. (You could double-check by measuring with the car's -X straight down, but when you measure something twice, you have to explain why the measurements are different :-))

Do the same thing with the car's Y and Z axes for the other two matrix rows. The resulting matrix will transform car coords to accelerometer coords. In the likely case of wanting to transform the other way, invert the matrix.


问:
How do I calculate the duration of a movement of a stepper motor
my program controls a stepper motor. The API of the motor allows me to set the maximum speed, maximum acceleration, maximum deceleration and the jerk. I would like to pre-calculate the duration of a movement.
e.g. Max Speed = 10m/s, Max Acceleration/Deceleration = 500m/s², Jerk = 1000m/s³, distance to new location = 0.5m, duration = ?.
Initial speed and acceleration are 0.

To be honest, I didn't know about the jerk before and I used the naive calculation time = distance / speed. I know that even without the jerk this calculation is wrong as I ignore the non-constant acceleration.

I find a lot of posts in the internet (e.g. https://en.wikipedia.org/wiki/Fourth,_fifth,_and_sixth_derivatives_of_position) but nothing that takes into account a max speed and max acceleration.

答:
Let's suppose the movement should traverse the distance as fast as possible. It will accelerate for the first part, then reverse the signs and decelerate for the second part. While in progress you want it moving as fast as possible (either because it hits the speed limit, or because it reaches the midway point before hitting the speed limit). To hit the highest speed, you accelerate as hard as possible (again, subject to the acceleration limit). To accelerate as fast as possible, you apply the max jerk for as long as possible (subject to etc etc).

The wikipedia page for Jerk shows how to compute the distance traveled, as a cubic function of time, with the jerk as a parameter. I would start by plugging in your values, assuming max jerk, to see what happens. If that travels the distance without exceeding your max acceleration or velocity, you have the easy solution. If instead it hits a limit, it's still a piecewise solution, just with more than two pieces.

Apply max jerk to accelerate as hard as you can. If it's going to hit the speed limit first, you need to reduce the acceleration to become zero just as it hits the speed limit, so apply negative jerk as late as possible. If it's going to hit the acceleration limit first, just stop jerking when you get there. Then keep an eye on the speed as before.

In all cases, the maximum jerk is applied for as long as possible, as early/late as possible.

As an aside, it's perfectly plausible for a real system to accelerate harder than it can decelerate. Electrical power accelerating the system imparts a bunch of kinetic energy. Unless the controller has regenerative braking, the only place for the K.E. to go is to become heat dissipated in the motor windings or brake, and we hate that smoke. Similarly, pushing the system to be as fast as possible ignores other considerations such as wear, noise, or vibration.


问:
Confusion about Gimbal lock
I am trying to understand exactly what is happening when Gimbal lock occurs. I have read many explanations now which provide the high-level idea, in that Gimbal lock occurs when two axes are aligned during the sequence of three rotations. But I am struggling to understand what this really means in terms of the calculations that are going on.

Here's my understanding of the setup. We have an initial 3D rotation of a body, and a target 3D rotation of that body, and we want to move the body from the initial to the target rotation. To do this, we calculate the Euler angles representing this rotation. (I don't know the maths behind this, but I will assume that this is just an algorithm that takes in an initial and target rotation, and returns a third rotation).

So, we now have a rotation expressed as Euler angles, which we want the body to move over. And now I will explain my confusion. From what I have been reading, there are two ideas:

Gimbal lock occurs when two of the axes become aligned, during the sequence of three rotations, which means that there is one axis about which rotation cannot occur.

Due to Gimbal lock, the path which the rotation occurs over will not be "straight", and there will be a curve.

This to me is very confusing. 1) is saying that Gimbal lock means that we cannot rotate the body along a certain axis. 2) is saying that Gimbal lock means that we can rotate the body, but that the path will not be "straight".

Please can somebody explain which one is correct, and help me to overcome this confusion? Thanks!

答:
Gimbal lock is a specific form of the larger concept of a kinematic singularity when one or more axes of a gimbal become aligned.

A classic gimbal mount has successive semi-circular mounts which can pivot and provide yaw, pitch, and roll degrees of freedom for the gimbal. (If you sort those from outside in yaw, pitch, roll.) If you pitch down, such that the central body is vertical. You will notice that the yaw axis is about vertical, but so is the roll axis, since it rotated 90 degrees down from the X axis. In this configuration if you wanted to rotate the body about the X axis (where the roll joint was originaly pointed) by a little bit, it is impossible because you have two joints vertically aligned along the vertical Z axis (yaw, and formerly roll) and one still along the Y axis.

This is gimbal lock, because your three degrees of freedom system can no longer move in all three directions.

The above explanation is for a physical gimbal. But people refer to this issue as well when using Euler angles because the mathematical construction of Euler angles uses the same representation of sequential yaw, pitch, and roll angle representations. This representation has the same issue as the physical example above.

There's a full description of this on Wikipedia

There are other ways to express this with higher level math such as the jacobian matrix's rank

Specifically to understand the difference that you're seeing between 1) and 2). You could move the vehicle to another position which is close and offset around the axis where you no longer have control. But to get there based on the constraints it won't be a quick move. Aka you will need to move to another position and then back as per the explanation 2). While 1) is saying that you can't move directly towards your goal, which is consistent.

If you're working with simply the math of orientation there are alternative ways to represent position such as quaterions which do not incur these singularities in their representation.

Title: Confusion about Gimbal lock


问:
Unwanted Arduino reconnect: Servo + Arduino + Python (Raspberry Pi)
I am having difficulty sustaining a connection between my Raspberry Pi (Model B running Raspbian) and my Arduino (Uno) while sending signals from the Raspberry Pi to a continuously rotating servo (PowerHD AR- 3606HB Robot Servo) via Python. I'm not sure if there is a more efficent way of sending servo instructions via Python to the Arduino to rotate the servo. I'm attempting to communicate signals from the Raspberry Pi to the Arduino via USB using what I believe is considered a "digital Serial connection". My current connection:

Wireless Xbox 360 Controller
->
Wireless Xbox 360 Controller Receiver
->
Raspberry Pi
->
Externally Powered USB Hub
->
Arduino
->
Servo

```<code>Servo connection to Arduino:
   Signal (Orange) - pin 9
   Power (Red) - +5 V
   Ground (Black) - GND
</code>```

On the Raspberry Pi I have installed the following (although not all needed for addressing this problem):

xboxdrv

pyserial

Python-Arduino-Command-API

PyGame

lego-pi

Arduino

The sketch I've uploaded to the Arduino Uno is the corresponding sketch provided with the Python-Arduino-Command-API. *Again, I'm not positive that this is the best method means of driving my servo from Python to Arduino (to the servo).

From the Raspberry Pi, I can see the Arduino is initially correctly connected via USB:

```<code>pi@raspberrypi ~/Python-Arduino-Command-API $ dir /dev/ttyA*
/dev/ttyACM0  /dev/ttyAMA0
</code>```

and

```<code>pi@raspberrypi ~/Python-Arduino-Command-API $ lsusb
Bus 001 Device 002: ID 0424:9512 Standard Microsystems Corp.
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 001 Device 003: ID 0424:ec00 Standard Microsystems Corp.
Bus 001 Device 004: ID 045e:0719 Microsoft Corp. Xbox 360 Wireless Adapter
Bus 001 Device 005: ID 1a40:0201 Terminus Technology Inc. FE 2.1 7-port Hub
Bus 001 Device 006: ID 0bda:8176 Realtek Semiconductor Corp. RTL8188CUS 802.11n WLAN Adapter
Bus 001 Device 007: ID 046d:c52b Logitech, Inc. Unifying Receiver
Bus 001 Device 008: ID 2341:0043 Arduino SA Uno R3 (CDC ACM)
</code>```

From the Raspberry Pi, I'm able to rotate the servo as a test clockwise for one second, counter-clockwise for one second, then stop the servo, with the following Python script:

```<code>#!/usr/bin/env python
from Arduino import Arduino
import time

board = Arduino(9600, port='/dev/ttyACM0')

board.Servos.attach(9) # Declare servo on pin 9
board.Servos.write(9, 0) # Move servo to full speed, clockwise
time.sleep(1) # Sleep for 1 second
print board.Servos.read(9) # Speed check (should read "0")

board.Servos.write(9, 180)
time.sleep(1)
print board.Servos.read(9) # (Should read "180")

board.Servos.write(9, 90)
print board.Servos.read(9)
board.Servos.detach(9)
</code>```

The output via the Raspberry Pi terminal reads:

```<code>0
180
90
</code>```

Although this only performs full-speed in both direction (as well as the calibrated "stop" speed of 90), I have successfully alternated from a full-speed to slower speeds, for example, going from 0 up to 90 in increments of 10.

From the Raspberry Pi, I'm able to send input from my Xbox controller to drive the servo with a small custom Python script I've created along with xboxdrv (which works flawlessly with other projects I'm doing):

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

from legopi.lib import xbox_read
from Arduino import Arduino

# To catch Ctrl+C
import signal
import sys

# The deadzone within which we ignore inputs, approximately 1/3 of total possible input
DEADZONE = 12000

def signal_handler(signal, frame):
    print "Stopping Wrapper"
    sys.exit(0)

# Capture Ctrl+C so we can shut down nicely
signal.signal(signal.SIGINT, signal_handler)

print "Starting Wrapper"
print "Press Ctrl+C at any time to quit"

board = Arduino(9600, port='/dev/ttyACM0')
board.Servos.attach(9)
board.Servos.write(9, 90)

for event in xbox_read.event_stream(deadzone=DEADZONE):
    print "Xbox event: %s" % (event)

    # If the RB button it's being held, rotate the servo counter-clockwise at full-speed.
    # When the RB button is released, stop the servo.
    if(event.key=='RB'):
        if(event.value>0):
            board.Servos.write(9, 180)
            print board.Servos.read(9)
        else:
            board.Servos.write(9, 90)
            print board.Servos.read(9)
        continue
</code>```

This script runs, and I'm able to control the servo using the RB button on my controller. However, it eventually fails - sometimes after minutes, sometimes after seconds (rapid and intermittent input seemingly having no influence on expediting a crash). Input is no longer read by the script, the terminal comes to a halt, the servo freezes on whatever the last command given was (either spinning endlessly or stopped), and I'm forced to Ctrl + C out of the script. If I check to see if the Arduino is still connected to the Raspberry Pi, it shows that it has reconnected itself to the Raspberry Pi as "ttyACM1" (from /dev/ttyACM0 to /dev/ttyACM1):

```<code>pi@raspberrypi ~/robotarm $ dir /dev/ttyA*
/dev/ttyACM1  /dev/ttyAMA0
</code>```

Why does the Arduino reconnect itself? Is there some other way I should be processing this information? Distance to the wireless Xbox receiver is not a factor as all of these pieces are adjacent to one another for testing purposes. It will prove impossible to use this servo as a wheel for my robot if I'm constantly tending to this issue.

答:
I suspect that your Arduino is reseting, by the fact that the stall current of the motor by the product sheet is $800mA$, and you are using the USB power, then Arduino regulator, to supply the motor.

As you are using the USB/Serial converter to make the connection to the Raspberry Pi, when it resets, it can be creating a new "virtual" serial port on the Raspberry Pi, like you are saying.

The USB maximum supply of current is near $500mA$ in this case, but in reality there's more
than the motor, Arduino, and so on, so its less.

To test it, just disconnect the motor and see if don't reset or create a new "virtual" serial port.

Being the problem (or even if its not) you should power your motor from an external supply capable of suppling the required current under the voltage you want.

The servo was attempting to draw more than 500mA over USB and was triggering the
  Arduino's USB Overcurrent Protection feature. An appropriate power supply for the
  Arduino fixed the issue.

I still suggest you to use other power supply to the motor, or use the same power supply but not use the power from the Arduino to supply the motor. This is due that even with an external power supply the Arduino has an power regulator with a $1000mA$ capacity, and also this could put some noise on the ATMega supply. Of course the power supply should be in the motor voltage supply range.