Source code for klampt.sim.settle

from ..robotsim import *
from ..math import vectorops,so3,se3
import random
from .. import vis
import time
import math
from ..model.trajectory import Trajectory
from import ContactPoint

[docs]def settle(world,obj, forcedir=(0,0,-1),forcept=(0,0,0), settletol=1e-4,orientationDamping=0.0, perturb=0,margin=None, debug=False): """Assuming that all other elements in the world besides object are frozen, this "settles" the object by applying a force in the direction forcedir and simulating until the object stops moving. An exception is raised if the object is already colliding with the world. Args: world (WorldModel): the world containing other static and moving objects obj: a RigidObjectModel, RobotModelLink, or floating-base Robot that will be settled. forcedir (list of 3 floats, optional): a vector parallel to the direction of force whose magnitude is the maximum distance this procedure will try to move the object. forcept (list of 3 floats, optional): local coordinates of the center of force application. settletol (float, optional): the simulation will stop when two subsequent transforms lie within this tolerance. orientationDamping (float, optional): a virtual spring will attempt to keep the initial orientation with this torsional spring constant perturb (float, optional): if nonzero, the application force will be perturbed at random by this amount every step. If equal to 1, this means the force is sampled from a 45 degree cone in the direction forcedir. margin (float, optional): the collision detection margin used in simulation. If None, uses the Simulator default. Otherwise, overrides the default. Must be at least settletol. debug (bool, optional): if True, uses the visualization to debug the settling process Returns: tuple: A pair (transform,touched) with: - transform (se3 transform): The resulting se3 transform of the object, or None if the object didn't hit anything by the time it translated by ||forcedir|| units. - touched (dict): a dictionary whose keys are object IDs touched by the object at the final transform, and whose values are lists of ContactPoints (see :mod:``) giving the contacts between obj and the touched object. To convert the result to a hold, call:: h = Hold() h.setFixed(obj,sum(touched.values(),[])) """ assert isinstance(world,WorldModel) if isinstance(obj,(str,int)): obj = world.rigidObject(obj) assert obj.index >= 0,"Object "+str(obj)+" does not exist in world" elif isinstance(obj,RobotModel): raise NotImplementedError("TODO: settle free-floating robots") elif isinstance(obj,RobotModelLink): if world.index != raise ValueError("Object is not present in the given world") assert obj.robotIndex >= 0 and obj.robotIndex < world.numRobots() robot = world.robot(obj.robotIndex) assert obj.index >= 0 and obj.index < robot.numLinks() newWorld = WorldModel() for i in range(world.numRobots()): if i == obj.robotIndex: continue newWorld.add(world.robot(i).getName(),world.robot(i)) for i in range(world.numRigidObjects()): newWorld.add(world.rigidObject(i).getName(),world.rigidObject(i)) for i in range(world.numTerrains()): newWorld.add(world.terrain(i).getName(),world.terrain(i)) newObj = newWorld.makeRigidObject("obj") newObj.geometry().set(obj.geometry()) newObj.setMass(obj.getMass()) #newObj.setContactParameters(obj.getContactParameters()) newObj.setTransform(*obj.getTransform()) return settle(newWorld,newObj, forcedir,forcept, settletol,orientationDamping, perturb,margin,debug) elif isinstance(obj,RigidObjectModel): pass else: raise ValueError("Invalid object type given, only supports RigidObjectModels, RobotModelLinks, and RobotModels") #get a bounding box around the object forcept_world = se3.apply(obj.getTransform(),forcept) bmin,bmax = obj.geometry().getBB() #compute radius about forcept, expand BB due to potential for orientation change R = 0 for i in range(3): R += pow(max(forcept_world[i]-bmin[i],bmax[i]-forcept_world[i]),2) R = math.sqrt(R) bmin = [x - R for x in forcept_world] bmax = [x + R for x in forcept_world] #expand BB about force direction for i in range(3): if forcedir[i] < 0: bmin[i] += forcedir[i] else: bmax[i] += forcedir[i] if world.index != raise ValueError("Object is not present in the given world") assert obj.index >= 0 and obj.index < world.numRigidObjects() #exclude objects that have no chance of being in way of object newWorld = WorldModel() newObj = None for i in range(world.numRobots()): robot = world.robot(i) for j in range(robot.numLinks()): link = if _bboverlap((bmin,bmax),link): #add robot link as static geometry newObj = newWorld.makeRigidObject(robot.getName()+":"+link.getName()) newObj.geometry().set(link.geometry()) mass = Mass() mass.setMass(float('inf')) mass.setCom([0]*3) mass.setInertia([float('inf')]*3) newObj.setMass(mass) newObj.setTransform(*link.getTransform()) #TODO: what surface properties? for i in range(world.numRigidObjects()): if _bboverlap((bmin,bmax),world.rigidObject(i)): o = newWorld.add(world.rigidObject(i).getName(),world.rigidObject(i)) if i == obj.index: newObj = o else: assert i != obj.index for i in range(world.numTerrains()): if _bboverlap((bmin,bmax),world.terrain(i)): newWorld.add(world.terrain(i).getName(),world.terrain(i)) world = newWorld obj = newObj movedist = vectorops.norm(forcedir) if movedist < settletol: print("sim.settle(): warning, force movement distance less than settletol. Was this intended?") return (obj.getTransform(),[]) forcedir = vectorops.div(forcedir,movedist) forceamt = obj.getMass().mass sim = Simulator(world) body = sim.body(obj) otherbodies = [] otherids = [] for i in range(world.numRigidObjects()): otherids.append(world.rigidObject(i).getID()) otherbodies.append(sim.body(world.rigidObject(i))) for i in range(world.numTerrains()): otherids.append(world.terrain(i).getID()) otherbodies.append(sim.body(world.terrain(i))) otherids.remove(obj.getID()) otherbodies = [b for b in otherbodies if b.body != body.body] if len(otherbodies) == 0: print("sim.settle(): no objects in direction",vectorops.mul(forcedir,movedist)) return (None,[]) if margin != None: assert margin >= settletol,"Collision margin must be at least settletol" for b in otherbodies: b.setCollisionPadding(0) body.setCollisionPadding(margin) else: margin = body.getCollisionPadding() margin += min([b.getCollisionPadding() for b in otherbodies]) #set up simulation dt = max(settletol,margin*0.5) forceamt /= dt*0.5 sim.setGravity((0,0,0)) sim.setSimStep(dt) for id in otherids: sim.enableContactFeedback(obj.getID(),id) #turn off all restitution s = body.getSurface() s.kRestitution = 0 body.setSurface(s) for b in otherbodies: s = b.getSurface() s.kRestitution = 0 b.setSurface(s) #disable other bodies for b in otherbodies: b.setVelocity([0.0]*3,[0.0]*3) b.enableDynamics(False) body.setVelocity([0.0]*3,vectorops.mul(forcedir,movedist)) sim.simulate(0) s = sim.getStatus() if s == Simulator.STATUS_CONTACT_UNRELIABLE: print("sim.settle(): warning, object already penetrating other objects. Trying to pull back...") T0 = body.getTransform() body.setTransform(T0[0],vectorops.madd(T0[1],forcedir,margin)) sim.simulate(0) s = sim.getStatus() if s == Simulator.STATUS_CONTACT_UNRELIABLE: print(" pulling back failed.") return (None,[]) if debug: vis.createWindow("settle") vis.add("world",world) time.sleep(1.0) springanchorworld = se3.apply(obj.getTransform(),forcept) Rspringanchor = obj.getTransform()[0] numSettled = 0 Told = body.getTransform() t = 0 while t < 1: #print("Simulating, t =",t) if perturb: fpert = (random.gauss(0,perturb),random.gauss(0,perturb),random.gauss(0,perturb)) fpert = vectorops.sub(fpert,vectorops.mul(forcedir,,fpert))) f = vectorops.add(forcedir,fpert) else: f = forcedir springanchorbody = se3.apply(body.getObjectTransform(),forcept) Rspringbody = body.getObjectTransform()[0] #vis.add("Tobject",body.getObjectTransform()) kSpring = forceamt #kSpring = 0 f = vectorops.madd(vectorops.mul(f,forceamt),vectorops.sub(springanchorworld,springanchorbody),kSpring) body.applyForceAtLocalPoint(f,forcept) if orientationDamping > 0: #local orientation change: transform from body to anchor frame wlocal = so3.moment(so3.mul(Rspringanchor,so3.inv(Rspringbody))) #world orientation spring w = so3.apply(Rspringbody,wlocal) #w = so3.apply(Rspringbody,so3.error(Rspringanchor,Rspringbody)) #vis.add("orientation",Trajectory([0,1],[Told[1],vectorops.madd(Told[1],w,1)])) #vis.add("orientationloc",Trajectory([0,1],[Told[1],vectorops.madd(Told[1],wlocal,1)])) #vis.setColor("orientationloc",0,1,0) #body.applyWrench([0]*3,vectorops.mul(w,orientationDamping)) body.applyWrench([0]*3,vectorops.mul(wlocal,orientationDamping)) if debug: vis.lock() sim.simulate(dt) sim.updateWorld() vis.unlock() time.sleep(0) time.sleep(0.1) else: sim.simulate(dt) sim.updateWorld() #test for settling w,v = body.getVelocity() T = body.getTransform() err = se3.error(T,Told) #if debug: # print("Status:",sim.getStatus(),"velocity",w,v,"error",vectorops.norm(err)) if vectorops.norm(err) < settletol: numSettled += 1 else: numSettled = 0 if numSettled >= 2: print("sim.settle(): Settled at time",t) touched = [id for id in otherids if sim.inContact(obj.getID(),id)] cps = [sim.getContacts(obj.getID(),id) for id in touched] tdict = dict() for id,cplist in zip(touched,cps): tdict[id] = [ContactPoint(ci[0:3],ci[3:6],ci[6]) for ci in cplist] if debug: return (body.getObjectTransform(),tdict) #apply drag body.setVelocity(vectorops.mul(w,0.8),vectorops.mul(v,0.8)) Told = T springanchorworld = vectorops.madd(springanchorworld,forcedir,dt*movedist) t += dt if debug: print("Failed to settle? Final velocity",body.getVelocity()) touched = [id for id in otherids if sim.inContact(obj.getID(),id)] cps = [sim.getContacts(obj.getID(),id) for id in touched] tdict = dict() for id,cplist in zip(touched,cps): tdict[id] = [ContactPoint(ci[0:3],ci[3:6],ci[6]) for ci in cplist] return (body.getObjectTransform(),tdict)
def _bboverlap(bb,element): if isinstance(element,RobotModel): return any(_bboverlap(bb, for i in range(element.numLinks())) else: bb2 = element.geometry().getBB() print("BBox",bb) print(" Testing overlap with",element.getName(),"bbox",bb2) for (a,b,c,d) in zip(bb[0],bb[1],bb2[0],bb2[1]): if not (a >= c and a <= d or b >= c and b <= d) and not (c >= a and c <= b or d >= a and d <= b): print(" No overlap") return False print(" Overlap") return True