def test_OdeControlModel():
# mcfg.defaultContactMode = ode.ContactBounce | ode.ContactSoftERP | ode.ContactSoftCFM
# mcfg.defaultJointAxes = []
# mcfg.defaultJointLoStop = mm.deg2Rad(-5)
# mcfg.defaultJointHiStop = mm.deg2Rad(5)
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
motion = motion[30:]
AX = numpy.array([1, 0, 0])
AY = numpy.array([0, 1, 0])
AZ = numpy.array([0, 0, 1])
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
mcfg.defaultKp = 100.
mcfg.defaultKd = 1.
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('body2')
node.length = 1
node.offset = (0, 0, .2)
# node.jointAxes = [AY]
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
controlModel.fixRootBody()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'model',
yr.OdeModelRenderer(controlModel, (255, 255, 255),
yr.POLYGON_FILL))
def simulateCallback(frame):
for i in range(stepsPerFrame):
torques = controlModel.calcPDTorque(motion[frame])
controlModel.applyTorque(torques)
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_delNode():
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultBoneRatio = .8
node = mcfg.addNode('body1')
# node = mcfg.addNode('body2')
node = mcfg.addNode('body3')
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
controlModel.fixRootBody()
motionModel = OdeMotionModel(odeWorld.world, odeWorld.space, motion[0],
mcfg)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'controlModel',
yr.OdeModelRenderer(controlModel, (255, 255, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.OdeModelRenderer(motionModel, (255, 255, 255), yr.POLYGON_LINE))
def preFrameCallback(frame):
motionModel.update(motion[frame])
viewer.setPreFrameCallback(preFrameCallback)
def simulateCallback(frame):
for i in range(stepsPerFrame):
torques = controlModel.calcPDTorque(motion[frame])
controlModel.applyTorque(torques)
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_OdeMotionModel():
bvhFilePath = '../samples/block_3_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
node = mcfg.getNode('body1')
node.mass = 1.
node = mcfg.getNode('body2')
node.mass = 1.
node.boneRatio = .5
node.offset = (0, 0, .1)
node = mcfg.getNode('body3')
node.mass = 1.
node.length = .1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
odeWorld = yow.OdeWorld(wcfg, mcfg)
motionModel = OdeMotionModel(odeWorld.world, odeWorld.space, motion[0],
mcfg)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion',
yr.JointMotionRenderer(motion, (0, 0, 255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'model',
yr.OdeModelRenderer(motionModel, (255, 255, 255), yr.POLYGON_LINE))
def preFrameCallback(frame):
motionModel.update(motion[frame])
viewer.setPreFrameCallback(preFrameCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_biped_delNode():
def buildMassMap():
massMap = {}
massMap = massMap.fromkeys([
'Head', 'Head_Effector', 'Hips', 'LeftArm', 'LeftFoot',
'LeftForeArm', 'LeftHand', 'LeftHand_Effector', 'LeftLeg',
'LeftShoulder1', 'LeftToes', 'LeftToes_Effector', 'LeftUpLeg',
'RightArm', 'RightFoot', 'RightForeArm', 'RightHand',
'RightHand_Effector', 'RightLeg', 'RightShoulder', 'RightToes',
'RightToes_Effector', 'RightUpLeg', 'Spine', 'Spine1'
], 0.)
# torso : 10
massMap['Hips'] += 2.
massMap['Spine'] += 6.
massMap['Spine1'] += 2.
# head : 3
massMap['Spine1'] += 1.
massMap['Head'] += 2.
# right upper arm : 2
massMap['Spine1'] += .5
massMap['RightShoulder'] += .5
massMap['RightArm'] += 1.
# left upper arm : 2
massMap['Spine1'] += .5
massMap['LeftShoulder1'] += .5
massMap['LeftArm'] += 1.
# right lower arm : 1
massMap['RightForeArm'] = .8
massMap['RightHand'] = .2
# left lower arm : 1
massMap['LeftForeArm'] = .8
massMap['LeftHand'] = .2
# right thigh : 7
massMap['Hips'] += 2.
massMap['RightUpLeg'] += 5.
# left thigh : 7
massMap['Hips'] += 2.
massMap['LeftUpLeg'] += 5.
# right shin : 5
massMap['RightLeg'] += 5.
# left shin : 5
massMap['LeftLeg'] += 5.
# right foot : 4
massMap['RightFoot'] += 2.
massMap['RightToes'] += 2.
# left foot : 4
massMap['LeftFoot'] += 2.
massMap['LeftToes'] += 2.
return massMap
massMap = buildMassMap()
bvhFilePath = '../samples/wd2_WalkSameSame00.bvh'
motion = yf.readBvhFile(bvhFilePath, .01)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Head')
node.length = .2
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
node = mcfg.getNode('LeftFoot')
node.length = .25
mcfg.delNode('Spine1')
mcfg.delNode('RightShoulder')
mcfg.delNode('LeftShoulder1')
mcfg.delNode('RightHand')
mcfg.delNode('LeftHand')
mcfg.delNode('RightToes')
mcfg.delNode('LeftToes')
wcfg = ypc.WorldConfig()
wcfg.planeHeight = None
stepsPerFrame = 10
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
# controlModel.fixRootBody()
print controlModel
motionModel = OdeMotionModel(odeWorld.world, odeWorld.space, motion[0],
mcfg)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion', yr.JointMotionRenderer(motion, (0, 0, 255),
yr.LINK_LINE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'controlModel',
yr.OdeModelRenderer(controlModel, (255, 255, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.OdeModelRenderer(motionModel, (255, 255, 255), yr.POLYGON_LINE))
def preFrameCallback(frame):
motionModel.update(motion[frame])
viewer.setPreFrameCallback(preFrameCallback)
def simulateCallback(frame):
for i in range(stepsPerFrame):
# torques = controlModel.calcPDTorque(motion[frame])
# controlModel.applyTorque(torques)
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_ode():
M_PI = 3.14159265358979323846
M_TWO_PI = 6.28318530717958647693 #// = 2 * pi
M_PI_SQRT2 = 2.22144146907918312351 # // = pi / sqrt(2)
M_PI_SQR = 9.86960440108935861883 #// = pi^2
M_RADIAN = 0.0174532925199432957692 #// = pi / 180
M_DEGREE = 57.2957795130823208768 # // = pi / 180
## vpWorld world;
# world = ode.World()
# space = ode.Space()
odeWorld = yow.OdeWorld()
world = odeWorld.world
space = odeWorld.space
## vpBody ground, pendulum;
## vpBody base, body1, body2;
base = ode.Body(world)
body1 = ode.Body(world)
body2 = ode.Body(world)
## vpBJoint J1;
## vpBJoint J2;
J1 = ode.BallJoint(world)
J2 = ode.BallJoint(world)
M1 = ode.AMotor(world)
M2 = ode.AMotor(world)
M1.setNumAxes(2)
M2.setNumAxes(2)
## Vec3 axis1(1,1,1);
## Vec3 axis2(1,0,0);
axis1 = numpy.array([1, 1, 0])
axis2 = numpy.array([1, 1, 1])
axisX = numpy.array([1, 0, 0])
axisY = numpy.array([0, 1, 0])
axisZ = numpy.array([0, 0, 1])
## scalar timeStep = .01;
## int deg1 = 0;
## int deg2 = 0;
timeStep = .01
deg1 = [0.]
deg2 = 0.
odeWorld.timeStep = timeStep
## ground.AddGeometry(new vpBox(Vec3(10, 10, 0)));
## ground.SetFrame(Vec3(0,0,-.5));
## ground.SetGround();
# // bodies
## base.AddGeometry(new vpBox(Vec3(3, 3, .5)));
## base.SetGround();
geom_base = ode.GeomBox(space, (3, .5, 3))
geom_base.setBody(base)
geom_base.setPosition((0, .5, 0))
# geom_base.setRotation(mm.SO3ToOdeSO3(mm.exp((1,0,0),math.pi/2)))
fj = ode.FixedJoint(world)
fj.attach(base, ode.environment)
fj.setFixed()
mass_base = ode.Mass()
mass_base.setBox(100, 3, .5, 3)
base.setMass(mass_base)
## body1.AddGeometry(new vpCapsule(0.2, 4), Vec3(0, 0, 2));
## body2.AddGeometry(new vpCapsule(0.2, 4), Vec3(0, 0, 2));
## body1.SetCollidable(false);
## body2.SetCollidable(false);
# geom_body1 = ode.GeomCapsule(space, .2, 4)
geom_body1 = ode.GeomBox(space, (.4, 4, .4))
geom_body1.setBody(body1)
geom_body1.setPosition((0, .5 + .25 + 2, 0))
mass_body1 = ode.Mass()
mass_body1.setBox(100, .4, 4, .4)
body1.setMass(mass_body1)
# init_ori = mm.exp((1,0,0),math.pi/2)
# geom_body1.setRotation(mm.SO3ToOdeSO3(init_ori))
# geom_body2 = ode.GeomCapsule(space, .2, 4)
geom_body2 = ode.GeomBox(space, (.4, 4, .4))
geom_body2.setBody(body2)
geom_body2.setPosition((0, .5 + .25 + 2 + 4.2, 0))
# geom_body2.setRotation(mm.SO3ToOdeSO3(mm.exp((1,0,0),math.pi/2)))
mass_body2 = ode.Mass()
mass_body2.setBox(100, .4, 4, .4)
body2.setMass(mass_body2)
## axis1.Normalize();
## axis2.Normalize();
axis1 = mm.normalize(axis1)
axis2 = mm.normalize(axis2)
## //J1.SetAxis(axis1);
## base.SetJoint(&J1, Vec3(0, 0, .1));
## body1.SetJoint(&J1, Vec3(0, 0, -.1));
J1.attach(base, body1)
J1.setAnchor((0, .5 + .25, 0))
M1.attach(base, body1)
## //J2.SetAxis(axis2);
## //body1.SetJoint(&J2, Vec3(0, 0, 4.1));
## //body2.SetJoint(&J2, Vec3(0, 0, -.1));
J2.attach(body1, body2)
J2.setAnchor((0, .5 + .25 + 4.1, 0))
M2.attach(body1, body2)
## world.AddBody(&ground);
## world.AddBody(&base);
## world.SetGravity(Vec3(0.0, 0.0, -10.0));
world.setGravity((0, 0, 0))
def PDControl(frame):
# global deg1[0], J1, J2, M1, M2
# scalar Kp = 1000.;
# scalar Kd = 100.;
Kp = 100.
Kd = 2.
# SE3 desiredOri1 = Exp(Axis(axis1), scalar(deg1[0] * M_RADIAN));
# SE3 desiredOri2 = Exp(Axis(axis2), scalar(deg2 * M_RADIAN));
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
# desiredOri2 = mm.exp(axis2, deg2 * M_RADIAN)
# se3 log1= Log(J1.GetOrientation() % desiredOri1);
# se3 log2= Log(J2.GetOrientation() % desiredOri2);
parent1 = J1.getBody(0)
child1 = J1.getBody(1)
parent1_desired_SO3 = mm.exp((0, 0, 0), 0)
child1_desired_SO3 = desiredOri1
# child1_desired_SO3 = parent1_desired_SO3
parent1_body_SO3 = mm.odeSO3ToSO3(parent1.getRotation())
child1_body_SO3 = mm.odeSO3ToSO3(child1.getRotation())
# init_ori = (mm.exp((1,0,0),math.pi/2))
# child1_body_SO3 = numpy.dot(mm.odeSO3ToSO3(child1.getRotation()), init_ori.transpose())
align_SO3 = numpy.dot(parent1_body_SO3,
parent1_desired_SO3.transpose())
child1_desired_SO3 = numpy.dot(align_SO3, child1_desired_SO3)
diff_rot = mm.logSO3(
numpy.dot(child1_desired_SO3, child1_body_SO3.transpose()))
# print diff_rot
parent_angleRate = parent1.getAngularVel()
child_angleRate = child1.getAngularVel()
# print child_angleRate
angleRate = numpy.array([
-parent_angleRate[0] + child_angleRate[0],
-parent_angleRate[1] + child_angleRate[1],
-parent_angleRate[2] + child_angleRate[2]
])
# torque1 = Kp*diff_rot - Kd*angleRate
# print torque1
# J1_ori =
# log1 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# log2 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# Vec3 torque1 = Kp*(Vec3(log1[0],log1[1],log1[2])) - Kd*J1.GetVelocity();
# Vec3 torque2 = Kp*(Vec3(log2[0],log2[1],log2[2])) - Kd*J2.GetVelocity();
M1.setAxis(0, 0, diff_rot)
M1.setAxis(1, 0, angleRate)
# M2.setAxis(0,0,torque1)
## J1.SetTorque(torque1);
## J2.SetTorque(torque2);
M1.addTorques(-Kp * mm.length(diff_rot), 0, 0)
M1.addTorques(0, Kd * mm.length(angleRate), 0)
# print diff_rot
# print angleRate
# M2.addTorques(torque2, 0, 0)
# cout << "SimulationTime " << world.GetSimulationTime() << endl;
# cout << "deg1[0] " << deg1[0] << endl;
# cout << "J1.vel " << J1.GetVelocity();
# cout << "J1.torq " << torque1;
# cout << endl;
def calcPDTorque(Rpd, Rcd, Rpc, Rcc, Wpc, Wcc, Kp, Kd, joint):
Rpc = mm.odeSO3ToSO3(Rpc)
Rcc = mm.odeSO3ToSO3(Rcc)
Ra = numpy.dot(Rpc, Rpd.transpose())
Rcd = numpy.dot(Ra, Rcd)
dR = mm.logSO3(numpy.dot(Rcd, Rcc.transpose()))
dW = numpy.array(
[-Wpc[0] + Wcc[0], -Wpc[1] + Wcc[1], -Wpc[2] + Wcc[2]])
# joint.setAxis(0,0,dR)
# joint.setAxis(1,0,dW)
joint.setAxis(0, 0, dR - dW)
# joint.addTorques(-Kp*mm.length(dR),0,0)
# joint.addTorques(0,Kd*mm.length(dW),0)
joint.addTorques(-Kp * mm.length(dR) - Kd * mm.length(dW), 0, 0)
def PDControl3(frame):
# Kp = 100.*70;
# Kd = 10.*3;
Kp = 1000.
Kd = 10.
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
desiredOriX = mm.exp(axisX, deg1[0] * M_RADIAN)
desiredOriY = mm.exp(axisY, deg1[0] * M_RADIAN)
desiredOriZ = mm.exp(axisZ, deg1[0] * M_RADIAN)
calcPDTorque(mm.I_SO3(), mm.exp(axisX, -90 * M_RADIAN),
base.getRotation(), body1.getRotation(),
base.getAngularVel(), body1.getAngularVel(), Kp, Kd,
M1)
# calcPDTorque(mm.I_SO3, desiredOriY, base.getRotation(), body1.getRotation(), base.getAngularVel(), body1.getAngularVel(), Kp, Kd, M1);
# calcPDTorque(mm.I_SO3, desiredOriX, body1.getRotation(), body2.getRotation(), body1.getAngularVel(), body2.getAngularVel(), Kp, Kd, M2);
# ground = ode.GeomPlane(space, (0,1,0), 0)
viewer = ysv.SimpleViewer()
viewer.setMaxFrame(100)
# viewer.record(False)
viewer.doc.addRenderer('object',
yr.OdeRenderer(space, (255, 255, 255)))
def preFrameCallback(frame):
# global deg1[0]
print 'deg1[0]', deg1[0]
deg1[0] += 1
# PDControl(frame)
PDControl3(frame)
viewer.setPreFrameCallback(preFrameCallback)
def simulateCallback(frame):
odeWorld.step()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def test_body_pos_vel_acc_functions():
# bvhFilePath = '../samples/chain_6.bvh'
# bvhFilePath = '../samples/block_3_rotate.bvh'
bvhFilePath = '../samples/block_tree_rotate.bvh'
motion = yf.readBvhFile(bvhFilePath)
bvhFilePath = '../samples/chain_1.bvh'
motion2 = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
mcfg2 = ypc.ModelConfig()
for i in range(motion2[0].skeleton.getElementNum()):
mcfg2.addNode(motion2[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = -1.
wcfg.gravity = (0, 0, 0)
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / stepsPerFrame
odeWorld = yow.OdeWorld(wcfg, mcfg)
motionModel = OdeMotionModel(odeWorld.world, odeWorld.space, motion[0],
mcfg)
controlModel = OdeControlModel(odeWorld.world, odeWorld.space,
motion[0], mcfg)
controlModel2 = OdeControlModel(odeWorld.world, odeWorld.space,
motion2[0], mcfg2)
controlModel.fixRootBody()
# controlModel2.setBodyPositionGlobal(0, (0,1,0))
controlModel2.getBody('link0').setPosition((0, 1, 0))
print controlModel
print controlModel2
cm_p = [mm.O_Vec3()] * controlModel.getBodyNum()
cm_v = [mm.O_Vec3()] * controlModel.getBodyNum()
cm_a = [mm.O_Vec3()] * controlModel.getBodyNum()
cm_av = [mm.O_Vec3()] * controlModel.getBodyNum()
cm_aa = [mm.O_Vec3()] * controlModel.getBodyNum()
viewer = ysv.SimpleViewer()
viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'controlModel',
yr.OdeModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_LINE))
# viewer.doc.addRenderer('motionModel', yr.OdeModelRenderer(motionModel, (100,100,100), yr.POLYGON_LINE))
viewer.doc.addRenderer(
'controlModel2',
yr.OdeModelRenderer(controlModel2, (255, 240, 255),
yr.POLYGON_LINE))
viewer.doc.addRenderer('cm_p', yr.PointsRenderer(cm_p, (255, 0, 0)))
viewer.doc.addRenderer('cm_v',
yr.VectorsRenderer(cm_v, cm_p, (0, 0, 255)))
# viewer.doc.addRenderer('cm_a', yr.VectorsRenderer(cm_a, cm_p, (0,255,0)))
viewer.doc.addRenderer('cm_av',
yr.VectorsRenderer(cm_av, cm_p, (255, 255, 0)))
# viewer.doc.addRenderer('cm_aa', yr.VectorsRenderer(cm_aa, cm_p, (0,255,255)))
def simulateCallback(frame):
#
for i in range(stepsPerFrame):
odeWorld.step()
# motionModel.update(motion[frame])
controlModel.getBody('left1').setTorque((0, 0, 10))
controlModel2.getBody('link0').setTorque((0, 0, 10))
#
cm_p[:] = controlModel.getBodyPositionsGlobal(
) + controlModel2.getBodyPositionsGlobal()
cm_v[:] = controlModel.getBodyVelocitiesGlobal(
) + controlModel2.getBodyVelocitiesGlobal()
# there is no acceleration function in ode
# cm_a[:] = controlModel.getBodyAccelerationsGlobal() + controlModel2.getBodyAccelerationsGlobal()
cm_av[:] = controlModel.getBodyAngVelocitiesGlobal(
) + controlModel2.getBodyAngVelocitiesGlobal()
# there is no acceleration function in ode
# cm_aa[:] = controlModel.getBodyAngAccelerationsGlobal() + controlModel2.getBodyAngAccelerationsGlobal()
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()