def __init__(self,
world,
space,
node1,
node2,
radius=0.1,
mass=1.0,
rest_length=1,
elasticity=0,
color=(100., 0., 100.),
fixed=False):
self.nodes = [node1, node2]
self.radius = radius
self.mass = mass
self.rest_length = rest_length
self.elasticity = elasticity
self.color = color
self.joints = []
is_strut = elasticity == 0
self.body = ode.Body(world)
positions = [node.get_position() for node in self.nodes]
center = [(positions[0][i] + positions[1][i]) / 2
for i in range(len(positions[0]))]
lengths = [(positions[1][i] - positions[0][i])
for i in range(len(positions[0]))]
length = math.sqrt(dot(lengths, lengths))
self.update()
print("lengths=", lengths)
print("center=", center)
if is_strut:
geom_length = 0.9 * (length - (node1.radius + node2.radius))
self.geom = ode.GeomBox(space,
lengths=[radius, geom_length, radius])
self.geom.setBody(self.body)
for node in self.nodes:
joint = ode.FixedJoint(world)
joint.attach(self.body, node.body)
joint.setFixed()
self.joints.append(joint)
if fixed:
joint = ode.FixedJoint(world)
joint.attach(self.body, ode.environment)
joint.setFixed()
self.joints.append(joint)
def reset(self):
# Object
self.obj.setPosition(0, 0, 0)
# Wall
for i in range(nWall):
self.wall[i] = Box(self.space, self.world,
(WALL_LENGTH[i], WALL_THICK, WALL_TALL), None,
[0.3, 0.7, 0.1])
self.wall[i].setPosition(WALL_POS[i, 0], WALL_POS[i, 1],
WALL_TALL / 2)
if WALL_DIR[i] == 1:
Q = axisangle_to_quat(np.array([0, 0, 1]), np.pi / 2)
self.wall[i].setQuat(*Q)
# Robots
for i in range(4):
self.robot[i].setPosition(0.33 - i * 0.22, 0.13, ROBOT_RADIUS)
for i in range(4, 8):
self.robot[i].setPosition(0.33 - (i - 4) * 0.22, -0.13,
ROBOT_RADIUS)
for i in range(8, 10):
self.robot[i].setPosition(0.33, 0.047 - (i - 8) * 0.087,
ROBOT_RADIUS)
for i in range(10, 12):
self.robot[i].setPosition(-0.33, 0.047 - (i - 10) * 0.087,
ROBOT_RADIUS)
for i in range(self.n_robots):
self.joint[i] = ode.FixedJoint(self.world)
self.joint[i].attach(self.obj.body, self.robot[i].body)
self.joint[i].setFixed()
# Enemy bot
for ebot in self.enemy_bot:
loc = (self.np_random.rand(), self.np_random.rand())
ebot.setPosition(loc[0], loc[1], ROBOT_RADIUS)
def __init__(self, gravity=9.8, mass=1.0, tau=0.02, size_pole=(1.0, .1)):
self.gravity = gravity
self.mass = mass
self.dt = tau
self.viewer = None
self.viewerSize = 500
self.spaceSize = 7.
self.size_pole = size_pole
self.max_force = 3.
self.theta_threshold = 3.2
self.action_space = spaces.Box(low=-self.max_force,
high=self.max_force,
shape=(1, ))
high = np.array([self.theta_threshold * 2, np.finfo(np.float32).max])
self.observation_space = spaces.Box(-high, high)
self.world = ode.World()
self.world.setGravity((0, -9.81, 0))
self.body1 = ode.Body(self.world)
self.body2 = ode.Body(self.world)
self.create_basebox(self.body1, (0., 0., 0.))
self.create_link(self.body2, (0., 0.5, 0.), self.mass, size_pole)
self.space = ode.Space()
self.j1 = ode.FixedJoint(self.world)
self.j1.attach(self.body1, ode.environment)
self.j1.setFixed()
self.j2 = ode.HingeJoint(self.world)
self.j2.attach(self.body1, self.body2)
self.j2.setAnchor((0., 0., 0.))
self.j2.setAxis((0, 0, -1))
self.j2.setFeedback(1)
def create_tensegrity():
global nodes, links, node_bodies
nodes, links = load_spr()
for node in nodes:
endcap, geom = create_sphere(world, space, 10, .2)
endcap.setPosition([val / 50000.0 for val in node.position])
node_bodies.append(endcap)
bodies.append(endcap)
print("link length=", len(links))
for link in links:
is_strut = link.elasticity > 200
if link.elasticity > 100:
end_points = [nodes[node_id].position for node_id in link.nodes]
dx = end_points[0][0] - end_points[1][0]
dy = end_points[0][1] - end_points[1][1]
dz = end_points[0][2] - end_points[1][2]
length = sqrt(dx * dx + dy * dy + dz * dz)
if is_strut:
bar, geom = create_box(world, space, 1, .2, length / 50000.0, .2)
else:
bar, geom = create_box(world, space, 1, .05, length / 50000.0, .05)
center = [(end_points[0][i] + end_points[1][i]) / 100000.0 for i in range(3)]
bar.setPosition(center)
c1 = sqrt(dx * dx + dy * dy);
s1 = dz;
c2 = dx / c1 if c1 != 0 else 1.0
s2 = dy / c1 if c1 != 0 else 0.0
quat = rotation_quat([0, 1, 0], normalize([dx, dy, dz]))
bar.setQuaternion(quat)
bodies.append(bar)
link_bodies.append(bar)
if is_strut:
joint = ode.FixedJoint(world)
joint.attach(bar, node_bodies[link.nodes[0]])
joint.setFixed()
joints.append(joint)
joint = ode.FixedJoint(world)
joint.attach(bar, node_bodies[link.nodes[1]])
joint.setFixed()
joints.append(joint)
geoms.append(geom)
def create_fixed_joint(sim, body1, body2=ode.environment):
"""
Create a fixed joint between two bodies.
"""
fixed = ode.FixedJoint(sim.world)
fixed.attach(body1, body2)
fixed.setFixed()
sim.fixed.append(fixed)
def addFixedJoint(self, body1, body2):
joint = ode.FixedJoint(self.sim.world)
joint.attach(body1, body2)
joint.setFixed()
joint.style = "fixed"
self.joints.append(joint)
return joint
def __init__(self):
self.dt=.005
self.viewer = None
self.viewerSize = 500
self.spaceSize = 6.4
self.resolution = self.viewerSize/self.spaceSize
self.init_rod_template()
self.seed()
self.world = ode.World()
#self.world.setGravity((0,-9.81,0))
self.world.setGravity((0,0,0))
self.body1 = ode.Body(self.world)
self.body2 = ode.Body(self.world)
self.body3 = ode.Body(self.world)
self.body4 = ode.Body(self.world)
self.create_link(self.body1,(0.5,0,0))
self.create_link(self.body2,(1.5,0,0))
self.create_link(self.body3,(2.5,0,0))
self.space = ode.Space()
self.body4_col = ode.GeomSphere(self.space,radius=0.1)
self.create_ee(self.body4,(3,0,0),self.body4_col)
# Connect body1 with the static environment
self.j1 = ode.HingeJoint(self.world)
self.j1.attach(self.body1, ode.environment)
self.j1.setAnchor( (0,0,0) )
self.j1.setAxis( (0,0,1) )
self.j1.setFeedback(1)
# Connect body2 with body1
self.j2 = ode.HingeJoint(self.world)
self.j2.attach(self.body1, self.body2)
self.j2.setAnchor( (1,0,0) )
self.j2.setAxis( (0,0,-1) )
self.j2.setFeedback(1)
#connect body3 with body2
self.j3 = ode.HingeJoint(self.world)
self.j3.attach(self.body2, self.body3)
self.j3.setAnchor( (2,0,0) )
self.j3.setAxis( (0,0,-1) )
self.j3.setFeedback(1)
#connect end effector
self.j4 = ode.FixedJoint(self.world)
self.j4.attach(self.body3,self.body4)
self.j4.setFixed()
self.controlMode = "POS"
self.targetPos = self.rand_target()
self.targetTime = 0
self.P_gains = np.array([1000,1000,1000])
self.D_gains = np.array([70,50,20])
def _parseFixedJoint(self, world, link1, link2):
def start(name, attrs):
raise errors.ChildError('fixed', name)
def end(name):
if (name == 'fixed'):
self._parser.pop()
joint = ode.FixedJoint(world, self._jg)
joint.attach(link1, link2)
self.setODEObject(joint)
self._parser.push(startElement=start, endElement=end)
def construct(self): self.body = ode.Body(objects.world) x, y, _ = self.helper.getPosition() self.body.setPosition((x, y, 0)) M = ode.Mass() M.setSphereTotal(self.helper.strong_mass, self.helper.rad) self.body.setMass(M) objects.ODEThing.construct(self) self.body.setAngularVel(self.helper.getAngularVel()) self.body.setLinearVel(self.helper.body.getLinearVel()) self.fixed_joint = ode.FixedJoint(objects.world, self.jointgroup) self.fixed_joint.attach(self.body, self.helper.body) self.fixed_joint.setFixed()
def build_world():
# Create the world through ode
density = 1
start_up = True
world = ode.World()
world.setGravity((0, -gravity, 0))
# Make the cart
cart = ode.Body(world)
M = ode.Mass()
M.setBox(density, cart_width, cart_height, cart_height)
M.mass = cart_mass
cart.setMass(M)
cart.setPosition((0, 0, 0))
print 'cart mass = ', cart.getMass().mass
# Make a heavy ball
ball = ode.Body(world)
M = ode.Mass()
M.setSphere(density, 0.5)
M.mass = ball_mass
ball.setMass(M)
ball.setPosition((0, 1, 0))
# And a rod with a negligible weight
rod = ode.Body(world)
M = ode.Mass()
M.setCylinder(0.01, 2, 0.01, rod_length)
M.mass = 1e-2
rod.setMass(M)
rod.setPosition((0, 0.5, 0))
## Connect the cart to the world through a slider joint
cart_joint = ode.SliderJoint(world)
cart_joint.setAxis((1, 0, 0))
cart_joint.attach(cart, ode.environment)
# Connect the rod with the cart through a Hinge joint.
pendulum_joint = ode.HingeJoint(world)
pendulum_joint.attach(rod, cart)
pendulum_joint.setAnchor((0, 0, 0))
pendulum_joint.setAxis((0, 0, 1))
# Connect rod with ball with a fixed joint
rod_ball_joint = ode.FixedJoint(world)
rod_ball_joint.attach(rod, ball)
rod_ball_joint.setFixed()
return world, cart, ball, rod, pendulum_joint, cart_joint, rod_ball_joint
def __init__(self,
world,
space,
pos,
color,
capsules,
radius,
mass=2,
fixed=False,
orientation=v(1, 0, 0, 0)):
"capsules is a list of (start, end) points"
self.capsules = capsules
self.body = ode.Body(world)
self.body.setPosition(pos)
self.body.setQuaternion(orientation)
m = ode.Mass()
# computing MOI assuming sphere with .5 m radius
m.setSphere(mass / (4 / 3 * math.pi * .5**3),
.5) # setSphereTotal is broken
self.body.setMass(m)
self.geoms = []
self.geoms2 = []
for start, end in capsules:
self.geoms.append(ode.GeomTransform(space))
x = ode.GeomCapsule(None, radius, (end - start).mag())
self.geoms2.append(x)
self.geoms[-1].setGeom(x)
self.geoms[-1].setBody(self.body)
x.setPosition((start + end) / 2 + v(random.gauss(
0, .01), random.gauss(0, .01), random.gauss(0, .01)))
a = (end - start).unit()
b = v(0, 0, 1)
x.setQuaternion(
sim_math_helpers.axisangle_to_quat((a % b).unit(),
-math.acos(a * b)))
self.color = color
self.radius = radius
if fixed:
self.joint = ode.FixedJoint(world)
self.joint.attach(self.body, None)
self.joint.setFixed()
def create_fixed(self, key, pos, bod):
""" Create a fixed joint with the world.
Arguments:
key : number id to assign the hinge
pos : position of the joint
bod : body to attach the world to
"""
# Auto label the joint key or not.
key = len(self.joints) if key == -1 else key
j = ode.FixedJoint(self.world)
j.attach(self.bodies[bod], ode.environment)
j.setFixed()
j.style = "fixed"
self.joints[key] = j
return key
def do_joint_motor(self, jointtype, record=0):
b = TestBodyPartGraph(new_network_args, 2, jointtype)
b.bodyparts[0].rotation = (0, (1, 0, 0))
b.bodyparts[1].rotation = (math.pi / 2, (0, 1, 0))
for i in 0, 1:
b.bodyparts[i].motor_input = None
s = sim.BpgSim(SECONDS)
s.relaxed = 1
s.add(b)
s.bpgs[0].bodyparts[0].motor_input = None
s.bpgs[0].bodyparts[1].motor_input = None
s.world.setGravity((0, 0, 0))
s.space.getGeom(0).getBody()
b1 = s.space.getGeom(1).getBody()
s.moveBps(0, 0, 5)
# fix bp so it cant move
j = ode.FixedJoint(s.world)
j.attach(b1, ode.environment)
j.setFixed()
# start motor
m = s.bpgs[0].bodyparts[1].motor
motor_data = m.log('test/' + jointtype)
m.dangle[0] = math.pi / 2
m.dangle[1] = math.pi / 4
m.dangle[2] = -math.pi / 2
s.initSignalLog('test/signal.log')
if not record:
runSim(s)
else:
runSim(s, 1, '%s/record_test.avi' % TESTDIR)
m.endlog()
t = Template(file='plot_motor.t')
t.data = motor_data
t.out = motor_data[:-4] + '.pdf'
t.joint = jointtype
f = open('tmp.r', 'w')
f.write(t.respond())
f.close()
os.system('R -q --no-save < tmp.r >> r.out 2>&1')
def __init__(self, world, position=(0.0, 0.0, 0.0)):
#stores the initial position
self.initPos = position
color = getColor()
#inits the cube with player position, mass, size and color
objects.Cube.__init__(self, world, position, 50, (2.0, 2.0, 2.0),
getRGBValues(color))
self.geom.name = self.setName(color)
#inits the trigger with according values
#trigger box is around player cube
self.trigger = TriggerBox(world, position, (2.2, 2.2, 2.2),
self.geom.name)
#joins the player body with the trigger
self.j = ode.FixedJoint(world)
self.j.attach(self.body, self.trigger.body)
self.j.setFixed()
self.min = [0, 0]
self.max = [0, 0]
#gets the largest and smallest coordinates of the players square
a = 10.0 * position[0] / abs(position[0])
b = 10.0 * position[2] / abs(position[2])
#stores the player`s square min and max coordinates
if a > 0:
self.max[0] = a
else:
self.min[0] = a
if b > 0:
self.max[1] = b
else:
self.min[1] = b
self.camera = None
self.setAngle()
def __init__(self,
world,
space,
name="",
position=(0., 0., 0.),
velocity=(0., 0., 0.),
radius=0.5,
mass=1.0,
color=(100., 0., 100.),
fixed=False):
radius *= 2
self.name = name
self.radius = radius
self.mass = mass
self.color = color
self.joints = []
self.body = ode.Body(world)
M = ode.Mass()
M.setSphereTotal(mass, radius)
self.body.name = name
self.body.shape = "sphere"
self.body.radius = radius
self.body.setMass(M)
self.body.setPosition(position)
self.body.setLinearVel(velocity)
#self.geom = ode.GeomSphere(space, radius=self.body.radius)
#self.geom = ode.GeomBox(space, lengths=(1, 1, 1))
#self.geom.setBody(self.body)
if fixed:
joint = ode.FixedJoint(world)
joint.attach(self.body, ode.environment)
joint.setFixed()
self.joints.append(joint)
def _createODEjoint(self):
# Create the ODE joint
self.odejoint = ode.FixedJoint(self.odedynamics.world)
def _loadObjects(self):
"""
Spawns the Differential Drive robot based on the input pose.
"""
# Create a Box
box_pos = [
self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2]
]
rot = self.baserot
boxbody = ode.Body(self.world)
boxgeom = ode.GeomBox(space=self.space,
lengths=(self.cubesize, self.cubesize * 0.75,
self.cubesize))
boxgeom.setBody(boxbody)
boxgeom.setPosition(box_pos)
boxgeom.setRotation(rot)
boxM = ode.Mass()
boxM.setBox(self.cubemass, self.cubesize, self.cubesize * 0.75,
self.cubesize)
boxbody.setMass(boxM)
# Create two Cylindrical Wheels
rot = self.multmatrix(self.genmatrix(math.pi / 2, 2), rot)
leftwheel_pos = [
box_pos[0] + 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5,
box_pos[2] + self.cubesize * 0.2
]
rightwheel_pos = [
box_pos[0] - 0.5 * self.track, box_pos[1] - self.wheelradius * 0.5,
box_pos[2] + self.cubesize * 0.2
]
leftwheelbody = ode.Body(self.world)
leftwheelgeom = ode.GeomCylinder(space=self.space,
radius=self.wheelradius,
length=self.wheelradius / 3.0)
leftwheelgeom.setBody(leftwheelbody)
leftwheelgeom.setPosition(leftwheel_pos)
leftwheelgeom.setRotation(rot)
leftwheelM = ode.Mass()
leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius,
self.wheelradius / 3.0)
leftwheelbody.setMass(leftwheelM)
lefthinge = ode.HingeJoint(self.world)
lefthinge.attach(leftwheelbody, boxbody)
lefthinge.setAnchor(leftwheel_pos)
lefthinge.setAxis(self.rotate((0, 0, 1), rot))
lefthinge.setParam(ode.ParamFMax, self.hingemaxforce)
rightwheelbody = ode.Body(self.world)
rightwheelgeom = ode.GeomCylinder(space=self.space,
radius=self.wheelradius,
length=self.wheelradius)
rightwheelgeom.setBody(rightwheelbody)
rightwheelgeom.setPosition(rightwheel_pos)
rightwheelgeom.setRotation(rot)
rightwheelM = ode.Mass()
rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius,
self.cubemass / 3.0)
rightwheelbody.setMass(rightwheelM)
righthinge = ode.HingeJoint(self.world)
righthinge.attach(rightwheelbody, boxbody)
righthinge.setAnchor(rightwheel_pos)
righthinge.setAxis(self.rotate((0, 0, 1), rot))
righthinge.setParam(ode.ParamFMax, self.hingemaxforce)
# Create a spherical wheel at the back for support
caster_pos = [
box_pos[0], box_pos[1] - self.cubesize * 0.5,
box_pos[2] - self.cubesize * 0.5
]
casterbody = ode.Body(self.world)
castergeom = ode.GeomSphere(space=self.space,
radius=self.cubesize / 5.0)
castergeom.setBody(casterbody)
castergeom.setPosition(caster_pos)
castergeom.setRotation(rot)
casterM = ode.Mass()
casterM.setSphere(self.cubemass * 100.0, self.cubesize / 5.0)
casterbody.setMass(casterM)
self.fixed = ode.FixedJoint(self.world)
self.fixed.attach(casterbody, boxbody)
self.fixed.setFixed()
# WHEW, THE END OF ALL THAT FINALLY!
# Build the Geoms and Joints arrays for rendering.
self.boxgeom = boxgeom
self._geoms = [
boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground
]
self.lefthinge = lefthinge
self.righthinge = righthinge
self._joints = [self.lefthinge, self.righthinge, self.fixed]
def loadObstacles(self, obstaclefile):
"""
Loads obstacles from the obstacle text file.
"""
# Initiate data structures.
data = open(obstaclefile, "r")
obs_sizes = []
obs_positions = []
obs_masses = []
reading = "None"
# Parse the obstacle text file.
for line in data:
linesplit = line.split()
if linesplit != []:
if linesplit[0] != "#":
obs_sizes.append([
float(linesplit[0]),
float(linesplit[1]),
float(linesplit[2])
])
obs_positions.append([
float(linesplit[3]),
float(linesplit[4]),
float(linesplit[5])
])
obs_masses.append(float(linesplit[6]))
# Go through all the obstacles in the list and spawn them.
for i in range(len(obs_sizes)):
obs_size = obs_sizes[i]
obs_pos = obs_positions[i]
obs_mass = obs_masses[i]
# Create the obstacle.
body = ode.Body(self.world)
geom = ode.GeomBox(space=self.space, lengths=obs_size)
geom.setBody(body)
geom.setPosition(obs_pos)
M = ode.Mass()
M.setBox(obs_mass, obs_size[0], obs_size[1], obs_size[2])
body.setMass(M)
# Append all these new pointers to the simulator class.
self._geoms.append(geom)
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 fixRootBody(self):
self.fixedJoint = ode.FixedJoint(self.world)
self.fixedJoint.attach(self.rootNode.body, ode.environment)
self.fixedJoint.setFixed()
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 SetFixed(self):
"""Fix this body to the ode world using a FixedJoint."""
self._fixedJoint = ode.FixedJoint(self._world)
self._fixedJoint.attach(self, ode.environment)
self._fixedJoint.setFixed()
def __init__(self):
self.dt = .005
self.viewer = None
self.viewerSize = 800
self.spaceSize = 8.4
self.resolution = self.viewerSize / self.spaceSize
self.perspective_transform_on = False
self.init_rod_template()
self._seed()
self.world = ode.World()
self.world.setGravity((0, 0, 0))
self.body1 = ode.Body(self.world)
self.body2 = ode.Body(self.world)
self.body3 = ode.Body(self.world)
self.body4 = ode.Body(self.world)
self.body5 = ode.Body(self.world)
self.body6 = ode.Body(self.world)
self.body7 = ode.Body(self.world)
self.body8 = ode.Body(self.world)
self.create_link(self.body1, (0.3, 0, 0))
self.create_link(self.body2, (.9, 0, 0))
self.create_link(self.body3, (1.5, 0, 0))
self.create_link(self.body4, (2.1, 0, 0))
self.create_link(self.body5, (2.7, 0, 0))
self.create_link(self.body6, (3.3, 0, 0))
self.create_link(self.body7, (3.9, 0, 0))
self.space = ode.Space()
self.body8_col = ode.GeomSphere(self.space, radius=0.1)
self.create_ee(self.body8, (4.2, 0, 0), self.body8_col)
self.colours = []
self.dir = 1.01
# Connect body1 with the static environment
self.j1 = ode.HingeJoint(self.world)
self.j1.attach(self.body1, ode.environment)
self.j1.setAnchor((0, 0, 0))
self.j1.setAxis((0, -1, 0))
self.j1.setFeedback(1)
# Connect body2 with body1
self.j2 = ode.HingeJoint(self.world)
self.j2.attach(self.body1, self.body2)
self.j2.setAnchor((0.6, 0, 0))
self.j2.setAxis((0, 0, -1))
self.j2.setFeedback(1)
# connect body3 with body2
self.j3 = ode.HingeJoint(self.world)
self.j3.attach(self.body2, self.body3)
self.j3.setAnchor((1.2, 0, 0))
self.j3.setAxis((0, 0, -1))
self.j3.setFeedback(1)
# connect body4 with body3
self.j4 = ode.HingeJoint(self.world)
self.j4.attach(self.body3, self.body4)
self.j4.setAnchor((1.8, 0, 0))
# self.j4.setAxis( (0,0,-1) )
self.j4.setAxis((0, 1, 0))
self.j4.setFeedback(1)
# connect body5 with body4
self.j5 = ode.HingeJoint(self.world)
self.j5.attach(self.body4, self.body5)
self.j5.setAnchor((2.4, 0, 0))
# self.j4.setAxis( (0,0,-1) )
self.j5.setAxis((0, 1, 0))
self.j5.setFeedback(1)
# connect body6 with body5
self.j6 = ode.HingeJoint(self.world)
self.j6.attach(self.body5, self.body6)
self.j6.setAnchor((3.0, 0, 0))
# self.j4.setAxis( (0,0,-1) )
self.j6.setAxis((0, 0, -1))
self.j6.setFeedback(1)
# connect body4 with body3
self.j7 = ode.HingeJoint(self.world)
self.j7.attach(self.body6, self.body7)
self.j7.setAnchor((3.6, 0, 0))
# self.j4.setAxis( (0,0,-1) )
self.j7.setAxis((0, 1, 0))
self.j7.setFeedback(1)
# connect end effector
self.j8 = ode.FixedJoint(self.world)
self.j8.attach(self.body7, self.body8)
self.j8.setFixed()
self.controlMode = "POS"
self.targetPos = self.rand_target()
self.targetTime = 0
self.targetTime2 = 0
self.success = 0
self.fail = 0
self.P_gains = np.array([1000, 1000, 1000, 1000, 1000, 1000, 1000])
self.D_gains = np.array([70, 50, 40, 20, 15, 10, 5])
def set_fixed(self):
sim = self.sim
joint = ode.FixedJoint(sim.world)
joint.attach(self.body, None)
joint.setFixed()
self._fixed_joint = joint
def loadConfig(self, filename, reload=False):
# parameters are given in (our own brand of) config-file syntax
self.config = ConfigGrabber(filename,
sectionId="<!--odeenvironment parameters",
delim=("<", ">"))
# <passpairs>
self.passpairs = []
for passpairstring in self.config.getValue("passpairs")[:]:
self.passpairs.append(eval(passpairstring))
if self.verbosity > 0:
print("-------[pass tuples]--------")
print((self.passpairs))
print("----------------------------")
# <centerOn>
# set focus of camera to the first object specified in the section, if any
if self.render:
try:
self.centerOn(self.config.getValue("centerOn")[0])
except IndexError:
pass
# <affixToEnvironment>
for jointName in self.config.getValue("affixToEnvironment")[:]:
try:
# find first object with that name
obj = self.root.namedChild(jointName).getODEObject()
except IndexError:
print(("ERROR: Could not affix object '" + jointName +
"' to environment!"))
sys.exit(1)
if isinstance(obj, ode.Joint):
# if it is a joint, use this joint to fix to environment
obj.attach(obj.getBody(0), ode.environment)
elif isinstance(obj, ode.Body):
# if it is a body, create new joint and fix body to environment
j = ode.FixedJoint(self.world)
j.attach(obj, ode.environment)
j.setFixed()
# <colors>
for coldefstring in self.config.getValue("colors")[:]:
# ('name', (0.3,0.4,0.5))
objname, coldef = eval(coldefstring)
for (body, _) in self.body_geom:
if hasattr(body, 'name'):
if objname == body.name:
body.color = coldef
break
if not reload:
# add the JointSensor as default
self.sensors = []
## self.addSensor(self._jointSensor)
# <sensors>
# expects a list of strings, each of which is the executable command to create a sensor object
# example: DistToPointSensor('legSensor', (0.0, 0.0, 5.0))
sens = self.config.getValue("sensors")[:]
for s in sens:
try:
self.addSensor(eval('sensors.' + s))
except AttributeError:
print((dir(sensors)))
warnings.warn("Sensor name with name " + s +
" not found. skipped.")
else:
for s in self.sensors:
s._connect(self)
for a in self.actuators:
a._connect(self)
def __init__(self, **kwargs):
self.gui = None
self.boat = {
'x': 0.0,
'y': 0.0,
'speed': 0.0,
'direction': 0.0,
'tiler': 0.0
}
self.odeBody = ob()
self.odeWorld = ode.World()
self.odeWorld.setGravity((0, -9.81, 0))
self.odeBoat = ode.Body(self.odeWorld)
mas = ode.Mass()
mas.setBox(10, 10, 10, 10)
mas.mass = 1.0
self.odeBoat.setMass(mas)
self.odeBoat.setPosition((self.boat['x'], 0, self.boat['y']))
self.odeR = ode.Body(self.odeWorld)
M = ode.Mass()
M.setBox(10, 10, 10, 10)
M.mass = 1.0
self.odeR.setMass(M)
self.odeR.setPosition((self.boat['x'] + 2, 0, self.boat['y']))
self.odeJR = ode.FixedJoint(self.odeWorld)
self.odeJR.setFeedback(True)
self.odeJR.attach(self.odeBoat, self.odeR)
self.odeJR.setFixed()
#j.setAnchor( (2.0,0,0) )
self.dirAvg = self.boat['direction']
self.heewCounter = 0.0
self.pitchCounter = 0.0
self.k = {'up': False, 'down': False, 'left': False, 'right': False}
self.canvas = RenderContext(compute_normal_mat=True)
self.canvas.shader.source = resource_find('simple.glsl')
self.d3h = D3Helper()
self.d3h.loadObj([
[
'boat', "./3dModels/boat2_3dex_boat.obj",
"./3dModels/oiysh_profile2.jpeg"
],
['roseta', './3dModels/3d_roseta.obj', ''],
[
'genoa', "./3dModels/boat2_3dex_sailGenoa_onPower.obj",
"./3dModels/3d_texture_genoa.jpg"
],
[
'main', "./3dModels/boat2_3dex_sailMain_sailup_onPower.obj",
"./3dModels/IMG_5410.jpg"
],
['boom', "./3dModels/boat2_3dex_boom.obj", ""],
['ruder', "./3dModels/boat2_3dex_ruder.obj", ""],
['tiler', "./3dModels/boat2_3dex_tiler.obj", ""],
#['hdr1','./3dModels/hdr1.obj','./3dModels/hdr1.png'],
['xAxis', './3dModels/x-axis.obj', './3dModels/red.png'],
['yAxis', './3dModels/y-axis.obj', './3dModels/green.png'],
['zAxis', './3dModels/z-axis.obj', './3dModels/blue.png'],
['box', './3dModels/3d_box.obj', ''],
[
'waterTile', './3dModels/waterTile.obj',
"./3dModels/IMG_5410.jpg"
],
[
'hdr2', './3dModels/hdr2.obj',
"./3dModels/PANO_20160311_160253.jpg"
],
['kap2', './3dModels/tile.obj', "./3dModels/kap2.png"],
["infWat", "./3dModels/infinityWater.obj", ""]
])
super(d3tex2, self).__init__(**kwargs)
with self.canvas:
self.cb = Callback(self.setup_gl_context)
PushMatrix()
self.setup_scene()
PopMatrix()
self.cb = Callback(self.reset_gl_context)
if d3tex2RunAlone:
self.on_displayNow()
Window.bind(on_key_down=self.on_key_down)
Window.bind(on_key_up=self.on_key_up)
Window.bind(on_touch_down=self.on_touchDown)
Window.bind(on_touch_move=self.on_touchMove)
Window.bind(on_touch_up=self.on_touchUp)
def makeFJ(self, o0, o1):
print("makeFJ")
odeJR = ode.FixedJoint(self.odeWorld)
odeJR.setFeedback(True)
odeJR.attach(o0, o1)
return odeJR
def _loadObjects(self): """ Spawns the Differential Drive robot based on the input pose. """ # Create a Box box_pos = [self.basepos[0], self.basepos[1] + self.cubesize, self.basepos[2]] rot = self.baserot boxbody = ode.Body(self.world) boxgeom = ode.GeomBox(space=self.space, lengths=(self.cubesize, self.cubesize*0.75, self.cubesize) ) boxgeom.setBody(boxbody) boxgeom.setPosition(box_pos) boxgeom.setRotation(rot) boxM = ode.Mass() boxM.setBox(self.cubemass,self.cubesize,self.cubesize*0.75,self.cubesize) boxbody.setMass(boxM) # Create two Cylindrical Wheels rot = self.multmatrix(self.genmatrix(math.pi/2,2),rot) leftwheel_pos = [box_pos[0] + 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] rightwheel_pos = [box_pos[0] - 0.5*self.track, box_pos[1] - self.wheelradius*0.5, box_pos[2] + self.cubesize*0.2] leftwheelbody = ode.Body(self.world) leftwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius/3.0 ) leftwheelgeom.setBody(leftwheelbody) leftwheelgeom.setPosition(leftwheel_pos) leftwheelgeom.setRotation(rot) leftwheelM = ode.Mass() leftwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.wheelradius/3.0) leftwheelbody.setMass(leftwheelM) lefthinge = ode.HingeJoint(self.world) lefthinge.attach(leftwheelbody,boxbody) lefthinge.setAnchor(leftwheel_pos) lefthinge.setAxis(self.rotate((0,0,1),rot)) lefthinge.setParam(ode.ParamFMax,self.hingemaxforce) rightwheelbody = ode.Body(self.world) rightwheelgeom = ode.GeomCylinder(space=self.space, radius = self.wheelradius, length = self.wheelradius ) rightwheelgeom.setBody(rightwheelbody) rightwheelgeom.setPosition(rightwheel_pos) rightwheelgeom.setRotation(rot) rightwheelM = ode.Mass() rightwheelM.setCappedCylinder(self.wheelmass, 1, self.wheelradius, self.cubemass/3.0) rightwheelbody.setMass(rightwheelM) righthinge = ode.HingeJoint(self.world) righthinge.attach(rightwheelbody,boxbody) righthinge.setAnchor(rightwheel_pos) righthinge.setAxis(self.rotate((0,0,1),rot)) righthinge.setParam(ode.ParamFMax,self.hingemaxforce) # Create a spherical wheel at the back for support caster_pos = [box_pos[0], box_pos[1] - self.cubesize*0.5, box_pos[2] - self.cubesize*0.5] casterbody = ode.Body(self.world) castergeom = ode.GeomSphere(space=self.space, radius = self.cubesize/5.0) castergeom.setBody(casterbody) castergeom.setPosition(caster_pos) castergeom.setRotation(rot) casterM = ode.Mass() casterM.setSphere(self.cubemass*100.0, self.cubesize/5.0) casterbody.setMass(casterM) self.fixed = ode.FixedJoint(self.world) self.fixed.attach(casterbody,boxbody) self.fixed.setFixed() # WHEW, THE END OF ALL THAT FINALLY! # Build the Geoms and Joints arrays for rendering. self.boxgeom = boxgeom self._geoms = [boxgeom, leftwheelgeom, rightwheelgeom, castergeom, self.ground] self.lefthinge = lefthinge self.righthinge = righthinge self._joints = [self.lefthinge, self.righthinge, self.fixed]
# And a rod without any weight
rod = ode.Body(world)
M = ode.Mass()
M.setCylinder(0.01, 2, 0.01, 1)
M.mass = 1e-3
rod.setMass(M)
rod.setPosition((0, 0.5, 0))
# Connect the rod with the world through a Hinge joint.
world_joint = ode.HingeJoint(world)
world_joint.attach(rod, ode.environment)
world_joint.setAnchor((0, 0, 0))
world_joint.setAxis((0, 0, 1))
# Connect rod with ball with a fixed joint
rod_ball_joint = ode.FixedJoint(world)
rod_ball_joint.attach(rod, ball)
rod_ball_joint.setFixed()
# Create the viewer window
viewer = Euv.Viewer(size=(600, 600),
view_port_center=(0, 0),
view_port_width=2.5,
flip_y=True)
# Do the simulation
total_time = 0.0
dt = 0.02
Kf = 0.5 # Friction force
while total_time < 30:
def world_tick(self):
#if random.randrange(10) == 0:
#boat_lidar.get_lidar_range() # Use and publish whoa!
water_vel = self.get_water_vel(V(body.getPosition()))
body.addForceAtRelPos((0, 0, self.buoyancy_force(-body.getPosition()[2], 0.5)), (0, 0, .1))
# the following frictional forces are from darsen's models
#frictional force opposite to the velocity of the boat
body_velocity=body.vectorFromWorld(body.getLinearVel())
# adds a resistance force for the water proportional to the velocity (where [0] is x, [1] is y, and [2] is z). units in N/(m/s)
friction_force_forward=self.friction_coefficient_forward*body_velocity[0]-self.friction_coefficient_forward_reduction*(-1.0*pow(body_velocity[0],2) if body_velocity[0]<0.0 else pow(body_velocity[0],2))
friction_force_lateral=self.friction_coefficient_lateral*body_velocity[1]-self.friction_coefficient_lateral_reduction*(-1.0*pow(body_velocity[1],2) if body_velocity[1]<0.0 else pow(body_velocity[1],2))
body.addRelForce([-friction_force_forward,-friction_force_lateral,body_velocity[2]*-450])
# adds a angular resistance force for the water proportional to the velocity
# angular_velocity is a 3-tuple ([0]=roll,[1]=pitch,[2]=yaw)
angular_velocity=body.vectorFromWorld(body.getAngularVel())
friction_force_roll=self.friction_coefficient_rotational*angular_velocity[0]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[0],2) if angular_velocity[0]<0.0 else pow(angular_velocity[0],2))
friction_force_pitch=self.friction_coefficient_rotational*angular_velocity[1]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[1],2) if angular_velocity[1]<0.0 else pow(angular_velocity[1],2))
friction_force_yaw=self.friction_coefficient_rotational*angular_velocity[2]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[2],2) if angular_velocity[2]<0.0 else pow(angular_velocity[2],2))
body.addRelTorque([-friction_force_roll,-friction_force_pitch,-friction_force_yaw])
#print thrusters
# map the thrusters position on the boat
self.boat_model.vectors = []
# id2=starboard, id3=port
if not self.killed:
for thruster_id in [0, 1, 2, 3]:
# set the thrusters position on the boat
if thruster_id == 0:
relpos = v(self.BL_offset[0], self.BL_offset[1], 0)
reldir = v(1, 1, 0)
if thruster_id == 1:
relpos = v(self.BR_offset[0], self.BR_offset[1], 0)
reldir = v(1, -1, 0)
if thruster_id == 2:
relpos = v(self.FL_offset[0], self.FL_offset[1], 0)
reldir = v(1, -1, 0)
if thruster_id == 3:
relpos = v(self.FR_offset[0], self.FR_offset[1], 0)
reldir = v(1, 1, 0)
force = self.thrusts[thruster_id]
body.addRelForceAtRelPos(reldir*force, relpos)
self.boat_model.vectors.append((relpos, relpos + 0.05*reldir*force))
keys = pygame.key.get_pressed()
for keycode, force in [
(pygame.K_k, v(-50, 0, 0)),
(pygame.K_i, v(+50, 0, 0)),
(pygame.K_j, v(0, +50, 0)),
(pygame.K_l, v(0, -50, 0)),
(pygame.K_o, v(0, 0, +50)),
(pygame.K_m, v(0, 0, -50)),
]:
if keys[keycode]:
body.addRelForce(force*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1))
for keycode, torque in [
(pygame.K_COMMA, v(-20, 0, 0)),
(pygame.K_u, v(+20, 0, 0)),
(pygame.K_h, v(0, +20, 0)),
(pygame.K_SEMICOLON, v(0, -20, 0)),
(pygame.K_0, v(0, 0, +20)),
(pygame.K_n, v(0, 0, -20)),
]:
if keys[keycode]:
body.addRelTorque(torque*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1))
if keys[pygame.K_1]:
self.killed = True
if keys[pygame.K_2]:
self.killed = False
global locked
if keys[pygame.K_3]:
locked = True
if keys[pygame.K_4]:
locked = False
contactgroup = ode.JointGroup()
if self.locked:
j = ode.FixedJoint(world, contactgroup)
j.attach(body, None)
j.setFixed()
near_pairs = []
space.collide(None, lambda _, geom1, geom2: near_pairs.append((geom1, geom2)))
for geom1, geom2 in near_pairs:
for contact in ode.collide(geom1, geom2):
contact.setBounce(0.2)
contact.setMu(5000)
j = ode.ContactJoint(world, contactgroup, contact)
j.attach(geom1.getBody(), geom2.getBody())
dt = 1/50
self.world_time += dt
world.step(dt)
contactgroup.empty()
pos = body.getPosition()
q = V(body.getQuaternion())
# Avoid stupid [0, 0, 0, 0] quaternion initialization
if not numpy.any(q):
q = [1, 0, 0, 0]
# Publish tf /enu
self.enu_tf_br.sendTransform(
translation = (pos[0], pos[1], pos[2]),
rotation = (q[1], q[2], q[3], q[0]),
time = rospy.Time(self.world_time),
child = '/base_link',
parent = '/enu')
# Publish odom
msg = Odometry()
msg.header.stamp = rospy.Time(self.world_time)
msg.header.frame_id = '/enu'
msg.child_frame_id = '/base_link'
msg.pose.pose.position = Point(*pos)
msg.pose.pose.orientation = Quaternion(q[1], q[2], q[3], q[0])
msg.twist.twist.linear = Vector3(*q.conj().quat_rot(body.getLinearVel()))
msg.twist.twist.angular = Vector3(*q.conj().quat_rot(body.getAngularVel()))
self.odom_pub.publish(msg)
# XXX
msg = Odometry()
msg.header.stamp = rospy.Time(self.world_time)
msg.header.frame_id = '/ecef'
msg.child_frame_id = '/base_link'
# ecef_loc = gps.ecef_from_latlongheight(math.radians(36.802002), math.radians(-76.191019), 7)
enu_loc = numpy.array([53.6686215007, -20.8502282916, -0.0733864689281])
# msg.pose.pose.position = Point(*gps.ecef_from_enu(enu_v=body.getPosition() - enu_loc, ecef_pos=ecef_loc) + ecef_loc)
self.abs_odom_pub.publish(msg)
reactor.callLater(max(0, self.world_time + dt - reactor.seconds()), self.world_tick)
