Your browser was unable to load all of the resources. They may have been blocked by your firewall, proxy or browser configuration.
Press Ctrl+F5 or Ctrl+Shift+R to have your browser try again.

krishauser / Klampt

Kris' Locomotion and Manipulation Planning Toolkit

Calculation of impact between object and robot? #9

paperstiger ·
#1

I am doing a simulation task where in the first a few seconds, an object has to fall on a plate held by the robot from a very short height (a few cm). However, it seems that the calculation of impact is not correct. After that it is giving weird errors. The logs are

ODESimulator: ODESimulator: Rigid object box energy 5.67279 exceeds threshold 3
ODESimulator: ODESimulator: Rigid object box energy 7674.79 exceeds threshold 3.00246
ODESimulator: ODESimulator: Robotur5 energy 3754.87 exceeds threshold 37.9606
ODESimulator: ODECustomMesh: Triangles penetrate margin 0.0025+0.0025: can't trust contact detector
ODESimulator: ODESimulation: rolling back due to new penetration between bodies plate_link and box
ODESimulator: previously had margin 0.00391427
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 0.0005
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 0.00025
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 0.000125
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 6.25e-05
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 3.125e-05
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 1.5625e-05
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 7.8125e-06
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 3.90625e-06
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 1.95313e-06
ODESimulator: ODESimulation: Rolling back at time 0.303 time step halved to 9.76563e-07
ODESimulator: ODESimulation: Rollback rejected because timestep 9.76563e-07 below minimum threshold 1e-06
ODESimulation: collision plate_link - box changed from no contact to margin 0
ODESimulation: Adaptive sub-step of size 9.76563e-07 is valid, arriving at time 0.303.
ODESimulator: reset time step to 0.000999023
ODESimulator: ODESimulation: Adaptive time step done, arrived at time 0.304
ODESimulator: ODESimulator: Rigid object box energy 3.28698e+08 exceeds threshold 11515.2
ODESimulator: ODESimulator: Robotur5 energy 1.78854e+08 exceeds threshold 5653.5
ODESimulator: ODERobot::SetVelocities: Error, Get/SetVelocities don't match
ODESimulator: dq = 9 0 -20.835 -13.0784 -2.97687 114.474 -1.16815 -13.0039 0 0
ODESimulator: from GetVelocities = 9 0 -20.8349 -3.37868 -1.17843 104.468 -1.03439 -11.93 0 0
ODESimulator: Error: 14.0929
ODESimulator: did you remember to set the robot's configuration?
Press enter to continue...
^CODESimulator: ODESimulation: rolling back due to new penetration between bodies Terrain4 and shoulder_link
ODESimulator: no previous contact
ODESimulator: ODESimulation: Rolling back at time 0.304 time step halved to 0.0005
ODESimulator: ODESimulation: Rolling back at time 0.304 time step halved to 0.00025
ODESimulator: ODESimulation: Rolling back at time 0.304 time step halved to 0.000125
ODESimulator: ODESimulation: Rolling back at time 0.304 time step halved to 6.25e-05
ODESimulator: ODESimulation: Rolling back at time 0.304 time step halved to 3.125e-05
ODESimulation: collision Terrain4 - shoulder_link erased entirely
ODESimulation: Adaptive sub-step of size 3.125e-05 is valid, arriving at time 0.304.
ODESimulator: reset time step to 0.00096875
ODESimulator: ODESimulation: rolling back due to new penetration between bodies Terrain4 and shoulder_link
ODESimulator: no previous contact
ODESimulator: ODESimulation: Rolling back at time 0.304031 time step halved to 0.000484375
ODESimulator: ODESimulation: Rolling back at time 0.304031 time step halved to 0.000242187
ODESimulation: collision Terrain4 - shoulder_link erased entirely
ODESimulation: Adaptive sub-step of size 0.000242187 is valid, arriving at time 0.304031.
ODESimulator: reset time step to 0.000726562
ODESimulator: ODESimulation: Adaptive time step done, arrived at time 0.305
ODESimulator: ODESimulator: Rigid object box energy 3.28698e+08 exceeds threshold 100000
ODESimulator: ODESimulator: Robotur5 energy 9.32313e+08 exceeds threshold 100000
ODESimulator: ODERobot::SetVelocities: Error, Get/SetVelocities don't match
ODESimulator: dq = 9 0 -158.663 -263.492 259.827 -338.916 -84.5102 251.178 0 0
ODESimulator: from GetVelocities = 9 0 -158.548 180.752 123.009 -123.765 34.5131 -201.118 0 0
ODESimulator: Error: 693.614
ODESimulator: did you remember to set the robot's configuration?
Press enter to continue...

The problem setup looks like this

Untitled.png

If I want to avoid this impact and just want to let the object rest on the plate, what can I do?

  • replies 5
  • views 6.2K
  • likes 1
krishauser · Team
#2

This type of thing happens usually when the PID controller is going unstable, or when the object is already penetrating through the robot upon simulator initialization.

Can you attach the code, robot file, and object file that generates this error?

You might play with the object mass and the link’s mass to see if anything changes. You might also check the output of sim.getStatus().

1 likes
krishauser · Team
#3

I'm pretty sure this a problem with the PID parameters not being tuned well. If you open this in SimTest, you can see the robot vibrating slightly after you move it or apply a force.

paperstiger · Author
#4

Thanks for your reply. I played with object mass and somehow avoided this issue. Previously the mass is 3 kg and now I change it to 0.1 kg. I also tuned PID to make them smaller.

paperstiger · Author
#5

More on this issue. When I am recording video, it seems that the video recording suddenly stops if ODESimulator raise warning information. Besides, can the default ffmpeg command to include -pix_fmt yuv420p? Moreover, in my simulation some weird behavior occurs such as the object jumps up for no reason. A video is here https://youtu.be/xC2SvfWh3q8 showing this behavior. Now my object has mass 0.1 and PID parameters for robots are like (P: 100 200 200 20 15 25, I: 0, D: 20 8 10 5 5 5)

krishauser · Team
#6

Just to follow up: I discussed this issue in person, and the problem was apparently that the URDF file had links whose inertia matrices were on the order of 10^-8. This made simulation very numerically sensitive.

A second issue, which manifests itself as popping and visual "tearing", has to do with multithreading. The visualization starts a second thread, and if you are visualizing the same world model as the simulator is using, then the memory is actually shared. The vis module's updates to the robot configuration in the visualization thread conflicts with the simulator's use of the robot configuration in the main thread. There are two ways around this:

Put locks around the simulation loop

while vis.shown()
  vis.lock()
  ...do simulation
  vis.unlock()
  time.sleep(...)

Make a copy of the world

visworld = world.copy()
vis.add("world",visworld)
while vis.shown()
  ...do simulation
  vis.lock()
  visworld.robot(0).setConfig(world.robot(0).getConfig()
  vis.unlock()
  time.sleep(...)

Note that the world copy consumes almost no resources. You could also copy the vis world into the simulator, it doesn't matter.

The benefit of this approach is that the visualization stays a bit more responsive since it is not locked while the simulator is running.