fromabcimportABCMeta,abstractmethodfrom...constantsimportG_VECTORfrom...lib.pydispatchimportdispatcherfrom...model.collisionimportode_adapterascollfrom...model.physicsimportode_adapterasphsfrom...model.robotimportjointsasjofrom...utilsimportgeometryasgemutfrom...utilsimportmathematicalasmufrom.importsignalsclassSimulation(object):def__init__(self,FPS,STEPS_PF):self._FPS=FPSself._DT=1.0/FPSself._STEPS_PF=STEPS_PF# steps per frameself.paused=Falseself._sim_time=0.0self.num_iter=0self.num_frame=0self._contact_group=Noneself._world=Noneself._space=Noneself._floor_geom=Noneself._objects={}self._joints={}# storage of functions to be called on each step of a specific frame# e.g. addTorqueself.all_frame_steps_callbacks=[]self.coll_engine=coll.Engine()self.phs_engine=phs.Engine()self.graph_adapter=None# perhaps visualization is a better suited namedefadd_basic_simulation_objects(self,gravity=G_VECTOR):"""Create the basic simulation objects needed for physics and collision such as a contact group (holds temporary contact joints generated during collisions), a simulation 'world' (where physics objects are processed) and a collision space (the same thing for geoms and their intersections). :param gravity: Gravity acceleration. :type gravity: 3 floats tuple. """self._contact_group=coll.ContactGroup()self._world=self.phs_engine.world_class(gravity=gravity)self._space=coll.Space()defon_idle(self):self.num_frame+=1# before each visualization frametry:dispatcher.send(signals.SIM_PRE_FRAME,sender=self)exceptExceptionase:print(e)self.perform_sim_steps_per_frame()# after each visualization frametry:dispatcher.send(signals.SIM_POST_FRAME,sender=self)exceptExceptionase:print(e)# clear functions registered to be called in the steps of this past frameself.all_frame_steps_callbacks=[]self.update_actors()defperform_sim_steps_per_frame(self):time_step=self.time_stepforiinrange(self._STEPS_PF):# before each integration step of the physics enginetry:# send the signal so subscribers do their stuff in timedispatcher.send(signals.SIM_PRE_STEP,sender=self)# call all registered functions before each step in next frameforcallback_inself.all_frame_steps_callbacks:callback_()exceptExceptionase:print(e)# detect collisions and create contact jointsargs=coll.NearCallbackArgs(self._world,self._contact_group)self._space.collide(args)# step physics world simulationself._world.step(time_step)self._sim_time+=time_stepself.num_iter+=1# remove all contact jointsself._contact_group.empty()# after each integration step of the physics enginetry:dispatcher.send(signals.SIM_POST_STEP,sender=self)exceptExceptionase:print(e)defupdate_actors(self):"""Update pose of each simulated object's corresponding actor."""forkey_inself._objects:try:ifself._objects[key_].is_updatable():self._objects[key_].update_actor()except(ValueError,AttributeError)aserr:print(err)@propertydeftime_step(self):returnself._DT/self._STEPS_PF@propertydefsim_time(self):returnself._sim_time@propertydefgravity(self):returnself._world.gravity@propertydefcollision_space(self):returnself._spacedefadd_axes(self):try:gAxes=self.graph_adapter.Axes()exceptAttributeError:gAxes=Nonename='axes'returnself.add_object(SimulatedObject(name,actor=gAxes))defadd_floor(self,normal=(0,1,0),dist=0,box_size=(5,0.1,5),box_center=(0,0,0),color=(0.2,0.5,0.5)):"""Create a plane geom to simulate a floor. It won't be used explicitly later (space object has a reference to it)"""# FIXME: the relation between ODE's definition of plane and the center# of the boxself._floor_geom=coll.Plane(self._space,normal,dist)# TODO: use normal parameter for orientationtry:gFloor=self.graph_adapter.Box(box_size,box_center)gFloor.set_color(color)exceptAttributeError:gFloor=Nonename="floor"# TODO: why SimulatedObject? See 'add_trimesh_floor'returnself.add_object(SimulatedObject(name,actor=gFloor))defadd_trimesh_floor(self,vertices,faces,center=(0,0,0),color=(0.2,0.5,0.5)):self._floor_geom=coll.Trimesh(self._space,vertices,faces)self._floor_geom.set_position(center)try:gFloor=self.graph_adapter.Trimesh(vertices,faces,center)gFloor.set_color(color)exceptAttributeError:gFloor=Nonename='floor'# TODO: why SimulatedBody? See 'add_floor'returnself.add_object(SimulatedBody(name,actor=gFloor))defadd_sphere(self,radius,center,mass=None,density=None):body=phs.Sphere(self._world,self._space,radius,mass,density)body.set_position(center)try:g_sphere=self.graph_adapter.Sphere(radius,center)exceptAttributeError:g_sphere=Nonename='sphere'returnself.add_object(SimulatedBody(name,body,g_sphere))defadd_box(self,size,center,mass=None,density=None):body=phs.Box(self._world,self._space,size,mass,density)body.set_position(center)try:g_box=self.graph_adapter.Box(size,center)exceptAttributeError:g_box=Nonename='box'+str(center)# FIXMEreturnself.add_object(SimulatedBody(name,body,g_box))defadd_cylinder(self,length,radius,center,mass=None,density=None):body=phs.Cylinder(self._world,self._space,length,radius,mass,density)body.set_position(center)try:g_cylinder=self.graph_adapter.Cylinder(length,radius,center)exceptAttributeError:g_cylinder=Nonename='cylinder'returnself.add_object(SimulatedBody(name,body,g_cylinder))defadd_capsule(self,length,radius,center,mass=None,density=None):body=phs.Capsule(self._world,self._space,length,radius,mass,density)body.set_position(center)try:g_capsule=self.graph_adapter.Capsule(length,radius,center)exceptAttributeError:g_capsule=Nonename='capsule'returnself.add_object(SimulatedBody(name,body,g_capsule))@propertydefactors(self):"""Return a dict with each object actor indexed by the object name."""actors={}forkey_inself._objects:actor=self._objects[key_].actorifactor:actors[key_]=actorreturnactorsdefadd_object(self,sim_object):"""Add ``sim_object`` to the internal dictionary of simulated objects. If its name equals an already registered key, it will be modified using its string representation, for example: >>> add_object(sim_object) sphere/<ars.model.simulator.SimulatedBody object at 0x3a4bed0> :param sim_object: object to add :type sim_object: :class:`SimulatedObject` :return: name/key of the object :rtype: string """name=sim_object.get_name()if(nameinself._objects.keys())andname:name=name+'/'+str(sim_object)# TODOsim_object.set_name(name)self._objects[name]=sim_objectreturnnamedefadd_joint(self,sim_joint):name=sim_joint.get_name()if(nameinself._joints.keys())andname:name=name+'/'+str(sim_joint.joint)# TODOsim_joint.set_name(name)self._joints[name]=sim_jointreturnname@propertydefobjects(self):returnself._objectsdefget_object(self,name):returnself._objects[name]@propertydefjoints(self):returnself._jointsdefget_joint(self,name):returnself._joints[name]# TODO: change to property, analogous to :meth:`actors`defget_bodies(self):"""Return a list with all the bodies included in the simulation. :return: list of :class:`SimulatedBody` objects """bodies=[]forkey,objinself._objects.iteritems():# Not all objects are SimulatedBody instances wrapping# a physical instance, that's why we check ``obj.body``.try:body=obj.bodyexceptAttributeError:body=Noneifbody:# Note that ``obj`` is appended, not ``body`` which was only# retrieved to check it contained a valid physical bodybodies.append(obj)returnbodies#==========================================================================# Add joints#==========================================================================defadd_fixed_joint(self,obj1,obj2):body1=obj1.bodybody2=obj2.bodyf_joint=jo.Fixed(self._world,body1,body2)returnself.add_joint(SimulatedJoint(joint=f_joint))defadd_rotary_joint(self,name,obj1,obj2,anchor,axis):"""Adds a rotary joint between obj1 and obj2, at the specified anchor and with the given axis. If anchor is None, it will be set equal to the position of obj2"""body1=obj1.bodybody2=obj2.bodyifnotanchor:anchor=obj2.get_position()r_joint=jo.Rotary(self._world,body1,body2,anchor,axis)returnself.add_joint(SimulatedJoint(name,r_joint))defadd_universal_joint(self,obj1,obj2,anchor,axis1,axis2):body1=obj1.bodybody2=obj2.bodyu_joint=jo.Universal(self._world,body1,body2,anchor,axis1,axis2)returnself.add_joint(SimulatedJoint(joint=u_joint))defadd_ball_socket_joint(self,name,obj1,obj2,anchor):"""Adds a "ball and socket" joint between obj1 and obj2, at the specified anchor. If anchor is None, it will be set equal to the position of obj2. """body1=obj1.bodybody2=obj2.bodyifnotanchor:anchor=obj2.get_position()bs_joint=jo.BallSocket(self._world,body1,body2,anchor)returnself.add_joint(SimulatedJoint(name,bs_joint))defadd_slider_joint(self,name,obj1,obj2,axis):"""Add a :class:`jo.Slider` joint between ``obj1`` and ``obj2``. The only movement allowed is translation along ``axis``. :return: the name under which the slider was stored, which could be different from the given ``name`` """body1=obj1.bodybody2=obj2.bodys_joint=jo.Slider(self._world,body1,body2,axis)returnself.add_joint(SimulatedJoint(name,s_joint))classSimulatedObject(object):__metaclass__=ABCMeta_updatable=Falsedef__init__(self,name,actor=None):ifname:self._name=nameelse:self._name=str(self)self._actor=actor#==========================================================================# Getters and setters#==========================================================================defget_name(self):returnself._namedefset_name(self,name):self._name=name@propertydefactor(self):returnself._actordefhas_actor(self):returnnotself._actorisNonedefis_updatable(self):returnself.has_actor()andself._updatableclassSimulatedPhysicsObject(SimulatedObject):__metaclass__=ABCMeta_updatable=Truedefrotate(self,axis,angle):"""Rotate the object by applying a rotation matrix defined by the given axis and angle"""rot_now=mu.matrix_as_3x3_tuples(self.get_rotation())rot_to_apply=mu.matrix_as_3x3_tuples(gemut.calc_rotation_matrix(axis,angle))# Matrix (of the rotation to apply) multiplies# from the LEFT the actual onerot_final=mu.matrix_as_tuple(mu.matrix_multiply(rot_to_apply,rot_now))self.set_rotation(rot_final)defoffset_by_position(self,offset_pos):pos=self.get_position()new_pos=mu.add3(offset_pos,pos)self.set_position(new_pos)defoffset_by_object(self,obj):offset_pos=obj.get_position()self.offset_by_position(offset_pos)defupdate_actor(self):"""If there is no actor, it won't do anything"""ifself.has_actor()andself._updatable:pos=self.get_position()rot=self.get_rotation()self._actor.set_pose(pos,rot)@abstractmethoddefget_position(self):"""Get the position of the object. :return: position :rtype: 3-sequence of floats """pass@abstractmethoddefset_position(self,pos):"""Set the orientation of the object. :param pos: position :type pos: 3-sequence of floats """pass@abstractmethoddefget_rotation(self):"""Get the orientation of the object. :return: rotation matrix :rtype: 9-sequence of floats """pass@abstractmethoddefset_rotation(self,rot):"""Set the orientation of the object. :param rot: rotation matrix :type rot: 9-sequence of floats """passdefget_pose(self):"""Get the pose (3D position and rotation) of the object. :return: pose :rtype: :class:`ars.utils.geometry.Transform` """returngemut.Transform(self.get_position(),self.get_rotation())defset_pose(self,pose):"""Set the pose (3D position and rotation) of the object. :param pose: :type pose: :class:`ars.utils.geometry.Transform` """self.set_position(pose.get_translation())rot=pose.get_rotation()self.set_rotation(mu.matrix_as_tuple(rot))classSimulatedBody(SimulatedPhysicsObject):"""Class encapsulating the physics, collision and graphical objects representing a body. All the public attributes of the physics object (`_body`) are accessible as if they were from this class, by using a trick with `__getattr__`. This avoids code duplication and frequent changes to the interface. For example, the call `sim_body.get_linear_velocity()` works if method `sim_body._body.get_linear_velocity` exists. There are some exceptions such as the getters and setters of position and rotation because the base class `SimulatedPhysicsObject` defines those abstract methods (some other non-abstract methods use them) and requires its subclasses to implement them. Otherwise we get "TypeError: Can't instantiate abstract class SimulatedBody with abstract methods". """def__init__(self,name,body=None,actor=None,geom=None):super(SimulatedBody,self).__init__(name,actor)self._body=bodyself._geom=geom# we might need it in the future#def has_body(self):# return not self._body is None#==========================================================================# Getters and setters#==========================================================================@propertydefbody(self):returnself._bodydefget_position(self):"""Get the position of the body. :return: position :rtype: 3-sequence of floats """returnself._body.get_position()defset_position(self,pos):"""Set the orientation of the body. :param pos: position :type pos: 3-sequence of floats """self._body.set_position(pos)defget_rotation(self):"""Get the orientation of the body. :return: rotation matrix :rtype: 9-sequence of floats """returnself._body.get_rotation()defset_rotation(self,rot):"""Set the orientation of the body. :param rot: rotation matrix :type rot: 9-sequence of floats """self._body.set_rotation(rot)def_get_attr_in_body(self,attr,*args):"""Return attribute `attr` from object `self._body`. `attr` can be a field or method name. .. seealso:: http://docs.python.org/reference/datamodel.html#object.__getattr__ """returngetattr(self._body,attr,*args)def__getattr__(self,attr,*args):"""Called when an attribute lookup has not found the attribute (i.e. field or method) in this class. .. seealso:: http://docs.python.org/reference/datamodel.html#object.__getattr__ """returnself._get_attr_in_body(attr,*args)classSimulatedJoint(SimulatedPhysicsObject):def__init__(self,name=None,joint=None,actor=None):super(SimulatedJoint,self).__init__(name,actor)self._joint=joint#==========================================================================# Dynamic and kinematic interaction#==========================================================================defadd_torque(self,torque):try:self._joint.add_torque(torque)exceptExceptionasex:print(ex)#==========================================================================# Getters and setters#==========================================================================@propertydefjoint(self):returnself._jointdefget_position(self):# It is necessary to have this method, even if it's not implementedraiseNotImplementedError()defset_position(self,pos):# It is necessary to have this method, even if it's not implementedraiseNotImplementedError()defget_rotation(self):# It is necessary to have this method, even if it's not implementedraiseNotImplementedError()defset_rotation(self,rot):# It is necessary to have this method, even if it's not implementedraiseNotImplementedError()