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. ] ])
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]])
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