Commits

committed 62929b4

demos: backport of 11686975d30a

• Participants
• Parent commits c34faae
• Branches 0.3.x

File demos/CentrifugalForceTest.py

However, theta must be calculated with a numerical solver since no simple analytical
solution exists.
"""
-
from ars.app import Program
import ars.utils.mathematical as mut
import ars.constants as cts
return 4.0 / 3.0 * mut.pi * (radius ** 3)

+
class CentrifugalForceTest(Program):
-
+
OFFSET = (2,0.5,2)
BOX_PARAMS = (((5,0.5,5),(0,-0.25,0)),{'density':1}) # ((size, center), density)
-
+
WINDOW_SIZE = (900,600)
CAMERA_POSITION = (2,5,10) # (0,-4,1) in C++ example #vp_xyz = (0.0,-4.0,1.0) # position [meters]
FPS = 50
POLE_VISUAL_RADIUS = 0.05 # 5 cm. how it will be displayed
POLE_HEIGHT = 2 # 2 m
POLE_INITIAL_POS = (0.0,1.0,0.0) # (0.0,0.0,1.0) in C++ example
-
+
BALL_MASS = 1.0 # 1kg
BALL_RADIUS = 0.01 # 1 cm
BALL_VISUAL_RADIUS = 0.1 # 10 cm
BALL_INITIAL_POS = (0.0, 1.0, 1.0)
-
+
JOINT1_ANCHOR = (0.0,0.0,0.0)
JOINT1_AXIS = (0.0,1.0,0.0) # (0.0,0.0,1.0) Z-axis in C++ example
JOINT1_FMAX = 100
JOINT2_ANCHOR = (0.0,2.0,1.0) # (0.0,2.0,1.0) # (0.0,1.0,2.0) in C++ example
JOINT2_AXIS = (1.0,0.0,0.0) # X-axis
-
+
CABLE_LENGTH = mut.length3(mut.sub3(BALL_INITIAL_POS, JOINT2_ANCHOR))
-
+
JOINT2_ANGLE_RATE_CONTROLLER_KP = 500.0
JOINT1_ANGLE_RATE_INITIAL = 3.0
-
+
def __init__(self):
Program.__init__(self)

dispatcher.connect(self.on_pre_frame, SIM_PRE_FRAME_SIGNAL)
-
+
self.joint1_vel_user = self.JOINT1_ANGLE_RATE_INITIAL
self.large_speed_steps = True
#TODO: set ERP, CFM
-
-
+
def create_sim_objects(self):
# FIXME: pole should have no mass! => does not really matter, since speed is set
# FIXME: visual radius => did not affect the results noticeably
-
+
# bodies are rotated before attaching themselves through joints
self.sim.get_object(pole).rotate(cts.X_AXIS, mut.pi/2)
-
+
self.sim.get_object(box).offset_by_position(self.OFFSET)
self.sim.get_object(pole).offset_by_position(self.OFFSET)
self.sim.get_object(ball).offset_by_position(self.OFFSET)
-
+
self.sim.add_rotary_joint('r1',					# name, obj1, obj2, anchor, axis
self.sim.get_object(box),
self.sim.get_object(pole),
None, self.JOINT1_AXIS)
-
+
self.sim.get_object(pole),
self.sim.get_object(ball),
#time = self.sim.sim_time
self.set_joint1_speed()
self.apply_friction()
-
+
ball_pos = self.sim.get_object(self.ball).get_position()
ball_vel = self.sim.get_object(self.ball).get_linear_velocity()
ball_omega = self.sim.get_object(self.ball).get_angular_velocity()
theta_sim = mut.acos((z_top - ball_pos[1] + self.OFFSET[1]) / self.CABLE_LENGTH) # ball_pos[2] in C++ example

print((ball_pos, ball_vel, ball_omega, theta_sim))
-
+
except Exception as e:
print('Exception when executing on_pre_frame: %s' % str(e))
-
+
def inc_joint1_vel(self):
self.joint1_vel_user += self.POLE_SPEED_STEP
-
+
def dec_joint1_vel(self):
self.joint1_vel_user -= self.POLE_SPEED_STEP
-
+
def set_joint1_speed(self):
self.sim.get_joint('r1').joint.set_speed(self.joint1_vel_user, self.JOINT1_FMAX)
-
+
def apply_friction(self):
torque = -self.JOINT2_ANGLE_RATE_CONTROLLER_KP * self.sim.get_joint('r2').joint.angle_rate

if __name__ == '__main__':
sim_program = CentrifugalForceTest()
-	sim_program.start()
+	sim_program.start()

File demos/ControlledSimpleArm.py

from ars.lib.pydispatch import dispatcher
from ars.model.simulator import SIM_PRE_STEP_SIGNAL

+
def output_data(time, sp, cv, error, torque):

print('time: %f, sp: %f, cv: %f, error: %f, torque: %f' %
(time, sp, cv, error, torque))

+
class ControlledSimpleArm(Program):

R1_TORQUE = 3
if __name__ == '__main__':
print('Python file being executed: %s' % __file__)
sim_program = ControlledSimpleArm()
-	sim_program.start()
+	sim_program.start()

File demos/FallingBall.py

from ars.app import Program

+
class FallingBall(Program):
-
+
def create_sim_objects(self):

if __name__ == '__main__':
sim_program = FallingBall()
-	sim_program.start()
+	sim_program.start()

File demos/FallingBalls.py

from ars.app import Program

+
class FallingBalls(Program):
-
+
def create_sim_objects(self):

if __name__ == '__main__':
sim_program = FallingBalls()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example1_bouncing_ball.py

from ars.lib.pydispatch import dispatcher
from ars.model.simulator import SIM_PRE_STEP_SIGNAL

+
class Example1(Program):
-
+
FPS = 50
STEPS_PER_FRAME = 80
-
+
HEIGHT = 1
MASS = 1
-
+
def __init__(self):
Program.__init__(self)
dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)
-
+
def create_sim_objects(self):
-
+
def on_pre_step(self):
try:
time = self.sim.sim_time
sim_ball = self.sim.get_object(self.sphere)
pos = sim_ball.get_position()
vel = sim_ball.get_linear_velocity()
-
+
#print('time: %f, pos: %f, vel: %f' % (time, pos[1], vel[1]))
print('%.7e\t%.7e\t%.7e' % (time, pos[1], vel[1]))
-
+
except Exception:
print('Exception when executing on_pre_step')

if __name__ == '__main__':
sim_program = Example1()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example1_bouncing_balls-no_data.py

from ars.app import Program

+
class Example1NoData(Program):
-
+
FPS = 50
STEPS_PER_FRAME = 80
-
+
def create_sim_objects(self):
ball1 = self.sim.add_sphere(0.1, (1,1 + 0.1,1), 1.0)
ball2 = self.sim.add_sphere(0.1, (2,1.5 + 0.1,2), 1.0)
ball3 = self.sim.add_sphere(0.1, (3,2 + 0.1,3), 1.0)
-
+
print(self.sim.get_object(ball1).actor.set_color((1,1,1)))
print(self.sim.get_object(ball2).actor.set_color((0,0.8,0.8)))
print(self.sim.get_object(ball3).actor.set_color((0.7,0.5,0)))

if __name__ == '__main__':
sim_program = Example1NoData()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example2_conical_pendulum.py

SIM_PRE_STEP_SIGNAL = ars.model.simulator.SIM_PRE_STEP_SIGNAL

class Example2(Program):
-
+
# simulation & window parameters
CAMERA_POSITION = (6,3,6)
FPS = 50
STEPS_PER_FRAME = 80
-
+
# bodies' parameters
DELTA = 0.01 # to prevent the collision of the 2nd link with the floor
OFFSET = (1,0,2)
BOX_PARAMS = (((10,0.5,10),(0,-0.25,0)),{'density':100}) # ((size, center), density)
-
+
POLE_HEIGHT = 1
POLE_INITIAL_POS = (0.0,0.5 + DELTA,0.0)
POLE_MASS = 10.0
-
+
ARM_LENGTH = 1.0
ARM_INITIAL_POS = (0.0,0.5 + DELTA,0.1)
ARM_MASS = 10.0
-
+
JOINT1_ANCHOR = (0.0,0.0,0.0)
JOINT1_AXIS = cts.Y_AXIS
JOINT2_ANCHOR = (0.0,1.0 + DELTA,0.1)
JOINT2_AXIS = cts.X_AXIS
-
+
Q1_FRICTION_COEFF = 50e-3 * 100
Q2_FRICTION_COEFF = 50e-3 * 100

def __init__(self):
Program.__init__(self)
dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)
-
+
self.q1p_prev = 0.0
self.q2p_prev = 0.0
-
+
def create_simulation(self):
# we didn't need to code this method
# but if we want to modify the floor, we have to
-
+
# set up the simulation parameters
self.sim = Simulation(self.FPS, self.STEPS_PER_FRAME)
color=(0.7,0.7,0.7), dist=-0.5, box_center=(0,-0.5,0))
-
+
self.create_sim_objects()
-
+
self.sim.update_actors()
-
+
def create_sim_objects(self):
-
+
self.POLE_INITIAL_POS, mass=self.POLE_MASS)
self.ARM_INITIAL_POS, mass=self.ARM_MASS)
-
+
# bodies are rotated before attaching themselves through joints
self.sim.get_object(pole).rotate(cts.X_AXIS, mut.pi/2)
self.sim.get_object(arm).rotate(cts.X_AXIS, mut.pi/2)
-
+
self.sim.get_object(box).offset_by_position(self.OFFSET)
self.sim.get_object(pole).offset_by_position(self.OFFSET)
self.sim.get_object(arm).offset_by_position(self.OFFSET)
-
+
self.sim.add_rotary_joint('r1',         # name, obj1, obj2, anchor, axis
self.sim.get_object(box),
self.sim.get_object(pole),
None,
self.JOINT1_AXIS)
-
+
self.sim.get_object(pole),
self.sim.get_object(arm),
self.JOINT2_AXIS)
-
+
#self.sim.get_object(box).actor.set_color(cts.COLOR_RED)
self.sim.get_object(pole).actor.set_color(cts.COLOR_YELLOW)
self.sim.get_object(arm).actor.set_color(cts.COLOR_NAVY)
-
+
self.box = box
self.pole = pole
self.arm = arm
try:
time = self.sim.sim_time
torque1 = self.get_torque_to_apply(time)
-
+
self.apply_torque_to_joints(torque1, None)
self.apply_friction(self.q1p_prev, self.q2p_prev)
-
+
q1 = self.get_q1()
q2 = self.get_q2()
q1p = self.get_q1p()
q2p = self.get_q2p()
-
+
self.q1p_prev = q1p
self.q2p_prev = q2p
-
+
print('%.7e\t%.7e\t%.7e\t%.7e\t%.7e' % (time,q1,q1p,q2,q2p))
-
+
except Exception as e:
print('Exception when executing on_pre_step: %s' % str(e))
-
+
def get_torque_to_apply(self, time):
if time < self.SATURATION_TIME:
torque = time * self.MAX_TORQUE
else:
torque = self.MAX_TORQUE
-
+
def get_q1(self):
return self.sim.get_joint('r1').joint.angle
-
+
def get_q2(self):
return self.sim.get_joint('r2').joint.angle
-
+
def get_q1p(self):
return self.sim.get_joint('r1').joint.angle_rate
-
+
def get_q2p(self):
return self.sim.get_joint('r2').joint.angle_rate
-
+
def apply_torque_to_joints(self, torque1, torque2):
if torque1 is not None:
if torque2 is not None:
-
+
def apply_friction(self, q1p, q2p):
self.apply_torque_to_joints(-q1p * self.Q1_FRICTION_COEFF,
-q2p * self.Q2_FRICTION_COEFF)
if __name__ == '__main__':
sim_program = Example2()
sim_program.start()
-
+
# print arm links' inertia matrices
pole_body = sim_program.sim.get_object(sim_program.pole).body
arm_body = sim_program.sim.get_object(sim_program.arm).body
print(pole_body.get_inertia_tensor())
-	print(arm_body.get_inertia_tensor())
+	print(arm_body.get_inertia_tensor())

File demos/IROS/example3_speed_profile.py

from VehicleWithArm import VehicleWithArm, mut

+
class Example3(VehicleWithArm):

#WINDOW_SIZE = (1024,630)
print(ball_body.get_mass())
print(chassis_body.get_mass())
print(wheelR_body.get_mass())
-	print(wheelL_body.get_mass())
+	print(wheelL_body.get_mass())

File demos/IROS/example4_sinusoidal_terrain.py

from VehicleWithArm import VehicleWithArm

+
def random_heightfield(num_x, num_z, scale=1.0):
"""A height field where values are completely random"""
# that x and z are integers, not floats, does not matter
verts.append( (x,random()*scale,z) )
return verts

+
def sinusoidal_heightfield(num_x, num_z, height_scale=1.0, frequency_x=1.0):
"""Creates the vertices corresponding to a sinusoidal heightfield along the
X axis. The height_scale controls the amplitude of the wave, and
verts.append( (x, mut.sin(x * frequency_x)*height_scale, z) )
return verts

