def testGeometricUpdate(self):
w = World()
add_simplearm(w)
bodies = w.getbodies()
arm = bodies['Arm']
forearm = bodies['ForeArm']
hand = bodies['Hand']
w.update_geometric()
self.assertListsAlmostEqual(arm.pose, eye(4))
self.assertListsAlmostEqual(forearm.pose, [ [ 1. , 0. , 0. , 0. ],
[ 0. , 1. , 0. , 0.5],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ] ])
self.assertListsAlmostEqual(hand.pose, [ [ 1. , 0. , 0. , 0. ],
[ 0. , 1. , 0. , 0.9],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ] ])
def test(self):
w = World()
(bodies, tags) = _human36(w)
w.update_geometric()
def tag_positions(tag_frames):
pos_dict= {}
for (key, val) in tag_frames.iteritems():
pos_dict[key] = dot(val.body.pose, val._bpose)[0:3,3]
pos_array = zeros((len(tag_frames),3))
for t in _humans_tags(1.741):
pos_array[t['HumansId']-1,:] = pos_dict[t['HumansName']]
return pos_array
p = tag_positions(tags)
self.assertListsAlmostEqual(p,
[[ 2.15013500e-01, 2.08166817e-17, 8.72241000e-02],
[ -4.31768000e-02, 2.08166817e-17, 8.72241000e-02],
[ 1.15254200e-01, 2.08166817e-17, 1.40324600e-01],
[ 1.15254200e-01, 2.08166817e-17, 3.41236000e-02],
[ 0.00000000e+00, 3.86502000e-02, 1.30575000e-01],
[ 0.00000000e+00, 4.72681500e-01, 1.37713100e-01],
[ 0.00000000e+00, 8.94874000e-01, 1.63828100e-01],
[ 4.71811000e-02, 9.58594600e-01, 1.21347700e-01],
[ 2.15013500e-01, 2.08166817e-17, -8.72241000e-02],
[ -4.31768000e-02, 2.08166817e-17, -8.72241000e-02],
[ 1.15254200e-01, 2.08166817e-17, -1.40324600e-01],
[ 1.15254200e-01, 2.08166817e-17, -3.41236000e-02],
[ 0.00000000e+00, 3.86502000e-02, -1.30575000e-01],
[ 0.00000000e+00, 4.72681500e-01, -1.37713100e-01],
[ 0.00000000e+00, 8.94874000e-01, -1.63828100e-01],
[ 4.71811000e-02, 9.58594600e-01, -1.21347700e-01],
[ 1.20651300e-01, 1.25613150e+00, 0.00000000e+00],
[ 9.15766000e-02, 1.42674950e+00, 0.00000000e+00],
[ 0.00000000e+00, 1.47932770e+00, 2.25459500e-01],
[ 0.00000000e+00, 1.16316210e+00, 2.62194600e-01],
[ 0.00000000e+00, 8.96266800e-01, 2.83086600e-01],
[ 0.00000000e+00, 7.04408600e-01, 2.25459500e-01],
[ 0.00000000e+00, 1.47932770e+00, -2.25459500e-01],
[ 0.00000000e+00, 1.16316210e+00, -2.62194600e-01],
[ 0.00000000e+00, 8.96266800e-01, -2.83086600e-01],
[ 0.00000000e+00, 7.04408600e-01, -2.25459500e-01],
[ 0.00000000e+00, 1.49813050e+00, 0.00000000e+00],
[ 0.00000000e+00, 1.74100000e+00, 0.00000000e+00]])
# Given the dynamic equation of tree structure without control: # # M * d(gvel)/dt + (N + B) * gvel = gforce # # - dt is given by user. # - 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)
# Given the dynamic equation of tree structure without control: # # M * d(gvel)/dt + (N + B) * gvel = gforce # # - dt is given by user. # - 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
