return points
end_t = 0
p = ball.getPosition()
print 'Ball starting at: (%.2f,%.2f,%.2f)' % (p[0], p[1], p[2])
# The three joints are:
# [ hip yaw, hip pitch, knee pitch ]
joints = s.robot.joints
while True:
s.step()
points = checkVictory()
if points:
if end_t == 0.0:
# Now that we've won, set a time to end the simulation
end_t = s.getSimTime() + 5.0
print 'Victory detected! Counting down to exit...'
elif s.getSimTime() > end_t:
print "You won with %d points!" % points
break
lRates = StudentControl.control(sim_time=s.getSimTime(),
hip_yaw_angle=joints[0].getAngle() + s.robot.YAW_OFFSET,
hip_pitch_angle=joints[1].getAngle() + s.robot.PITCH_OFFSET,
knee_angle=joints[2].getAngle() + s.robot.KNEE_OFFSET)
for j, r in zip(joints, lRates):
j.setLengthRate(r)
for j in range(i + 1):
body, geom = s.createCapsule(mass=1.0e1,
length=1.0,
radius=0.1,
pos=(i * x_off, -5.0 - y_off * j + i * (y_off / 2), 0.7))
wickets.append(body)
static_geoms.append(geom)
robot = s.robot
yaw = robot.members['hip_yaw']
pitch = robot.members['hip_pitch']
knee = robot.members['knee_pitch']
shock = robot.members['foot_shock']
cycle_time = 10.0
while True:
s.step()
sim_t = s.getSimTime()
x_target = robot.YAW_L + robot.THIGH_L
y_target = 1.0 * cos(2 * pi * sim_t / cycle_time)
z_target = max(0.2 * sin(2 * pi * sim_t / cycle_time), -0.00) - robot.HIP_FROM_GROUND_HEIGHT
yaw_target, pitch_target, knee_target = robot.jointAnglesFromFootPosition((x_target, y_target, z_target))
yaw.setLengthRate(-0.25 * calcAngularError(yaw.getAngle(), yaw_target))
pitch.setLengthRate(-0.25 * calcAngularError(pitch.getAngle(), pitch_target))
knee.setLengthRate(-0.25 * calcAngularError(knee.getAngle(), knee_target))
#print shock.getPosition()
for body in wickets:
if body.getRotation() != (1.0, 0.0, 0.0,\
0.0, 1.0, 0.0,\
0.0, 0.0, 1.0):
points += 1
return points
end_t = 0
p = ball.getPosition()
print 'Ball starting at: (%.2f,%.2f,%.2f)'%(p[0],p[1],p[2])
# The three joints are:
# [ hip yaw, hip pitch, knee pitch ]
joints = s.robot.joints
while True:
s.step()
points = checkVictory()
if points:
if end_t == 0.0:
# Now that we've won, set a time to end the simulation
end_t = s.getSimTime() + 5.0
print 'Victory detected! Counting down to exit...'
elif s.getSimTime() > end_t:
print "You won with %d points!"%points
break
lRates = StudentControl.control( sim_time = s.getSimTime(), hip_yaw_angle=joints[0].getAngle()+s.robot.YAW_OFFSET, hip_pitch_angle=joints[1].getAngle()+s.robot.PITCH_OFFSET, knee_angle=joints[2].getAngle()+s.robot.KNEE_OFFSET )
for j,r in zip(joints, lRates):
j.setLengthRate(r)
draw_contacts=1,
publish_int=20)
yaw_joint = s.robot.joints['hip_yaw']
pitch_joint = s.robot.joints['hip_pitch']
knee_joint = s.robot.joints['knee_pitch']
yaw_joint_bl = s.robot.joints['hip_yaw_bl']
pitch_joint_bl = s.robot.joints['hip_pitch_bl']
knee_joint_bl = s.robot.joints['knee_pitch_bl']
shock_joint = s.robot.joints['foot_shock']
control_period = 5e-3
last_time = 0.0
while True:
s.step()
time = s.getSimTime()
if not s.getPaused() and time - last_time >= control_period:
valve_cmd = update(time,\
yaw_joint.getAngle()+yaw_joint_bl.getAngle(),\
pitch_joint.getAngle()+pitch_joint_bl.getAngle(),\
knee_joint.getAngle()+knee_joint_bl.getAngle(),\
shock_joint.getPosition())
last_time = time
commands[0] = valve_cmd[0]
commands[1] = valve_cmd[1]
commands[2] = valve_cmd[2]
yaw_joint.setValveCommand(valve_cmd[0])
pitch_joint.setValveCommand(valve_cmd[1])
knee_joint.setValveCommand(valve_cmd[2])
# Check command-line arguments to find the planner module
update = importPlanner()
d = {'offset':(0,0,1.0)}
s = Simulator(dt=1e-3,plane=1,pave=0,graphical=1,robot=SpiderWHydraulics,robot_kwargs=d, start_paused = True)
input_server = InputServer()
input_server.startListening()
try:
last_command = None
while True:
s.step()
if not s.getPaused():
time = s.getSimTime()+.0001 # FIXME: first time time_delta is called, it returns zero, which means pid commands infinity
# FIXME: Known bug, getAcceleration returns (0,0,0)
next_command = input_server.getLastCommand()
command = next_command if next_command != last_command else None
last_command = next_command
# print command[15]
lr = update(time,\
s.robot.getEncoderAngleMatrix(),\
s.robot.getOrientation(),\
s.robot.getAcceleration(),\
s.robot.getAngularRates(),
command)
s.robot.setLenRateMatrix( lr )
#s.robot.constantSpeedWalk()
#logger.info("Main loop iteration", time=time, hip_yaw_rate=lr[0], hip_pitch_rate=lr[1], knee_pitch_rate=lr[2], hip_yaw_angle=yaw_joint.getAngle(), hip_pitch_angle=pitch_joint.getAngle(), knee_pitch_angle=knee_joint.getAngle(), shock_depth=shock_joint.getPosition())
d = {'offset':(0,0,2.00)}
s = Simulator(dt=5e-3,plane=1,pave=0,graphical=1,robot=LegOnColumn,robot_kwargs=d, start_paused = 1, render_objs=1, draw_contacts=1, publish_int=20)
yaw_joint = s.robot.joints['hip_yaw']
pitch_joint = s.robot.joints['hip_pitch']
knee_joint = s.robot.joints['knee_pitch']
yaw_joint_bl = s.robot.joints['hip_yaw_bl']
pitch_joint_bl = s.robot.joints['hip_pitch_bl']
knee_joint_bl = s.robot.joints['knee_pitch_bl']
shock_joint = s.robot.joints['foot_shock']
control_period = 5e-3
last_time = 0.0
while True:
s.step()
time = s.getSimTime()
if not s.getPaused() and time - last_time >= control_period:
valve_cmd = update(time,\
yaw_joint.getAngle()+yaw_joint_bl.getAngle(),\
pitch_joint.getAngle()+pitch_joint_bl.getAngle(),\
knee_joint.getAngle()+knee_joint_bl.getAngle(),\
shock_joint.getPosition())
last_time = time
commands[0] = valve_cmd[0]
commands[1] = valve_cmd[1]
commands[2] = valve_cmd[2]
yaw_joint.setValveCommand(valve_cmd[0])
pitch_joint.setValveCommand(valve_cmd[1])
knee_joint.setValveCommand(valve_cmd[2])
ground_grade=0.,
robot=SpiderWHydraulics,
robot_kwargs=d,
render_objs=1,
draw_contacts=0)
last_t = 0
def toScale(val):
return float(val) / 128. - 1
try:
while True:
s.step()
global_time.updateTime(s.getSimTime())
if s.getSimTime() - last_t > .001:
cmd = input_server.getLastCommand()
if not cmd:
time.sleep(.5)
continue
x = toScale(cmd[18])
y = toScale(cmd[17])
z = toScale(cmd[20])
rot = toScale(cmd[19])
print x, y, z, rot
s.robot.constantSpeedWalkSmart(x_scale=x,
y_scale=y,
z_scale=z,
rot_scale=rot)
last_t = s.getSimTime()
plane=1,
pave=0,
graphical=1,
ground_grade=.00,
robot=SpiderWHydraulics,
robot_kwargs=d,
render_objs=1,
draw_contacts=0)
last_t = 0
x_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
y_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
z_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
r_low = LowPassFilter(gain=1.0, corner_frequency=1.2)
while True:
s.step()
global_time.updateTime(s.getSimTime())
if s.getSimTime() - last_t > .001:
x = -(stick.get_axis(1) + .15466)
if x < 0:
x *= 2
x /= .7
y = -(stick.get_axis(0) + .15466)
if y < 0:
y *= 2
y /= .7
z = -(stick.get_axis(3) - .29895)
if z < 0:
z *= 2
rot = stick.get_axis(2) + .226806
x = x_low.update(x)
s = Simulator(dt=1e-3,
plane=1,
pave=0,
graphical=1,
robot=SpiderWHydraulics,
robot_kwargs=d,
start_paused=True)
input_server = InputServer()
input_server.startListening()
try:
last_command = None
while True:
s.step()
if not s.getPaused():
time = s.getSimTime(
) + .0001 # FIXME: first time time_delta is called, it returns zero, which means pid commands infinity
# FIXME: Known bug, getAcceleration returns (0,0,0)
next_command = input_server.getLastCommand()
command = next_command if next_command != last_command else None
last_command = next_command
# print command[15]
lr = update(time, s.robot.getEncoderAngleMatrix(),
s.robot.getOrientation(), s.robot.getAcceleration(),
s.robot.getAngularRates(), command)
s.robot.setLenRateMatrix(lr)
# s.robot.constantSpeedWalk()
# logger.info("Main loop iteration", time=time, hip_yaw_rate=lr[0], hip_pitch_rate=lr[1], knee_pitch_rate=lr[2], hip_yaw_angle=yaw_joint.getAngle(), hip_pitch_angle=pitch_joint.getAngle(), knee_pitch_angle=knee_joint.getAngle(), shock_depth=shock_joint.getPosition())
except:
input_server.stopListening()
# logger.error("Main loop exception!")
knee_joint = s.robot.members['knee_pitch']
shock_joint = s.robot.members['foot_shock']
# def jointAnglesFromFootPosition( hip_yaw_angle, hip_pitch_angle, knee_angle, robot ):
# def footPositionFromJointAngles( foot_x, foot_y, foot_z, robot ):
# class Trajectory:
# def __init__( self, start_foot_pos, target_foot_pos, robot, start_sim_t, end_sim_t ):
# def getTargetJointAngles( self, sim_t ):
state = 0
yaw_control = JointController(yaw_joint)
pitch_control = JointController(pitch_joint)
knee_control = JointController(knee_joint)
while True:
start_t = s.getSimTime()
shock_depth = shock_joint.getPosition()
pos = footPositionFromJointAngles(yaw_joint.getAngle(), pitch_joint.getAngle(), knee_joint.getAngle(), shock_depth, s.robot)
print "state", state
print "start", pos
print "shock", shock_depth
if state == 0:
target_pos = (pos[0], pos[1], -4.0)
end_t = start_t + 4
elif state == 1:
target_pos = (pos[0], pos[1], pos[2] - shock_depth + 1.25)
end_t = start_t + 1
end_t = 0
p = ball.getPosition()
print 'Ball starting at: (%.2f,%.2f,%.2f)' % (p[0], p[1], p[2])
# The three joints are:
# [ hip yaw, hip pitch, knee pitch ]
joints = s.robot.joints
while True:
s.step()
points = checkVictory()
if points:
if end_t == 0.0:
# Now that we've won, set a time to end the simulation
end_t = s.getSimTime() + 5.0
print 'Victory detected! Counting down to exit...'
elif s.getSimTime() > end_t:
print "You won with %d points!" % points
break
lRates = StudentControl.control(
sim_time=s.getSimTime(),
hip_yaw_angle=joints[0].getAngle() + s.robot.YAW_OFFSET,
hip_pitch_angle=joints[1].getAngle() + s.robot.PITCH_OFFSET,
knee_angle=joints[2].getAngle() + s.robot.KNEE_OFFSET)
for j, r in zip(joints, lRates):
j.setLengthRate(r)
