def __init__(self, world, config):
"""
Create articulated rigid body fly object for dynamic simulation.
The fly's body is initialized so that it's center of mass is
(0,0,0) in world coordinates.
Inputs:
world = ODE simulation world object
config = simulation configuration dictionary
"""
# Create fly body
self.body = ode.Body(world)
# Create wings
self.r_wing = ode.Body(world)
self.l_wing = ode.Body(world)
# Create wing joints
self.r_joint = ode.BallJoint(world)
self.l_joint = ode.BallJoint(world)
# Create angular motors
self.r_amotor = ode.AMotor(world)
self.l_amotor = ode.AMotor(world)
# Set body according to configuration
self.set2Config(config)
def addMotorJoint(self, body1, body2, axis):
joint = ode.AMotor(self.world)
joint.attach(body1, body2)
joint.setNumAxes(1)
#joint.setAnchor(anchor)
joint.setAxis(0, 1, axis)
joint.style = "amotor"
self.joints.append(joint)
return joint
def end(name):
if (name == 'amotor'):
joint = ode.AMotor(world, self._jg)
joint.attach(link1, link2)
if (anchor[0] is not None):
joint.setAnchor(anchor[0])
if (len(axes) > 3):
raise errors.InvalidError('Wrong number of axes for '
' amotor joint.')
joint.setNumAxes(len(axes))
for i in range(len(axes)):
joint.setAxis(i, 0, self._parser.parseVector(axes[i]))
self._applyAxisParams(joint, i, axes[i])
self.setODEObject(joint)
self._parser.pop()
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 _createJoint(self, joint, parentT, posture):
P = mm.TransVToSE3(joint.offset)
T = numpy.dot(parentT, P)
# R = mm.SO3ToSE3(posture.localRMap[joint.name])
R = mm.SO3ToSE3(posture.localRs[posture.skeleton.getElementIndex(
joint.name)])
T = numpy.dot(T, R)
temp_joint = joint
nodeExistParentJoint = None
while True:
if temp_joint.parent == None:
nodeExistParentJoint = None
break
elif temp_joint.parent.name in self.nodes:
nodeExistParentJoint = temp_joint.parent
break
else:
temp_joint = temp_joint.parent
# if joint.parent and len(joint.children)>0:
if nodeExistParentJoint and len(joint.children) > 0:
if joint.name in self.config.nodes:
p = mm.SE3ToTransV(T)
node = self.nodes[joint.name]
cfgNode = self.config.getNode(joint.name)
if cfgNode.jointAxes != None:
if len(cfgNode.jointAxes) == 0:
node.joint = ode.BallJoint(self.world)
node.motor = ode.AMotor(self.world)
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.joint.setAnchor(p)
# node.motor.attach(self.nodes[joint.parent.name].body, node.body)
node.motor.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.motor.setNumAxes(3)
node.motor.setAxis(0, 0, (1, 0, 0))
node.motor.setAxis(1, 0, (0, 1, 0))
node.motor.setAxis(2, 0, (0, 0, 1))
node.motor.setParam(ode.ParamLoStop,
cfgNode.jointLoStop)
node.motor.setParam(ode.ParamHiStop,
cfgNode.jointHiStop)
node.motor.setParam(ode.ParamLoStop2,
cfgNode.jointLoStop)
node.motor.setParam(ode.ParamHiStop2,
cfgNode.jointHiStop)
node.motor.setParam(ode.ParamLoStop3,
cfgNode.jointLoStop)
node.motor.setParam(ode.ParamHiStop3,
cfgNode.jointHiStop)
elif len(cfgNode.jointAxes) == 1:
node.joint = ode.HingeJoint(self.world)
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.joint.setAnchor(p)
newR = mm.SE3ToSO3(self.newTs[joint.name])
node.joint.setAxis(
numpy.dot(newR, cfgNode.jointAxes[0]))
node.joint.setParam(ode.ParamLoStop,
cfgNode.jointLoStop)
node.joint.setParam(ode.ParamHiStop,
cfgNode.jointHiStop)
elif len(cfgNode.jointAxes) == 2:
pass
node.joint = ode.UniversalJoint(self.world)
#
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.joint.setAnchor(p)
newR = mm.SE3ToSO3(self.newTs[joint.name])
node.joint.setAxis1(
numpy.dot(newR, cfgNode.jointAxes[0]))
node.joint.setAxis2(
numpy.dot(newR, cfgNode.jointAxes[1]))
node.joint.setParam(ode.ParamLoStop,
cfgNode.jointLoStop)
node.joint.setParam(ode.ParamHiStop,
cfgNode.jointHiStop)
node.joint.setParam(ode.ParamLoStop2,
cfgNode.jointLoStop)
node.joint.setParam(ode.ParamHiStop2,
cfgNode.jointHiStop)
else:
node.joint = ode.FixedJoint(self.world)
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body, node.body)
# node.joint.setFixed()
node.Kp = cfgNode.Kp
node.Kd = cfgNode.Kd
else:
pass
for childJoint in joint.children:
self._createJoint(childJoint, T, posture)
def create(self):
#create body
self.bodyCar = ode.Body(self.world)
mCar = ode.Mass()
mCar.setBoxTotal((self.passengers * PERSONWEIGHT + CARWEIGHT),
CARLENGTH, CARHEIGHT, CARWIDTH - WHEELWIDTH)
self.bodyCar.setMass(mCar)
self.geomCar = ode.GeomBox(
self.space, (CARLENGTH, CARHEIGHT, CARWIDTH - WHEELWIDTH))
self.geomCar.setBody(self.bodyCar)
self.bodyCar.setPosition(
(0 + self.position[0],
WHEELRADIUS + CARHEIGHT / 2 + self.position[1],
0 + self.position[2]))
self.viz.addGeom(self.geomCar)
self.viz.GetProperty(self.bodyCar).SetColor(self.bodyCarColor)
#create wheels
self.bodyFWL, self.geomFWL = self.createWheels(CARLENGTH / 2,
-CARWIDTH / 2)
self.bodyFWR, self.geomFWR = self.createWheels(CARLENGTH / 2,
CARWIDTH / 2)
self.bodyRWL, self.geomRWL = self.createWheels(-CARLENGTH / 2,
-CARWIDTH / 2)
self.bodyRWR, self.geomRWR = self.createWheels(-CARLENGTH / 2,
CARWIDTH / 2)
#create front left joint
self.flJoint = ode.Hinge2Joint(self.world)
self.flJoint.attach(self.bodyCar, self.bodyFWL)
self.flJoint.setAnchor(
(CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
-CARWIDTH / 2 + self.position[2]))
self.flJoint.setAxis2((0, 0, 1))
self.flJoint.setAxis1((0, 1, 0))
self.flJoint.setParam(ode.ParamSuspensionCFM, 0.5)
self.flJoint.setParam(ode.ParamSuspensionERP, 0.8)
#create front right joint
self.frJoint = ode.Hinge2Joint(self.world)
self.frJoint.attach(self.bodyCar, self.bodyFWR)
self.frJoint.setAnchor(
(CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
CARWIDTH / 2 + self.position[2]))
self.frJoint.setAxis2((0, 0, 1))
self.frJoint.setAxis1((0, 1, 0))
self.frJoint.setParam(ode.ParamSuspensionCFM, 0.5)
self.frJoint.setParam(ode.ParamSuspensionERP, 0.8)
#create rear left joint
self.rlJoint = ode.HingeJoint(self.world)
self.rlJoint.attach(self.bodyCar, self.bodyRWL)
self.rlJoint.setAnchor(
(-CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
-CARWIDTH / 2 + self.position[2]))
self.rlJoint.setAxis((0, 0, 1))
#create rear right joint
self.rrJoint = ode.HingeJoint(self.world)
self.rrJoint.attach(self.bodyCar, self.bodyRWR)
self.rrJoint.setAnchor(
(-CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
CARWIDTH / 2 + self.position[2]))
self.rrJoint.setAxis((0, 0, 1))
# add Joints to allGroups
allGroups.append([self.bodyFWL, self.bodyCar])
allGroups.append([self.bodyFWR, self.bodyCar])
allGroups.append([self.bodyRWL, self.bodyCar])
allGroups.append([self.bodyRWR, self.bodyCar])
#add Wheels and Road to allGroups
allGroups.append([self.bodyFWL, self.road])
allGroups.append([self.bodyFWR, self.road])
allGroups.append([self.bodyRWL, self.road])
allGroups.append([self.bodyRWR, self.road])
#create front left motor roll
self.flMotorRoll = ode.AMotor(self.world)
self.flMotorRoll.attach(self.bodyCar, self.bodyRWL)
self.flMotorRoll.setNumAxes(1)
self.flMotorRoll.setAxis(0, 2, (0, 0, 1))
self.flMotorRoll.enable()
self.flMotorRoll.setParam(ode.ParamSuspensionCFM, 0.5)
self.flMotorRoll.setParam(ode.ParamSuspensionERP, 0.8)
#create front left motor yaw
self.flMotorYaw = ode.AMotor(self.world)
self.flMotorYaw.attach(self.bodyCar, self.bodyRWL)
self.flMotorYaw.setNumAxes(1)
self.flMotorYaw.setAxis(0, 2, (0, 1, 0))
self.flMotorYaw.enable()
self.flMotorYaw.setParam(ode.ParamSuspensionCFM, 0.5)
self.flMotorYaw.setParam(ode.ParamSuspensionERP, 0.8)
#create front right motor
self.frMotorRoll = ode.AMotor(self.world)
self.frMotorRoll.attach(self.bodyCar, self.bodyRWR)
self.frMotorRoll.setNumAxes(1)
self.frMotorRoll.setAxis(0, 2, (0, 0, 1))
self.frMotorRoll.enable()
self.frMotorRoll.setParam(ode.ParamSuspensionCFM, 0.5)
self.frMotorRoll.setParam(ode.ParamSuspensionERP, 0.8)
#create front right motor
self.frMotorYaw = ode.AMotor(self.world)
self.frMotorYaw.attach(self.bodyCar, self.bodyRWR)
self.frMotorYaw.setNumAxes(1)
self.frMotorYaw.setAxis(0, 2, (0, 1, 0))
self.frMotorYaw.enable()
self.frMotorYaw.setParam(ode.ParamSuspensionCFM, 0.5)
self.frMotorYaw.setParam(ode.ParamSuspensionERP, 0.8)