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])
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())
except:
