def __init__(self,
gravity=9.8,
mass_cart=1.0,
mass_pole=1.0,
tau=0.02,
size_box=(0.5, 0.3),
size_pole=(1.0, .1)):
self.gravity = gravity
self.mass_pole = mass_pole
self.mass_cart = mass_cart
self.dt = tau
self.viewer = None
self.viewerSize = 500
self.spaceSize = 7.
self.size_box = size_box
self.size_pole = size_pole
self.max_force = 10.
self.theta_threshold = 3.2
self.x_threshold = 3.0
self.action_space = spaces.Box(low=-self.max_force,
high=self.max_force,
shape=(1, ))
high = np.array([
self.x_threshold * 2,
np.finfo(np.float32).max, self.theta_threshold * 2,
np.finfo(np.float32).max
])
self.observation_space = spaces.Box(-high, high)
self.world = ode.World()
self.world.setGravity((0, -gravity, 0))
self.body1 = ode.Body(self.world)
self.body2 = ode.Body(self.world)
self.create_basebox(self.body1, (0., 0., 0.), self.mass_cart, size_box)
self.create_link(self.body2, (0., size_pole[0] / 2, 0.),
self.mass_pole, size_pole)
self.space = ode.Space()
self.j1 = ode.SliderJoint(self.world)
self.j1.attach(self.body1, ode.environment)
self.j1.setAxis((1, 0, 0))
self.j1.setFeedback(1)
self.j1.setParam(ode.ParamLoStop, -10)
self.j1.setParam(ode.ParamHiStop, 10)
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)
self.j2.setParam(ode.ParamLoStop, -np.pi)
self.j2.setParam(ode.ParamHiStop, np.pi)
def __init__(self, u, par, level):
Movable.__init__(self, u)
RawGLObject.__init__(self, u)
mass = ode.Mass()
r = 0.05
density = 100
mass.setSphere(
density, r
) #NB segment is a sphere mass -- so does not need to rotate. the bending is done by joints BETWEEN the masses
self.body = ode.Body(self.u.world)
self.body.setMass(mass)
self.level = level
x, y, z = par.getBodyPositionForChildSeg()
seg = 0.4
if level == 1:
x += 0.15 #65
z += 0.15
elif level == -1:
x += 0.15
z -= 0.15
elif level > 1:
x += seg
z += seg
elif level < -1:
x += seg
z -= seg
#x+=x_offset
#x=x+SEG_LENGTH #TODO fix properly
#z=z+z_offset
self.body.setPosition((x, y, z))
self.geom = ode.GeomSphere(u.segSpace, r) #SEPARATE SPACE!
self.geom.setBody(self.body)
self.sg.addChild(self.makeSceneGraph(r))
self.joint = ode.HingeJoint(
self.u.world
) #easier to use horiz/vertical pairs of hinges rather than balls; as have getAngle methods
self.joint.attach(par.body, self.body)
self.joint.setAnchor(par.body.getPosition())
self.joint.setAxis((0, 1, 0))
if abs(level) == 1: #follicle has different properties
self.k = 1.0
self.gamma = 0.5
else:
self.k = 0.1 #TODO we really need two different classes of sim: one of rmacro navigation excersizes, and one for very fine detailed vibrtation sims.
self.gamma = 0.05
self.par = par #needed for link graphics
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 addHingeJoint(self, body1, body2, anchor, axis, loStop = -ode.Infinity,
hiStop = ode.Infinity):
anchor = add3(anchor, self.offset)
joint = ode.HingeJoint(self.world)
joint.attach(body1, body2)
joint.setAnchor(anchor)
joint.setAxis(axis)
joint.setParam(ode.ParamLoStop, loStop)
joint.setParam(ode.ParamHiStop, hiStop)
joint.style = "hinge"
self.joints.append(joint)
return joint
def end(name):
if (name == 'hinge'):
joint = ode.HingeJoint(world, self._jg)
joint.attach(link1, link2)
if (anchor[0] is not None):
joint.setAnchor(anchor[0])
if (len(axes) != 1):
raise errors.InvalidError('Wrong number of axes for hinge'
' joint.')
joint.setAxis(self._parser.parseVector(axes[0]))
self._applyAxisParams(joint, 0, axes[0])
self.setODEObject(joint)
self._parser.pop()
def add_hingejoint(rb1, rb2, anchor, axis, fmax=10, lo=-4, hi=4):
"""Create a hinge joint between two rigid bodies.
"""
# Create the joint and attach it to both rigid bodies
joint = ode.HingeJoint(world)
joint.attach(rb1, rb2)
# Set the anchor and axis of the hinge joint
joint.setAnchor(anchor)
joint.setAxis(axis)
# Set hinge parameters
joint.setParam(ode.ParamFMax, fmax)
joint.setParam(ode.ParamLoStop, lo)
joint.setParam(ode.ParamHiStop, hi)
joints.append(joint)
return joint
def __init__(self, world: ode.World = None, visualized=False):
super(SlidingPendulum, self).__init__(world)
# Bodies and joints
# Create wagon
wagon = ode.Body(self.world)
M = ode.Mass()
M.setSphere(2500, 0.05)
wagon.setMass(M)
wagon.setPosition((0, 1, 0))
# Create pendulum
pendulum = ode.Body(self.world)
M = ode.Mass()
M.setSphere(2500, 0.05)
pendulum.setMass(M)
pendulum.setPosition((0, 2, 0))
# Connect wagon with the static environment using a slider joint
slider = ode.SliderJoint(self.world)
slider.attach(ode.environment, wagon)
slider.setAxis((1, 0, 0))
# Connect pendulum with wagon
arm = ode.HingeJoint(self.world)
arm.attach(wagon, pendulum)
arm.setAnchor(wagon.getPosition())
arm.setAxis((0, 0, 1))
self._wagon = wagon
self._pendulum = pendulum
self._slider = slider
self._arm = arm
slider.setParam(ode.ParamVel, 0.1)
slider.setParam(ode.ParamFMax, 22) # used to be 22
# visualization
self._visualized = visualized
if visualized:
surface = pygame.display.set_mode((640, 480))
SimMan.process(self._screenUpdater(surface))
def create_hinge(self,key,pos,bod_keys,axis=[0.,0.,1.],lims=[-1.,1.],max_force=100.):
""" 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.HingeJoint(self.world)
j.attach(self.bodies[bod_keys[0]], self.bodies[bod_keys[1]])
j.setAnchor((pos))
j.setAxis((axis))
j.setParam(ode.ParamLoStop, lims[0])
j.setParam(ode.ParamHiStop, lims[1])
j.setParam(ode.ParamFMax, max_force)
j.style = "hinge"
self.joints[key] = j
return key
def create_objects(self, world):
print 'chassis'
fmax = 5000 * 1.6
scale = 0.04
# chassis
density = 50.5
lx, ly, lz = (145 * scale, 10 * scale, 177 * scale)
# Create body
body = ode.Body(world.world)
mass = ode.Mass()
mass.setBox(density, lx, ly, lz)
body.setMass(mass)
chassis_mass = lx * ly * lz * density
print "chassis mass =", chassis_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 = 4
# left wheel
radius = 25 * scale
height = radius * 0.8
wheel_mass_ = math.pi * radius**2 * height * density
print "wheel mass =", wheel_mass_
px = lx / 2
py = 0
print 'left wheels'
for pz in (-(lz / 2), 0, lz / 2):
# cylinders
left_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setCylinder(density, 1, radius, height)
left_wheel_body.setMass(wheel_mass)
left_wheel_geom = ode.GeomCylinder(world.space,
radius=radius,
length=height)
left_wheel_geom.setBody(left_wheel_body)
left_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0))
left_wheel_body.setPosition((px - height / 2, py, pz))
# spheres
#left_wheel_body = ode.Body(world.world)
#wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
#left_wheel_body.setMass(wheel_mass)
#left_wheel_geom = ode.GeomSphere(world.space, radius=radius)
#left_wheel_geom.setBody(left_wheel_body)
#left_wheel_body.setPosition((px, py, pz))
world.add_body(left_wheel_body)
world.add_geom(left_wheel_geom)
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.setParam(ode.ParamFMax, fmax)
self._left_wheel_joints.append(left_wheel_joint)
print 'right wheels'
# right wheel
px = -(lx / 2)
for pz in (-(lz / 2), 0, lz / 2):
# cylinders
right_wheel_body = ode.Body(world.world)
wheel_mass = ode.Mass()
wheel_mass.setCylinder(density, 1, radius, height)
right_wheel_body.setMass(wheel_mass)
right_wheel_geom = ode.GeomCylinder(world.space,
radius=radius,
length=height)
right_wheel_geom.setBody(right_wheel_body)
right_wheel_body.setRotation((0, 0, 1, 0, 1, 0, -1, 0, 0))
right_wheel_body.setPosition((px - height / 2, py, pz))
# spheres
#right_wheel_body = ode.Body(world.world)
#wheel_mass = ode.Mass()
#wheel_mass.setSphere(density, radius)
#right_wheel_body.setMass(wheel_mass)
#right_wheel_geom = ode.GeomSphere(world.space, radius=radius)
#right_wheel_geom.setBody(right_wheel_body)
#right_wheel_body.setPosition((px, py, pz))
world.add_body(right_wheel_body)
world.add_geom(right_wheel_geom)
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.setParam(ode.ParamFMax, fmax)
self._right_wheel_joints.append(right_wheel_joint)
print "total mass =", float(chassis_mass + 6 * wheel_mass_)
def __init__(self):
# Constants
self.WINDOW_RESOLUTION = (640, 480)
self.DRAW_SCALE = self.WINDOW_RESOLUTION[0] / 5
"""Factor to multiply physical coordinates by to obtain screen size in pixels"""
self.DRAW_OFFSET = (self.WINDOW_RESOLUTION[0] / 2, 150)
"""Screen coordinates (in pixels) that map to the physical origin (0, 0, 0)"""
self.BACKGROUND_COLOR = (255, 255, 255)
self.GRAVITY = (0, -9.81, 0)
self.SPHERE1_POSITION = (0, 0, 0)
self.SPHERE1_MASS = 1
self.SPHERE1_RADIUS = 0.15
self.SPHERE1_COLOR = (55, 0, 200)
self.SPHERE2_POSITION = (0.05, 1, 0)
self.SPHERE2_MASS = 1
self.SPHERE2_RADIUS = 0.15
self.SPHERE2_COLOR = (200, 0, 200)
self.JOINT1_ANCHOR = (0, 0, 0)
self.JOINT1_COLOR = (200, 0, 55)
self.JOINT1_WIDTH = 2
"""Width of the line (in pixels) representing the joint"""
self.JOINT2_ANCHOR = self.SPHERE1_POSITION
self.JOINT2_COLOR = (200, 0, 55)
self.JOINT2_WIDTH = 2
"""Width of the line (in pixels) representing the joint"""
self.TIME_STEP = 0.01
self.EPS = 10**-5
self.original_speed = 0.0
self.target_speed = 0.0
self.current_speed = 0.0
self.pos = 0.0
self.frame = 0
# Initialize pygame
pygame.init()
# Open a display
self.screen = pygame.display.set_mode(self.WINDOW_RESOLUTION)
# Create a world object
self.world = ode.World()
self.world.setGravity(self.GRAVITY)
self.world.setAngularDamping(0.1)
# Create two bodies
self.kbody = ode.Body(self.world)
M = ode.Mass()
M.setSphereTotal(self.SPHERE1_MASS, self.SPHERE1_RADIUS)
#kbody.setMass(M)
self.kbody.setKinematic()
self.kbody.setPosition(self.SPHERE1_POSITION)
self.body2 = ode.Body(self.world)
M = ode.Mass()
M.setSphereTotal(self.SPHERE2_MASS, self.SPHERE2_RADIUS)
self.body2.setMass(M)
self.body2.setPosition(self.SPHERE2_POSITION)
self.body2.setMaxAngularSpeed(20.0)
# Connect body2 with kbody
self.j2 = ode.HingeJoint(self.world)
self.j2.attach(self.kbody, self.body2)
self.j2.setAnchor(self.JOINT2_ANCHOR)
self.j2.setAxis((0,0,1))
self.clk = pygame.time.Clock()
self.fps = 1.0 / self.TIME_STEP
self.cartPole_x = 0.5
self.xMin = -2.0
self.xMax = 2.0
self.acumuladorVueltasPorMovimiento = 1
self.vueltasPorMovimiento = 10
self.pos = 0
self.cartPole_velocidadAngular = 0.0
self.cartPole_angulo = 0.0
# 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.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, 20) # Connect body2 with body1 j2 = ode.BallJoint(world) j2.attach(body1, body2) j2.setAnchor((1, 2, 0)) # Display system using VPython p1 = body1.getPosition() p2 = body2.getPosition()
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 _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 __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 _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_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.HingeJoint(self.odedynamics.world)
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)
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)
def __init__(self,
time_stop=20,
time_step=0.01,
segment_size=(1.5, 0.5, 0.5),
segment_density=0.1,
segment_count=8,
joint_lostop=-1.6,
joint_histop=1.6,
joint_fmax=10,
drag_coefficient=0.35,
vis_scale=100,
json_fname=None,
json_step=0.05):
super(Ribbot, self).__init__()
self.world = ode.World()
self.world.setGravity((0.0, 0.0, 0.0))
self.space = ode.Space()
self.contact_group = ode.JointGroup()
self.bodies = []
self.geoms = []
self.joints = []
self.ignore_collide_pairs = []
self.time = 0.0
self.time_step = time_step
self.time_stop = time_stop
json_time_step = max(time_step, json_step)
self.time_step_cnt = 0
self.json_step_mod = int(round(json_time_step / self.time_step))
self.drag_coefficient = drag_coefficient
# All segments share the same surface parameters
self.segment_surface_parameters = [{
"area":
segment_size[1] * segment_size[2],
"norm": (1, 0, 0)
}, {
"area":
segment_size[0] * segment_size[2],
"norm": (0, 1, 0)
}, {
"area":
segment_size[0] * segment_size[1],
"norm": (0, 0, 1)
}, {
"area":
segment_size[1] * segment_size[2],
"norm": (-1, 0, 0)
}, {
"area":
segment_size[0] * segment_size[2],
"norm": (0, -1, 0)
}, {
"area":
segment_size[0] * segment_size[1],
"norm": (0, 0, -1)
}]
#
# Create the ribbon robot
#
segment_mass = ode.Mass()
segment_mass.setBox(segment_density, *segment_size)
segment_position = [0.0, segment_size[1] * 1.5, 0.0]
# Create rigid bodies
for s in xrange(segment_count):
seg = ode.Body(self.world)
seg.setPosition(segment_position)
seg.setMass(segment_mass)
self.bodies.append(seg)
geom = ode.GeomBox(self.space, lengths=segment_size)
geom.setBody(seg)
self.geoms.append(geom)
segment_position[0] += segment_size[0]
# Create hinge joints
joint_position = [segment_size[0] / 2., segment_size[1] * 1.5, 0.0]
for seg1, seg2 in zip(self.bodies, self.bodies[1:]):
joint = ode.HingeJoint(self.world)
joint.attach(seg1, seg2)
joint.setAnchor(joint_position)
joint.setAxis((0, 1, 0))
joint.setParam(ode.ParamLoStop, joint_lostop)
joint.setParam(ode.ParamHiStop, joint_histop)
joint.setParam(ode.ParamFMax, joint_fmax)
self.joints.append(joint)
self.ignore_collide_pairs.append((seg1, seg2))
joint_position[0] += segment_size[0]
#
# Setup Visualization
#
self.json_fname = json_fname
if json_fname is not None:
self.VIS_SCALE = vis_scale
self.json_object = {}
self.json_object["time_start"] = 0
self.json_object["time_stop"] = time_stop
self.json_object["time_step"] = json_time_step
self.json_object["primitives"] = []
for s in xrange(segment_count):
seg = {}
seg["geometry"] = {}
seg["geometry"]["shape"] = "cube"
seg["geometry"]["sizeX"] = segment_size[0] * self.VIS_SCALE
seg["geometry"]["sizeY"] = segment_size[1] * self.VIS_SCALE
seg["geometry"]["sizeZ"] = segment_size[2] * self.VIS_SCALE
seg["dynamics"] = {}
seg["dynamics"]["position"] = []
seg["dynamics"]["quaternion"] = []
seg["dynamics"]["visible"] = [[0., time_stop]]
self.json_object["primitives"].append(seg)
self.update_json()
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)
def ode_dynamics(force, dt, end):
"""ODE Dynamics
Simulates a 3-DOF system using the Open Dynamics Engine. The positional
results of this simulation will be validated against the dynamics
calculations for the same system.
Arguments:
force: The initial force applied to the center of mass of the 3rd link. [N]
dt: The time interval between positional checks. Also the duration of
the initially applied force. [s]
end: The duration of the simulation. [s]
Returns:
A list of positional argument tuples. Tuples are in the format of
(x, y, z) for each timestep encountered (end / dt)
"""
results = []
axes = {
'x': 1,
'y': 2,
'z': 3,
}
# Generate the simulation world
world = ode.World()
# Use a gravity-free environment
g = (0.0, 0.0, 0.0)
world.setGravity(g)
# Create link 1
body1 = ode.Body(world)
mass1 = ode.Mass()
mass1.setCylinderTotal(total_mass=1.0, direction=axes['y'], r=0.001, h=1.0)
body1.setMass(mass1)
body1.setPosition((0.0, 0.5, 0.0))
# Create link 2
body2 = ode.Body(world)
mass2 = ode.Mass()
mass2.setCylinderTotal(total_mass=1.0, direction=axes['x'], r=0.001, h=1.0)
body2.setMass(mass2)
body2.setPosition((0.5, 1.0, 0.0))
# Create link 3
body3 = ode.Body(world)
mass3 = ode.Mass()
mass3.setCylinderTotal(total_mass=1.0, direction=axes['x'], r=0.001, h=1.0)
body3.setMass(mass3)
body3.setPosition((1.5, 1.0, 0.0))
# Connect link 1 to the environment with a z-axis hinge joint
joint1 = ode.HingeJoint(world)
joint1.attach(body1, ode.environment)
joint1.setAnchor((0.0, 0.0, 0.0))
joint1.setAxis((0, 0, 1))
# Connect link 2 to link 3 with a z-axis hinge joint
joint2 = ode.HingeJoint(world)
joint2.attach(body2, body1)
joint2.setAnchor((0.0, 1.0, 0.0))
joint2.setAxis((0, 0, 1))
# Connect link 3 to link 2 with a y-axis hinge joint
joint3 = ode.HingeJoint(world)
joint3.attach(body2, body3)
joint3.setAnchor((1.0, 1.0, 0.0))
joint3.setAxis((0, 1, 0))
# Apply the desired force for the first 'dt' seconds
body3.addForce(force)
current_time = 0.0
while current_time <= end:
# Store the positional arguments at this point in time
pos = body3.getPosition()
results.append(pos)
# Move to the next timestep
world.step(dt)
current_time += dt
return np.array(results)
def __init__(self, max_simsecs=30, net=None, noise_sd=0.01, quick=0):
"""Creates the ODE and Geom bodies for this simulation"""
Sim.__init__(self, max_simsecs, noise_sd, quick)
log.debug('init PoleBalance sim')
self.network = net
CART_POSITION = (0, 0, 2)
POLE_POSITION = (0, 0, 3 + (5.0 / 2))
CART_SIZE = (8, 8, 2)
POLE_SIZE = (1, 1, 5)
HINGE_POSITION = (0, 0, 3)
CART_MASS = 10
POLE_MASS = 0.5
self.MAXF = 1000
self.INIT_U = [] # initial force, eg [10000]
self.cart_geom = ode.GeomBox(self.space, CART_SIZE)
self.geoms.append(self.cart_geom)
self.cart_body = ode.Body(self.world)
self.cart_body.setPosition(CART_POSITION)
cart_mass = ode.Mass()
cart_mass.setBoxTotal(CART_MASS, CART_SIZE[0], CART_SIZE[1],
CART_SIZE[2])
self.cart_body.setMass(cart_mass)
self.cart_geom.setBody(self.cart_body)
self.pole_geom = ode.GeomBox(self.space, POLE_SIZE)
self.geoms.append(self.pole_geom)
self.pole_body = ode.Body(self.world)
self.pole_body.setPosition(POLE_POSITION)
pole_mass = ode.Mass()
pole_mass.setBoxTotal(POLE_MASS, POLE_SIZE[0], POLE_SIZE[1],
POLE_SIZE[2])
self.pole_body.setMass(pole_mass)
self.pole_geom.setBody(self.pole_body)
self.cart_geom.setCategoryBits(long(0))
self.pole_geom.setCategoryBits(long(0))
# joint 0 - slide along 1D
self.slider_joint = ode.SliderJoint(self.world)
self.joints.append(self.slider_joint)
self.slider_joint.attach(self.cart_body, ode.environment)
self.slider_joint.setAxis((1, 0, 0))
self.slider_joint.setParam(ode.ParamLoStop, -5)
self.slider_joint.setParam(ode.ParamHiStop, 5)
# joint 1 - hinge between the two boxes
self.hinge_joint = ode.HingeJoint(self.world)
self.joints.append(self.hinge_joint)
self.hinge_joint.attach(self.cart_body, self.pole_body)
self.hinge_joint.setAnchor(HINGE_POSITION)
self.hinge_joint.setAxis((0, 1, 0))
self.last_hit = 0.0
self.init_u_count = 0
self.regular_random_force = 1
self.lqr = None
self.controlForce = 0
self.randomForce = 0
self.force_urge = 0
M = ode.Mass()
M.setSphere(1000, 0.01)
M.mass = 1.0
ball.setMass(M)
ball.setPosition((0, 1, 0))
# 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)
draw_body(cuisseG)
draw_body(tronc)
draw_body(brasL)
draw_body(brasG)
draw_body(avantbrasL)
draw_body(avantbrasG)
glutSwapBuffers()
troncfunc()
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)
