Selfcollisions freezejointpenalty

General description

This change detects and penalizes self collisions in agents. It works by freezing joints involved in a self collision for SelfCollisionJointFrozenTime, and then unfreezes the joints and lets them be controlled again before being potentially flagged for another self collision for SelfCollisionJointThawTime. Each joint is monitored individually such that only those joints involved in a self collision can be specified as needing to be frozen. Currently all joints in an arm or leg are frozen if any box collider in that arm or leg is involved in a self collision. The penalty for a self collision can be changed to beaming an agent if desired. Additionally an output file can be specified to write self collision info to.

Parameters

The following parameters in naosoccersim.rb that can be adjusted:

// maximum tolerance allowed for self collisions along the box collider separating planes 
SelfCollisionsTolerance;

// print a line every time a self collision is detected in an agent 
PrintSelfCollisions

// apply foul penalty every time self collision is detected in an agent 
FoulOnSelfCollisions

// how long joints are frozen after a self collision
SelfCollisionJointFrozenTime

// time after a joint is unfrozen before it can be frozen by a self collision again
SelfCollisionJointThawTime

// use beaming to penalize self collisions
SelfCollisionBeamPenalty;

// time after a selfcollision beam penalty has been applied in which no self collision beam penalty will be applied again
SelfCollisionBeamCooldownTime

// write self collisions to file
WriteSelfCollisionsToFile

// Filename where self collisions are written */
SelfCollisionRecordFilename