+
def constant_heightfield(num_x, num_z, height=0.0):
"""A height field where all the values are the same"""
# that x and z are integers, not floats, does not matter
verts.append((x, height, z))
return verts

+
class Example4(VehicleWithArm):

FPS = 50

File demos/IROS/example4b_sinusoidal_terrain_with_screenshot_recorder.py

if __name__ == '__main__':
sim_program = Example4SR()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example5_vehicle_with_user_input.py

from VehicleWithArm import VehicleWithArm

+
class Example5(VehicleWithArm):

FPS = 50

if __name__ == '__main__':
sim_program = Example5(False, False)
-	sim_program.start()
+	sim_program.start()

File demos/SimpleArm.py

import ars.utils.mathematical as mut
import ars.constants as cts

+
class SimpleArm(Program):

TORQUE = 3
if __name__ == '__main__':
print('Python file being executed: %s' % __file__)
sim_program = SimpleArm()
-	sim_program.start()
+	sim_program.start()

File demos/Vehicle1.py

from ars.app import Program

+
class Vehicle1(Program):
-
+
TORQUE = 500
-
+
def __init__(self):
Program.__init__(self)
-
+
def create_sim_objects(self):
# POR: point of reference
-
+
wheel2 = self.sim.add_cylinder(0.1, 0.2, (0,0,1), density=1)
wheel3 = self.sim.add_cylinder(0.1, 0.2, (1,0,0), density=1)
wheel4 = self.sim.add_cylinder(0.1, 0.2, (1,0,1), density=1)
-
+
chassis = self.sim.add_box((1.3,0.2,0.6), (0.5,0,0.5), density=10) # size, center, density
-
+
self.sim.get_object(wheel2).offset_by_object(self.sim.get_object(wheel1))
self.sim.get_object(wheel3).offset_by_object(self.sim.get_object(wheel1))
self.sim.get_object(wheel4).offset_by_object(self.sim.get_object(wheel1))
self.sim.get_object(chassis).offset_by_object(self.sim.get_object(wheel1))
-
+
self.sim.get_object(wheel1), (1,1,1), (0,0,1)) # name, obj1, obj2, anchor, axis
self.sim.get_object(wheel3), (2,1,1), (0,0,1))
self.sim.get_object(wheel4), (2,1,2), (0,0,1))
-
+
def go_forwards(self):
-
+
def go_backwards(self):
-
+
def turn_left(self):
-
+
def turn_right(self):

if __name__ == '__main__':
sim_program = Vehicle1()
-	sim_program.start()
+	sim_program.start()

File demos/Vehicle2.py

from ars.app import Program
import ars.constants as cts

+
class Vehicle2(Program):

TORQUE = 30
# test
#print(self.sim.get_object(wheelR).actor.set_color((0.8,0,0)))

-
def go_forwards(self):
"""Rotate both powered wheels in the same direction, forwards"""

if __name__ == '__main__':
sim_program = Vehicle2()
-	sim_program.start()
+	sim_program.start()

File demos/Vehicle2WithScreenshots.py

if __name__ == '__main__':
sim_program = Vehicle2WithScreenshots()
-	sim_program.start()
+	sim_program.start()

File demos/VehicleWithArm.py

free-rotating spherical wheel. It has a 2-link robotic arm attached,
with joints either friction-free or with friction proportional to
joint speed.
+
"""
-
from ars.app import Program
import ars.utils.mathematical as mut
import ars.constants as cts
from ars.lib.pydispatch import dispatcher
from ars.model.simulator import SIM_PRE_STEP_SIGNAL

+
class VehicleWithArm(Program):

STEPS_PER_FRAME = 200

if __name__ == '__main__':
sim_program = VehicleWithArm()
-	sim_program.start()
+	sim_program.start()

File demos/VehicleWithControlledArm.py

free-rotating spherical wheel. It has a 2-link robotic arm attached,
with joints either friction-free or with friction proportional to
joint speed. The second joint has a PD controller.
+
"""
+from VehicleWithArm import VehicleWithArm

-from VehicleWithArm import VehicleWithArm

def output_data(time, sp, cv, error, torque):

print('time: %f, sp: %f, cv: %f, error: %f, torque: %f' %
(time, sp, cv, error, torque))

+
class VehicleWithControlledArm(VehicleWithArm):

WHEEL_R_PARAMS = ((0.4,0.3,(0,0,-0.5)),{'density':1}) # ((length, radius, center), density)

if __name__ == '__main__':
sim_program = VehicleWithControlledArm(False, False)
-	sim_program.start()
+	sim_program.start()