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 buildObjects():
world = ode.World()
world.setGravity((0, -9.81, 0))
body1 = ode.Body(world)
M = ode.Mass()
M.setSphere(2500, 0.05)
body1.setMass(M)
body1.setPosition((1, 2, 0))
body2 = ode.Body(world)
M = ode.Mass()
M.setSphere(2500, 0.05)
body2.setMass(M)
body2.setPosition((2, 2, 0))
j1 = ode.BallJoint(world)
j1.attach(body1, ode.environment)
j1.setAnchor((0, 2, 0))
#j1 = ode.HingeJoint(world)
#j1.attach(body1, ode.environment)
#j1.setAnchor( (0,2,0) )
#j1.setAxis( (0,0,1) )
#j1.setParam(ode.ParamVel, 3)
#j1.setParam(ode.ParamFMax, 22)
j2 = ode.BallJoint(world)
j2.attach(body1, body2)
j2.setAnchor((1, 2, 0))
return world, body1, body2, j1, j2
def addBallJoint(self, body1, body2, anchor, baseAxis, baseTwistUp,
flexLimit = pi, twistLimit = pi, flexForce = 0.0, twistForce = 0.0):
anchor = add3(anchor, self.offset)
# create the joint
joint = ode.BallJoint(self.world)
joint.attach(body1, body2)
joint.setAnchor(anchor)
# store the base orientation of the joint in the local coordinate system
# of the primary body (because baseAxis and baseTwistUp may not be
# orthogonal, the nearest vector to baseTwistUp but orthogonal to
# baseAxis is calculated and stored with the joint)
joint.baseAxis = getBodyRelVec(body1, baseAxis)
tempTwistUp = getBodyRelVec(body1, baseTwistUp)
baseSide = norm3(cross(tempTwistUp, joint.baseAxis))
joint.baseTwistUp = norm3(cross(joint.baseAxis, baseSide))
# store the base twist up vector (original version) in the local
# coordinate system of the secondary body
joint.baseTwistUp2 = getBodyRelVec(body2, baseTwistUp)
# store joint rotation limits and resistive force factors
joint.flexLimit = flexLimit
joint.twistLimit = twistLimit
joint.flexForce = flexForce
joint.twistForce = twistForce
joint.style = "ball"
self.joints.append(joint)
return joint
def attach(self, frame_no):
'''Attach marker bodies to the corresponding skeleton bodies.
Attachments are only made for markers that are not in a dropout state in
the given frame.
Parameters
----------
frame_no : int
The frame of data we will use for attaching marker bodies.
'''
assert not self.joints
for label, j in self.channels.items():
target = self.targets.get(label)
if target is None:
continue
if self.visibility[frame_no, j] < 0:
continue
if np.linalg.norm(self.velocities[frame_no, j]) > 10:
continue
joint = ode.BallJoint(self.world.ode_world, self.jointgroup)
joint.attach(self.bodies[label].ode_body, target.ode_body)
joint.setAnchor1Rel([0, 0, 0])
joint.setAnchor2Rel(self.offsets[label])
joint.setParam(ode.ParamCFM, self.cfms[frame_no, j])
joint.setParam(ode.ParamERP, self.erp)
joint.name = label
self.joints[label] = joint
self._frame_no = frame_no
def end(name):
if (name == 'ball'):
joint = ode.BallJoint(world, self._jg)
joint.attach(link1, link2)
if (anchor[0] is not None):
joint.setAnchor(anchor[0])
self.setODEObject(joint)
self._parser.pop()
def addBallJoint(self, body1, body2, anchor):
anchor = add3(anchor, self.offset)
# create the joint
joint = ode.BallJoint(self.sim.world)
joint.attach(body1, body2)
joint.setAnchor(anchor)
joint.style = "ball"
self.joints.append(joint)
return joint
def joinBody(self, world, space, body, type, index):
if pygame.time.get_ticks() - self.pickupdelay < 500:
return
self.pickupJoint = ode.BallJoint(world)
self.pickupJoint.attach(body, self.mainbody.body)
pos = self.mainbody.body.getPosition()
realpos = (pos[0], pos[1], 0)
self.pickupJoint.setAnchor(realpos)
self.carryingitem = 1
self.pickupdelay = pygame.time.get_ticks()
self.carrytype = type
self.carryindex = index
def addBallJoint(self,
body1,
body2,
anchor,
baseAxis=None,
baseTwistUp=None,
flexLimit=pi,
twistLimit=pi,
flexForce=0.0,
twistForce=0.0):
def getBodyRelVec(b, v):
"""
Returns the 3-vector v transformed into the local coordinate system of ODE
body b.
"""
return rotate3(invert3x3(b.getRotation()), v)
anchor = add3(anchor, self.offset)
# create the joint
joint = ode.BallJoint(self.world)
joint.attach(body1, body2)
joint.setAnchor(anchor)
if baseAxis is not None:
joint.haveAxis = True
# store the base orientation of the joint in the local coordinate system
# of the primary body (because baseAxis and baseTwistUp may not be
# orthogonal, the nearest vector to baseTwistUp but orthogonal to
# baseAxis is calculated and stored with the joint)
joint.baseAxis = getBodyRelVec(body1, baseAxis)
tempTwistUp = getBodyRelVec(body1, baseTwistUp)
baseSide = norm3(cross(tempTwistUp, joint.baseAxis))
joint.baseTwistUp = norm3(cross(joint.baseAxis, baseSide))
# store the base twist up vector (original version) in the local
# coordinate system of the secondary body
joint.baseTwistUp2 = getBodyRelVec(body2, baseTwistUp)
# store joint rotation limits and resistive force factors
joint.flexLimit = flexLimit
joint.twistLimit = twistLimit
joint.flexForce = flexForce
joint.twistForce = twistForce
else:
joint.haveAxis = False
joint.style = "ball"
self.joints.append(joint)
return joint
def create_ball(self, key, pos, bod_keys):
""" Create a hinge joint.
Arguments:
key : number id to assign the hinge
pos : position of the joint
bod_keys : list of two bodies to attach the joint to
"""
# Auto label the joint key or not.
key = len(self.joints) if key == -1 else key
j = ode.BallJoint(self.world)
j.attach(self.bodies[bod_keys[0]], self.bodies[bod_keys[1]])
j.setAnchor((pos))
j.style = "ball"
self.joints[key] = j
return key
def __init__(self,num):
# Create a world object
self.world = ode.World()
self.world.setGravity( (0,-self.g,0) )
self.world.setERP(0.8)
self.world.setCFM(1.e-5)
self.space = ode.Space()
self.contactgroup = ode.JointGroup()
# create objects
self.anchor = Sphere(self.world,self.space,-2,1.e30,0.001,False)
self.pendulum = Sphere(self.world,self.space,-1,self.d,self.r,True)
self.joint = ode.BallJoint(self.world)
self.joint.attach(self.anchor.body,self.pendulum.body)
self.sat = [Sphere(self.world,self.space,s,self.ds,self.rs,False) for s in range(num)]
self.walls = (
ode.GeomPlane(self.space, ( 1, 0, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0, 1, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0, 0, 1), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, (-1, 0, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0,-1, 0), -self.l*0.1),
ode.GeomPlane(self.space, ( 0, 0,-1), -(self.l+self.r)*1.1)
)
# remember bodies
self.geoms = {}
self.geoms[self.anchor.geom] = -2
self.geoms[self.pendulum.geom] = -1
for s in range(num):
self.geoms[self.sat[s].geom] = s
for s in range(len(self.walls)):
self.geoms[self.walls[s]] = 1000+s
self.reset_1()
def loaddata(self, vpoints):
self.world = ode.World()
self.world.setGravity((0, -9.81, 0))
#vpoints = parse_vpoints(mechanism)
self.vlinks = {}
for i, vpoint in enumerate(vpoints):
for link in vpoint.links:
if link in self.vlinks:
self.vlinks[link].append(i)
else:
self.vlinks[link] = [i]
self.bodies = []
for name in tuple(self.vlinks):
if name == 'ground':
continue
vlink = self.vlinks[name]
while len(vlink) > 2:
n = vlink.pop()
for anchor in vlink[:2]:
i = 1
while 'link_{}'.format(i) in self.vlinks:
i += 1
self.vlinks['link_{}'.format(i)] = (anchor, n)
for name, vlink in self.vlinks.items():
#print(name, [(vpoints[i].cx, vpoints[i].cy) for i in vlink])
if name == 'ground':
continue
rad = 0.002
Mass = 1
link = ode.Body(self.world)
M = ode.Mass() # mass parameter
M.setZero() # init the Mass
M.setCylinderTotal(
Mass, 3, rad, self.__lenth(vpoints[vlink[0]],
vpoints[vlink[1]]))
link.setPosition(
self.__lenthCenter(vpoints[vlink[0]], vpoints[vlink[1]]))
print(vlink)
self.bodies.append(link)
self.joints = []
for name, vlink in self.vlinks.items():
link = list(vlink)
if name == 'ground':
for p in link:
if p in inputs:
print("input:", p)
j = ode.HingeJoint(self.world)
#j.attach(bodies[(vlinks[inputs[p][1]] - {p}).pop()], ode.environment)
j.attach(self.bodies[0], ode.environment)
j.setAxis((0, 0, 1))
j.setAnchor(self.__translate23d(vpoints[vlink[0]]))
j.setParam(ode.ParamVel, 2)
j.setParam(ode.ParamFMax, 22000)
else:
print("grounded:", p)
j = ode.BallJoint(self.world)
j.attach(self.bodies[p], ode.environment)
j.setAnchor(self.__translate23d(vpoints[vlink[1]]))
self.joints.append(j)
elif len(link) >= 2:
print("link:", link[0], link[1])
j = ode.BallJoint(self.world)
j.attach(self.bodies[link[0]], self.bodies[link[1]])
j.setAnchor(self.__translate23d(vpoints[link[0]]))
self.joints.append(j)
link = list(vlink)
print(link)
if name == 'ground':
for p in link:
if p in inputs:
print("input:", p)
j = ode.HingeJoint(world)
#j.attach(bodies[(vlinks[inputs[p][1]] - {p}).pop()], ode.environment)
j.attach(bodies[1], ode.environment)
j.setAxis((0, 0, 1))
j.setAnchor(bodies[0].getPosition())
j.setParam(ode.ParamVel, 2)
j.setParam(ode.ParamFMax, 22000)
else:
print("grounded:", p)
j = ode.BallJoint(world)
j.attach(bodies[p], ode.environment)
j.setAnchor(bodies[p].getPosition())
joints.append(j)
elif len(link) >= 2:
print("link:", link[0], link[1])
j = ode.BallJoint(world)
j.attach(bodies[link[0]], bodies[link[1]])
j.setAnchor(bodies[link[0]].getPosition())
joints.append(j)
# TODO : need to add select method in this joint type
for p in link[2:]:
print("other:", p, link[0], link[1])
for k in range(2):
j = ode.BallJoint(world)
j.attach(bodies[p], bodies[link[k]])
brasfunc() jambefunc() cuissefunc() avantbrasfunc() cuisseL_jambeL = ode.HingeJoint(world) cuisseL_jambeL.attach(cuisseL, jambeL) cuisseL_jambeL.setAnchor((0.5, 1, 0)) cuisseL_jambeL.setAxis((1, 0, 0)) cuisseG_jambeG = ode.HingeJoint(world) cuisseG_jambeG.attach(cuisseG, jambeG) cuisseG_jambeG.setAnchor((-0.5, 1, 0)) cuisseG_jambeG.setAxis((1, 0, 0)) tronc_cuisseG = ode.BallJoint(world) tronc_cuisseG.attach(cuisseG, tronc) tronc_cuisseG.setAnchor((-0.5, 2, 0)) tronc_cuisseL = ode.BallJoint(world) tronc_cuisseL.attach(cuisseL, tronc) tronc_cuisseL.setAnchor((0.5, 2, 0)) tronc_brasG = ode.BallJoint(world) tronc_brasG.attach(brasG, tronc) tronc_brasG.setAnchor((-1, 4, 0)) tronc_brasL = ode.BallJoint(world) tronc_brasL.attach(brasL, tronc) tronc_brasL.setAnchor((1, 4, 0))
def __init__(self, world, body1, body2, p1, p2, hinge=None):
# Cap on one side
cap1 = ode.Body(world)
cap1.color = (0, 128, 0, 255)
m = ode.Mass()
m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01)
cap1.setMass(m)
# set parameters for drawing the body
cap1.shape = "capsule"
cap1.length = .05
cap1.radius = .05
cap1.setPosition(p1)
# Attach the cap to the body
u1 = ode.BallJoint(world)
u1.attach(body1, cap1)
u1.setAnchor(p1)
u1.style = "ball"
# Cap on other side
cap2 = ode.Body(world)
cap2.color = (0, 128, 0, 255)
m = ode.Mass()
m.setCappedCylinderTotal(1.0, 3, 0.01, 0.01)
cap2.setMass(m)
# set parameters for drawing the body
cap2.shape = "capsule"
cap2.length = .05
cap2.radius = .05
cap2.setPosition(p2)
# Attach the cap to the body
u2 = ode.BallJoint(world)
u2.attach(body2, cap2)
u2.setAnchor(p2)
u2.style = "ball"
# The all-important slider joint
s = ode.SliderJoint(world)
s.attach(cap1, cap2)
s.setAxis(sub3(p1, p2))
s.setFeedback(True)
self.body1 = body1
self.body2 = body2
self.cap1 = cap1
self.cap2 = cap2
self.u1 = u1
self.u2 = u2
self.slider = s
self.gain = 1.0
self.neutral_length = dist3(p1, p2)
self.length_target = dist3(p1, p2)
if hinge is not None:
# Hinge is the joint this linear actuator controls
# This allows angular control
self.hinge = hinge
# Store these for later to save on math.
# TODO: I am being lazy and assuming u1->u2
# is orthogonal to the hinge axis
self.h_to_u1 = dist3(self.hinge.getAnchor(), self.u1.getAnchor())
self.h_to_u2 = dist3(self.hinge.getAnchor(), self.u2.getAnchor())
self.neutral_angle = thetaFromABC(self.h_to_u1, self.h_to_u2,
self.neutral_length)
body4.setPosition((2,2, 0)) # Connect body1 with the static environment #j1 = ode.BallJoint(world) #j1.attach(body1, ode.environment) #j1.setAnchor( (0,2,0) ) j1 = ode.HingeJoint(world) j1.attach(body1, ode.environment) j1.setAnchor( (0,0,0) ) j1.setAxis( (0,0,1) ) j1.setParam(ode.ParamVel, 0) j1.setParam(ode.ParamFMax, 220) # Connect body2 with body1 j2 = ode.BallJoint(world) j2.attach(body1, body2) j2.setAnchor( (2,0,0) ) j3 = ode.BallJoint(world) j3.attach(body2, body3) j3.setAnchor((2, 0, 0)) j4 = ode.BallJoint(world) j4.attach(body3, ode.environment) j4.setAnchor((2, 2, 0)) # Simulation loop... fps = 50
def __init__(self, bones: List[mmdpy_bone.mmdpyBone],
bodies: List[mmdpy_type.mmdpyTypePhysicsBody],
joints: List[mmdpy_type.mmdpyTypePhysicsJoint]):
self.bones: List[mmdpy_bone.mmdpyBone] = bones
self.bodies: List[mmdpy_type.mmdpyTypePhysicsBody] = bodies
self.joints: List[mmdpy_type.mmdpyTypePhysicsJoint] = joints
self.ode_bodies: List[Any] = []
self.ode_joints: List[Any] = []
self.ode_geoms: List[Any] = []
# 世界生成
self.world = ode.World()
self.world.setGravity([0, -9.81, 0])
self.world.setERP(0.80)
self.world.setCFM(1e-5)
# Create a space object
self.space = ode.Space()
# A joint group for the contact joints that are generated whenever
# two bodies collide
self.contactgroup = ode.JointGroup()
# 剛体
for i, body in enumerate(self.bodies):
body.cid = i
body.bone = self.bones[body.bone_id]
ode_body = ode.Body(self.world)
mass = ode.Mass()
density: float = 120.0
ode_geom = None
if body.type_id == 0: # 球
mass.setSphere(density, body.sizes[0])
ode_geom = ode.GeomSphere(self.space, radius=body.sizes[0])
if body.type_id == 1: # 箱
mass.setBox(density, body.sizes[0], body.sizes[1],
body.sizes[2])
ode_geom = ode.GeomBox(self.space, lengths=body.sizes)
if body.type_id == 2: # カプセル
mass.setCylinder(density, 2, body.sizes[0], body.sizes[1])
ode_geom = ode.GeomCylinder(self.space,
radius=body.sizes[0],
length=body.sizes[1])
mass.mass = (1 if body.calc == 0 else body.mass / density)
ode_body.setMass(mass)
if ode_geom is not None:
ode_geom.setBody(ode_body)
ode_body.setPosition(body.pos)
quat: np.ndarray = scipy.spatial.transform.Rotation.from_rotvec(
body.rot).as_matrix().reshape(9)
ode_body.setRotation(quat)
# debug
body.calc = 0
self.ode_bodies.append(ode_body)
self.ode_geoms.append(ode_geom)
# joint
# https://so-zou.jp/robot/tech/physics-engine/ode/joint/
for i, joint in enumerate(self.joints[:1]):
joint.cid = i
body_a = self.ode_bodies[joint.rigidbody_a]
body_b = self.ode_bodies[joint.rigidbody_b]
ode_joint = ode.BallJoint(self.world)
# ode_joint = ode.FixedJoint(self.world)
# ode_joint = ode.HingeJoint(self.world)
# ode_joint.attach(ode.environment, body_b)
ode_joint.attach(body_a, body_b)
ode_joint.setAnchor(joint.pos)
# ode_joint = ode.UniversalJoint(self.world)
# ode_joint.attach(body_a, body_b)
# ode_joint.setAnchor(joint.pos)
# ode_joint.setAxis1(joint.rot)
# ode_joint.setParam(ode.ParamLoStop, joint.)
self.ode_joints.append(ode_joint)
# # 接続確認
# print(
# self.bodies[joint.rigidbody_a].name,
# self.bodies[joint.rigidbody_b].name,
# ode.areConnected(body_a, body_b)
# )
# print(joint.pos, body_b.getPosition(), self.bodies[joint.rigidbody_b].pos)
# debug
self.bodies[joint.rigidbody_b].calc = 1
for i, body in enumerate(self.bodies):
ode_body = self.ode_bodies[i]
if body.calc == 0:
ode_joint = ode.BallJoint(self.world)
ode_joint.attach(ode.environment, ode_body)
ode_joint.setAnchor(body.pos)
self.ode_joints.append(ode_joint)
# 時刻保存
self.prev_time: float = time.time()
def create_objects(self, world):
print 'chassis'
# chassis
density = 10
lx, ly, lz = (8, 0.5, 8)
# Create body
body = ode.Body(world.world)
mass = ode.Mass()
mass.setBox(density, lx, ly, lz)
body.setMass(mass)
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (lx, ly, lz)
# Create a box geom for collision detection
geom = ode.GeomBox(world.space, lengths=body.boxsize)
geom.setBody(body)
#body.setPosition((0, 3, 0))
world.add_body(body)
world.add_geom(geom)
self._chassis_body = body
density = 1
print 'left wheel'
# left wheel
radius = 1
height = 0.2
px, py, pz = (lx / 2, 0, -(lz / 2))
left_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
wheel_mass.setCylinder(density, 1, radius, height)
left_wheel_body.setMass(wheel_mass)
#left_wheel_geom = ode.GeomSphere(world.space, radius=radius)
left_wheel_geom = ode.GeomCylinder(world.space, radius=radius,
length=height)
left_wheel_geom.setBody(left_wheel_body)
#left_wheel_body.setPosition((px, py, pz))
left_wheel_body.setRotation((0, 0, 1,
0, 1, 0,
-1, 0, 0))
left_wheel_body.setPosition((px - height / 2, py, pz))
world.add_body(left_wheel_body)
world.add_geom(left_wheel_geom)
print 'right wheel'
# right wheel
#radius = 1
px = -lx / 2
right_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
wheel_mass.setCylinder(density, 1, radius, height)
right_wheel_body.setMass(wheel_mass)
#right_wheel_geom = ode.GeomSphere(world.space, radius=radius)
right_wheel_geom = ode.GeomCylinder(world.space, radius=radius,
length=height)
right_wheel_geom.setBody(right_wheel_body)
#right_wheel_body.setPosition((px, py, pz))
right_wheel_body.setRotation((0, 0, 1,
0, 1, 0,
-1, 0, 0))
right_wheel_body.setPosition((px - height / 2, py, pz))
world.add_body(right_wheel_body)
world.add_geom(right_wheel_geom)
print 'front wheel'
# front wheel
#radius = 1
px, py, pz = (0, 0, lz / 2)
front_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setSphere(density, radius)
front_wheel_body.setMass(wheel_mass)
front_wheel_geom = ode.GeomSphere(world.space, radius=radius)
front_wheel_geom.setBody(front_wheel_body)
front_wheel_body.setPosition((px, py, pz))
world.add_body(front_wheel_body)
world.add_geom(front_wheel_geom)
#left_wheel_joint = ode.Hinge2Joint(world.world)
left_wheel_joint = ode.HingeJoint(world.world)
left_wheel_joint.attach(body, left_wheel_body)
left_wheel_joint.setAnchor(left_wheel_body.getPosition())
left_wheel_joint.setAxis((-1, 0, 0))
#left_wheel_joint.setAxis1((0, 1, 0))
#left_wheel_joint.setAxis2((1, 0, 0))
left_wheel_joint.setParam(ode.ParamFMax, 500000)
#left_wheel_joint.setParam(ode.ParamLoStop, 0)
#left_wheel_joint.setParam(ode.ParamHiStop, 0)
#left_wheel_joint.setParam(ode.ParamFMax2, 0.1)
#left_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2)
#left_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1)
self._left_wheel_joints.append(left_wheel_joint)
#right_wheel_joint = ode.Hinge2Joint(world.world)
right_wheel_joint = ode.HingeJoint(world.world)
right_wheel_joint.attach(body, right_wheel_body)
right_wheel_joint.setAnchor(right_wheel_body.getPosition())
right_wheel_joint.setAxis((-1, 0, 0))
#right_wheel_joint.setAxis1((0, 1, 0))
#right_wheel_joint.setAxis2((1, 0, 0))
right_wheel_joint.setParam(ode.ParamFMax, 500000)
#right_wheel_joint.setParam(ode.ParamLoStop, 0)
#right_wheel_joint.setParam(ode.ParamHiStop, 0)
#right_wheel_joint.setParam(ode.ParamFMax2, 0.1)
#right_wheel_joint.setParam(ode.ParamSuspensionERP, 0.2)
#right_wheel_joint.setParam(ode.ParamSuspensionCFM, 0.1)
self._right_wheel_joints.append(right_wheel_joint)
front_wheel_joint = ode.BallJoint(world.world)
front_wheel_joint.attach(body, front_wheel_body)
front_wheel_joint.setAnchor(front_wheel_body.getPosition())
front_wheel_joint.setParam(ode.ParamFMax, 5000)
def _createODEjoint(self):
# Create the ODE joint
self.odejoint = ode.BallJoint(self.odedynamics.world)
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 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()
# Create two bodies body1 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body1.setMass(M) body1.setPosition((1, 2, 0)) body2 = ode.Body(world) M = ode.Mass() M.setSphere(2500, 0.05) body2.setMass(M) body2.setPosition((2, 2, 0)) # Connect body1 with the static environment j1 = ode.BallJoint(world) j1.attach(body1, ode.environment) j1.setAnchor((0, 2, 0)) # Connect body2 with body1 j2 = ode.BallJoint(world) j2.attach(body1, body2) j2.setAnchor((1, 2, 0)) # Simulation loop... fps = 50 dt = 1.0 / fps loopFlag = True clk = pygame.time.Clock()
def loaddata(self, vpoints):
self.world = ode.World()
self.world.setGravity((0, -9.81, 0))
#vpoints = parse_vpoints(mechanism)
self.vlinks = {}
for i, vpoint in enumerate(vpoints):
for link in vpoint.links:
if link in self.vlinks:
self.vlinks[link].add(i)
else:
self.vlinks[link] = {i}
print(self.vlinks)
self.bodies = []
for i, vpoint in enumerate(vpoints):
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(250, 0.05)
body.setMass(M)
x, y = vpoint
body.setPosition((x, y, 0))
self.bodies.append(body)
self.bodies[0].setGravityMode(False)
self.joints = []
for name, vlink in self.vlinks.items():
link = list(vlink)
#print(link)
if name == 'ground':
for p in link:
if p in inputs:
#print("input:", p)
j = ode.HingeJoint(self.world)
#j.attach(bodies[(vlinks[inputs[p][1]] - {p}).pop()], ode.environment)
j.attach(self.bodies[1], ode.environment)
j.setAxis((0, 0, 1))
j.setAnchor(self.bodies[0].getPosition())
j.setParam(ode.ParamVel, 2)
j.setParam(ode.ParamFMax, 22000)
else:
#print("grounded:", p)
j = ode.BallJoint(self.world)
j.attach(self.bodies[p], ode.environment)
j.setAnchor(self.bodies[p].getPosition())
self.joints.append(j)
elif len(link) >= 2:
#print("link:", link[0], link[1])
j = ode.BallJoint(self.world)
j.attach(self.bodies[link[0]], self.bodies[link[1]])
j.setAnchor(self.bodies[link[0]].getPosition())
self.joints.append(j)
# TODO : need to add select method in this joint type
for p in link[2:]:
#print("other:", p, link[0], link[1])
for k in range(2):
j = ode.BallJoint(self.world)
j.attach(self.bodies[p], self.bodies[link[k]])
self.joints.append(j)
