I've added a Box obstacle to my MultibodyPlant using a slight variation of the AddShape() function given here. I've welded the obstacle to the plant using WeldFrames(); the obstacle also shows up in the correct place in the world when I visualize it.
However, DirectCollocation doesn't respect the obstacle when I add a MinimumDistanceConstraint, and produces a trajectory which collides with the obstacle.
Here's how I create my plant, scene_graph, and diagram.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = FindResource('models/undamped_cartpole.urdf')
Parser(plant).AddModelFromFile(urdf_path)
# add obstacles
# this function internally calls RegisterCollisionGeometry and
# RegisterVisualGeometry on the plant
plant = add_obstacles_plant(plant, obstacles)
plant.Finalize()
diagram = builder.Build()
Here's the relevant code snippet for DirectCollocation.
input_port_index = builder.ExportInput(plant.get_actuation_input_port(), "input")
dircol = DirectCollocation(
diagram, diagram.CreateDefaultContext(),
num_time_samples=num_time_samples,
minimum_timestep=0.1,
maximum_timestep=0.4,
input_port_index=input_port_index
)
# collision avoidance constraints
collision_constraint = MinimumDistanceConstraint(plant, 0.1,
diagram.GetMutableSubsystemContext(plant, diagram_context))
dircol.AddConstraintToAllKnotPoints(constraint=collision_constraint, vars=dircol.state()[:2])
Am I passing in the wrong context to MinimumDistanceConstraint / misunderstood what MinimumDistanceConstraint does?
So turns out the cartpole URDF model I was using didn't have collision geometries defined for its components. It seems AutoDiff works best with Sphere collisions, so I made a rough approximation of my model with Spheres (eg: constructed the Box cart as two Spheres placed side by side, the Cylinder pole as many tiny Spheres placed in a line), and wrote those collision geometries out to my URDF.
If interested, I generated the line of tiny Spheres using a small string generation script.
sphere_str = '<collision> <origin xyz="0 0 {s_o:.2f}"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>'
for i in range(50):
print(col_str.format(s_o=-0.01 + i*-0.02))
This prints out:
<collision> <origin xyz="0 0 -0.01"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>
<collision> <origin xyz="0 0 -0.03"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>
<collision> <origin xyz="0 0 -0.05"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>
...
<collision> <origin xyz="0 0 -0.99"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>
Which I then pasted into my URDF.
Adding the Sphere collision geometries into my URDF solved my problem and DirectCollocation started respecting the MinimumDistanceConstraint.
Related
I am setting up a simulation with some revolute and prismatic joints (fairly simple robot arm), and I was wondering if there is a straightforward way to add friction to the arm's joints?
Something simple like a coefficient of static friction and a coefficient of kinetic friction, and switching between the two based on the joint's velocity.
In the sim I just have gravity as an external force and then a PID controller controlling the joint positions and velocities. The way I see it, I could add friction to the joints by either:
Hacking together a torque to apply by looking at the gravity and actuation forces at each time step
Writing a new joint class that inherits from the linear/revolute joint classes and add in some sort of friction
Do either of these seem like the best way to do this? I'm hoping there's an easier way I am missing.
Thanks!
The joints (e.g., RevoluteJoint) already offer damping:
Viscous damping coefficient, in N⋅m⋅s, used to model losses within the joint. The damping torque (in N⋅m) is modeled as τ = -damping⋅ω, i.e., opposing motion, with ω the angular rate for this joint.
It can be set via the API or in SDFormat, e.g.,
<joint name="revolute_joint" type="revolute">
<child>link2</child>
<parent>link1</parent>
<axis>
<xyz expressed_in="__model__">0 0 1</xyz>
<limit>
<lower>-1</lower>
<upper>2</upper>
<effort>100</effort>
<velocity>100</velocity>
<drake:acceleration>200</drake:acceleration>
</limit>
<dynamics>
<damping>0.2</damping>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
Is that a close enough match for what you need?
For revolute joints, what is needed is a frictional torque 𝛕 that depends on |𝐅ₙ| (the magnitude of a force on the joint), a material-dependent coefficient of friction μₖ, and an effective radius rₑ. During sliding, the direction of the frictional torque 𝛕 is opposite the relevant angular velocity vector 𝛚. To avoid a divide by zero problem when 𝛚 = 𝟎, add an epsilon ε that is a small positive number that represents a very small |𝛚| (magnitude of angular velocity).
𝛕 = -μₖ * rₑ * |𝐅ₙ| * 𝛚 / (|𝛚| + ε)
Note: Sometimes the coefficient of friction μₖ and effective radius rₑ are combined into a single constant.
Note: I do not unambiguously understand the SDF spec. I can guess, but that seems somewhat antagonist to a specification.
http://sdformat.org/spec?ver=1.9&elem=joint#axis_dynamics
I'm currently working on a project that needs a bit of friction to run the real environment.
But, and it's why i'm here, I have problems.
I'm trying to put a stick on a pad, and I want them to be a bit sticky together, and have some friction between them before the stick falls off.
I precise that i'm new to Gazebo, and i'm open to all of your advices.
Here is the example, were the green pad surface has a friction of mu and mu2 around 10000 (for testing), same for the sphere.
I tried to change the visual and collision geometry to see if the problem came from here, without effect.
There is three frames to illustrate the slipperiness of these surface:
Straight on pad
Begin to fall
Completely out
I performed some tests to understand the differents parameters (such as mu and mu2 here).
The ramp has these parameters for the first test :
mu : 100
mu2: 50
The ramp has these parameters for the second test :
mu : 0.0
mu2: 0.0
I think that these parameters vary between 0 and 1, but when I create a world, the ground has these values.
In each case, cylinders have, in the order, these parameters :
mu : 0.0
mu2: 0.0
mu : 0.1
m2 : 0.1
mu : 0.3
mu2: 0.3
mu : 0.5
mu2: 0.5
mu : 0.8
mu2: 0.8
mu : 1.0
mu2: 1.0
Here, the picture of the first test, when cylinders roll on the ramp :
Test1
Here, the 2 pictures of the second test, the first one shows the cylinders rolling down the ramp, and the second one shows what happens after, on the ground :
Test2_1
Test2_2
In the idea, the result will show that the first cylinder will roll slower than the second one, the second one slower than the third and so one and so one.
However, this is not happened.
These examples show that the lower mu is taken between two object, but why, for the first test, there is no difference for the cylinders between 3 and 6 ?
Plus, how to simulate the friction for my real application ?
I thank you in advance !
Have a nice day !
Apple's SceneKit documentation suggests that the transform matrix of an object consists of rotation, position, and scale information. However the transform matrix is a 4x4 matrix, the last column being 0,0,0,1. What are these values exactly and is there a more detailed explanation to this matrix? Like which columns/rows represent what, why is there 4 rows and what is the last column for?
Example code:
for t in 0...3 {
print("\t")
for n in frame.camera.transform[t] {
print(String(format: "%10.1f", n),terminator: "");
}
}
Output:
0.1 -0.7 0.7 0.0
1.0 0.2 -0.1 0.0
-0.1 0.7 0.7 0.0
0.3 -0.1 0.0 1.0
I'm pretty sure this is the CATransform3D:
https://developer.apple.com/documentation/quartzcore/catransform3d
Which is some of the most asinine "documentation" in the history of Apple's notoriously asinine documentation.
Try this, from the wayback machine, when they used to at least talk about things a little more deeply... somewhat: https://web.archive.org/web/20111010140734/http://developer.apple.com/library/mac/#/web/20111012014313/https://developer.apple.com/library/mac/documentation/Cocoa/Conceptual/CoreAnimation_guide/Articles/Layers.html
m34 is the most interesting of the entire matrix, as it's usually referred to as being responsible for perspective.
And here's one of the best articles ever written about Core Animation, that explains some aspects of this transform: http://www.thinkandbuild.it/introduction-to-3d-drawing-in-core-animation-part-1/
I try to simulate behavior of different types of balls (football ball, tennis, golf, volleyball etc). For these purposes I tried to change density, mass, size parameters of the physics body.
In my game it doesn't have effect and I tried to simulate it in very simple app consists from ball and paddle
the ball always rises to the same height with the same speed regardless of mass, density, size
Is this because of restitution equals 1? How can I simulate behavior of different ball types in this way?
you need to change the restitution property as well as the Lin. Dampening and Ang. Dampening per ball
for my example i have
tennis ball - rest 0.6 lin 0.2 ang 0.2
soccer ball - rest. 0.5 lin 0.3 ang 0.2
golf ball - rest 0.8 lin 0.3 ang 0.3
shot put - rest 0.1 lin 0.1 ang 0.1
here is a link to the Apple definitions of the properties
https://developer.apple.com/documentation/spritekit/skphysicsbody/1519906-mass
All the docs I've seen for the ArcTo instruction for a SVG <path/>'s d attribute give the first two arguments as the x and y radius of the arc.
Earlier, though, I was playing around, and in FF8 and Safari 5, it seemed like the path
<path d="M 100 100 A 50 50 0 0 0 200 100 Z"/>
and the path
<path d="M 100 100 A 1 1 0 0 0 200 100 Z"/>
rendered identically. From a bit more playing it seemed like what was really being used was the ratio between rx and ry. This makes sense (since what else are you going to do if the current position is more than 2r away), but is it officially documented anywhere?
It'd be nice if I could rely on this behaviour so I didn't have to manually calculate the x and y radius and instead just state their ratio.
Per the SVG specification: If rx, ry are such that there is no solution (basically, the ellipse is not big enough to reach from (x1, y1) to (x2, y2)) then the ellipse is scaled up uniformly until there is exactly one solution (until the ellipse is just big enough).