def testSoftFingerContact(self):
     world = World()
     add_sphere(world, name='ball0')
     add_sphere(world, name='ball1')
     bodies = world.getbodies()
     bodies['ball0'].parentjoint.gpos[1,3] = 2.5
     bodies['ball0'].parentjoint.gvel[4] = -1.
     c = SoftFingerContact(world._shapes, 0.1)
     world.register(c)
     world.init()
     world.update_dynamic()
     dt = 0.001
     world.update_controllers(dt)
     world.update_constraints(dt)
     world.integrate(dt)
     world.update_dynamic()
     self.assertListsAlmostEqual(c.jacobian,
         [ [ 0., 1.,  0.,  0., 0., 0.,  0.   , -1. , 0.    , 0.,  0.,  0.],
           [-1., 0.,  0.,  0., 0., 1., -1.499,  0. , 0.    , 0.,  0., -1.],
           [ 0., 0., -1., -1., 0., 0.,  0.   ,  0. , -1.499, 1.,  0.,  0.],
           [ 0., 0.,  0.,  0., 1., 0.,  0.   ,  0. , 0.    , 0., -1., 0.]])
     self.assertListsAlmostEqual(bodies['ball0'].pose,
         [ [ 1.   ,  0.   ,  0.   ,  0.   ],
           [ 0.   ,  1.   ,  0.   ,  2.499],
           [ 0.   ,  0.   ,  1.   ,  0.   ],
           [ 0.   ,  0.   ,  0.   ,  1.   ] ])
Esempio n. 2
0
 def runTest(self):
     b0 = Body(mass=eye(6))
     w = World()
     w.add_link(w.ground, FreeJoint(), b0)
     w.init()
     ctrl = WeightController()
     w.register(ctrl)
     c0 = BallAndSocketConstraint(frames=(w.ground, b0))
     w.register(c0)
     w.init()
     w.update_dynamic()
     dt = 0.001
     w.update_controllers(dt)
     w.update_constraints(dt)
     self.assertListsAlmostEqual(c0._force, [0., 9.81, 0.])
     w.integrate(dt)
     w.update_dynamic()
     self.assertListsAlmostEqual(
         b0.pose, [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],
                   [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
Esempio n. 3
0
 def runTest(self):
     b0 = Body(mass=eye(6))
     w = World()
     w.add_link(w.ground, FreeJoint(), b0)
     w.init()
     ctrl =  WeightController()
     w.register(ctrl)
     c0 = BallAndSocketConstraint(frames=(w.ground, b0))
     w.register(c0)
     w.init()
     w.update_dynamic()
     dt = 0.001
     w.update_controllers(dt)
     w.update_constraints(dt)
     self.assertListsAlmostEqual(c0._force, [ 0.  ,  9.81,  0.  ])
     w.integrate(dt)
     w.update_dynamic()
     self.assertListsAlmostEqual(b0.pose,
         [[1.0, 0.0, 0.0, 0.0],
          [0.0, 1.0, 0.0, 0.0],
          [0.0, 0.0, 1.0, 0.0],
          [0.0, 0.0, 0.0, 1.0]])
# - M, N, B, gvel, gforce are computed by world (depending on current state)
M = w.mass
N = w.nleffects
B = w.viscosity
gvel = w.gvel
gforce = w.gforce

# if the world state is modified (if joints positions or velocities change),
# one has to update the kinematic and dynamic vectors and matrices
w.update_geometric()  # make forward kinematic
w.update_dynamic()  # make forward dynamic

# Simulate the world is done with a loop containing these functions
dt = .01  #s
w.update_dynamic()  # update dynamic of the world
w.update_controllers(dt)  # ...... generalized force of controllers
w.update_constraints(dt)  # ...... constraints for kinematic loops
w.integrate(dt)  # ...... the current state of the world

##### The Frame class ##########################################################

# This class represents the bodies and the subframes in the world
f = frames["EndEffector"]
name = f.name
body = f.body  # return parent body (itself if f is body)
pose = f.pose  # return H_0_f      (0 is world.ground)
bpose = f.bpose  # return H_b_f      (b is f.body)
twist = f.twist  # return T_f_0_f (T of f relative to 0 expressed in f)
jacobian = f.jacobian  # return J_f_0_f such as T_f_0_f = J_f_0_f * gvel
djacobian = f.djacobian  # return dJ_f_0_f
N      = w.nleffects
B      = w.viscosity
gvel   = w.gvel
gforce = w.gforce


# if the world state is modified (if joints positions or velocities change),
# one has to update the kinematic and dynamic vectors and matrices
w.update_geometric()    # make forward kinematic
w.update_dynamic()      # make forward dynamic


# Simulate the world is done with a loop containing these functions
dt = .01 #s
w.update_dynamic()              # update dynamic of the world
w.update_controllers(dt)        # ...... generalized force of controllers
w.update_constraints(dt)        # ...... constraints for kinematic loops
w.integrate(dt)                 # ...... the current state of the world



##### The Frame class ##########################################################

# This class represents the bodies and the subframes in the world
f = frames["EndEffector"]
name      = f.name
body      = f.body        # return parent body (itself if f is body)
pose      = f.pose        # return H_0_f      (0 is world.ground)
bpose     = f.bpose       # return H_b_f      (b is f.body)
twist     = f.twist       # return T_f_0_f (T of f relative to 0 expressed in f)
jacobian  = f.jacobian    # return J_f_0_f such as T_f_0_f = J_f_0_f * gvel