def __init__(self,
leg_model,
max_acceleration=None,
initial_velocity=None,
initial_position=None,
time_source=global_time):
# avoid mutable in function declaration
if None is max_acceleration:
max_acceleration = array([20.0, 20.0, 20.0])
self.model = leg_model
self.setMaxAcceleration(max_acceleration)
self.ts = time_source
if initial_velocity is None:
self.vel = array([0.0, 0.0, 0.0])
else:
self.vel = initial_velocity
if initial_position is None:
self.pos = self.model.getFootPos()
else:
self.pos = initial_position
self.setVelocity(self.vel)
def __init__(self,
leg_model,
max_acceleration=None,
initial_velocity=None,
initial_position=None,
time_source=global_time):
# avoid mutable in function declaration
if None is max_acceleration:
max_acceleration = array([20.0, 20.0, 20.0])
self.model = leg_model
self.setMaxAcceleration(max_acceleration)
self.ts = time_source
if initial_velocity is None:
self.vel = array([0.0, 0.0, 0.0])
else:
self.vel = initial_velocity
if initial_position is None:
self.pos = self.model.getFootPos()
else:
self.pos = initial_position
self.setVelocity(self.vel)
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Init path. Do this after the first update.
if path is None:
path = Pause(model, controller, 5.0)
# Monitor leg_paths
if path.isDone():
if state == S_INIT:
print "Move" * 1000
path = TrapezoidalFootMove(model, controller,
array([1.5, 0.0, -0.4]), 0.5, 0.5)
state = S_MOVE_JOINT
elif state == S_MOVE_JOINT:
print "Done" * 1000
state = S_DONE
pass
logger.info("State changed.", state=state)
print "Foot:", model.getFootPos()
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Init path. Do this after the first update.
if path is None:
path = Pause(model, controller, 5.0)
# Monitor leg_paths
if path.isDone():
if state == S_INIT:
print "Move" * 1000
path = TrapezoidalFootMove(model, controller, array([1.5, 0.0, -0.4]), 0.5, 0.5)
state = S_MOVE_JOINT
elif state == S_MOVE_JOINT:
print "Done" * 1000
state = S_DONE
pass
logger.info("State changed.", state=state)
print "Foot:", model.getFootPos()
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def update( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
# Update model
time_sources.global_time.updateTime(time)
self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
self.model.updateFootOnGround()
# Init path. Do this after the first update.
if self.path is None:
self.path = Pause(self.model, controller, 1.0)
if self.path.isDone():
self.path = TrapezoidalFootMove(self.model, controller,
array(self.points[self.index][0:3]),
self.points[self.index][3], 0.4)
self.index = (self.index+1)%len(self.points)
# Evaluate path and joint control
present_angles = self.model.getJointAngles()
target_angles = self.path.update()
controller.update(present_angles, target_angles)
control_lin_rates = [self.joint_controllers[i].update(target_angles[i], present_angles[i]) for i in range(3)]
target_lin_rates = [c.getDesiredLengthRate() for c in self.joint_controllers]
measured_lin_rates = [c.getLengthRate() for c in self.joint_controllers]
if 0:
s = ''
m = self.pist_helpers[0].clay_pit_pos.mem
for i in range(len(m)):
if not i%8:
s += '%2.2f '%(m[i])
print s
commands = [self.pist_helpers[i].update(control_lin_rates[i], measured_lin_rates[i]) for i in range(3)]
#commands = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]
if 0:
target_ang_rates = [c.desired_vel for c in self.joint_controllers]
measured_ang_rates = [c.getAngleRate() for c in self.joint_controllers]
s = 'Measured: '
for m in measured_ang_rates:
s += '%1.3f '%(m*180/pi)
print s
s = 'Target: '
for m in target_ang_rates:
s += '%1.3f '%(m*180/pi)
print s
s = 'M Lin'
for m in measured_lin_rates:
s += '%1.3f '%m
print s
s = 'T Lin'
for m in target_lin_rates:
s += '%1.3f '%m
print s
self.pist_helpers[0].clay_pit_pos.printpit()
return commands
def __init__(self, body_model, body_controller, max_velocity=2.0, acceleration=1.0):
leg_logger.logger.info("New path.", path_name="GoToStandardHexagon",
max_velocity=max_velocity, acceleration=acceleration)
self.model = body_model
self.controller = body_controller
self.max_velocity = max_velocity
self.acceleration = acceleration
self.final_joint_positions = self.controller.getTargetJointAngleMatrix()
self.foot_paths = [None for i in range(NUM_LEGS)]
self.lifted = array([1.651, 0, 0])
self.lowered = array([1.651,0,-1.6764])
self.NONE = 0
self.LOWER_ALL = 1
self.EVENS = 2
self.LOWER_EVENS = 3
self.ODDS = 4
self.LOWER_ODDS = 5
self.STAND = 6
self.state = self.LOWER_ALL
self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()]
self.in_place = [False]*6
# Put all 6 legs in place
for i in range(NUM_LEGS):
if not self.on_ground[i] and not self.in_place[i]:
self.foot_paths[i]=TrapezoidalFootMove(
self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.lifted,
self.max_velocity, self.acceleration)
self.in_place[i] = True
if all(self.in_place):
self.state = self.STAND
self.phase_done = False
self.done = False
def __init__(self, body_model, body_controller, max_velocity=2.0, acceleration=1.0):
leg_logger.logger.info("New path.",
path_name="GoToStandardHexagon",
max_velocity=max_velocity,
acceleration=acceleration)
self.model = body_model
self.controller = body_controller
self.max_velocity = max_velocity
self.acceleration = acceleration
self.final_joint_positions = self.controller.getTargetJointAngleMatrix()
self.foot_paths = [None for i in range(NUM_LEGS)]
self.lifted = array([1.651 , 0 , 0])
self.lowered = array([1.651 , 0 , -1.6764])
self.NONE = 0
self.LOWER_ALL = 1
self.EVENS = 2
self.LOWER_EVENS = 3
self.ODDS = 4
self.LOWER_ODDS = 5
self.STAND = 6
self.state = self.LOWER_ALL
self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()]
self.in_place = [False] * 6
# Put all 6 legs in place
for i in range(NUM_LEGS):
if not self.on_ground[i] and not self.in_place[i]:
self.foot_paths[i] = TrapezoidalFootMove(self.model.getLegs()[i],
self.controller.getLimbControllers()[i],
self.lifted,
self.max_velocity, self.acceleration)
self.in_place[i] = True
self.phase_done = False
self.done = False
def moveToInitialPosition(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
if not hasattr(self, 'initial_path'):
print "Starting move to initial position"
self.initial_path = TrapezoidalFootMove(self.model, self.controller,
array([2.0, 0.0, -0.2]),
0.4, 0.2)
self.controller.update(self.model.getJointAngles(), self.initial_path.update())
if self.initial_path.isDone():
self.callback = self.pause
return self.controller.getLengthRateCommands()
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Init path. Do this after the first update.
if path is None:
path = Pause(model, controller, 1.0)
# Monitor leg_paths
if path.isDone():
if state == S_MOVE3:
path = TrapezoidalFootMove(model, controller,
array([1.5, -0.6, -0.4]),
0.2, 0.1)
state = S_MOVE1
elif state == S_MOVE1:
path = PutFootOnGround(model, controller, 0.05)
state = S_LOWER
elif state == S_LOWER:
ground_level = model.getFootPos()[Z]
path = TrapezoidalFootMove(model, controller,
array([1.5, .6, ground_level]),
0.2, 0.1)
state = S_MOVE2
elif state == S_MOVE2:
path = TrapezoidalFootMove(model, controller,
array([1.5, 0.6, -0.4]),
0.2, 0.1)
state = S_MOVE3
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def moveToInitialPosition( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ):
self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
if not hasattr(self, 'initial_path'):
print "Starting move to initial position"
self.initial_path = TrapezoidalFootMove(self.model, self.controller,\
array([1.6, 0.0, -1.15]),\
0.4, 0.2)
self.controller.update(self.model.getJointAngles(), self.initial_path.update())
if self.initial_path.isDone():
self.callback = self.pause
lr = self.controller.getLengthRateCommands()
return lr
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
return array([0.0, 5.0, 0.0])
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
return array([0.0, 5.0, 0.0])
def __init__(
self,
leg_model,
max_acceleration=array([20.0, 20.0, 20.0]),
initial_velocity=None,
initial_position=None,
time_source=global_time,
):
self.model = leg_model
self.setMaxAcceleration(max_acceleration)
self.ts = time_source
if initial_velocity is None:
self.vel = array([0.0, 0.0, 0.0])
else:
self.vel = initial_velocity
if initial_position is None:
self.pos = self.model.getFootPos()
else:
self.pos = initial_position
self.setVelocity(self.vel)
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Init path. Do this after the first update.
if path is None:
path = Pause(model, controller, 1.0)
# Monitor leg_paths
if path.isDone():
if state == S_MOVE3:
path = TrapezoidalFootMove(model, controller,
array([1.5, -0.6, -0.4]), 0.2, 0.1)
state = S_MOVE1
elif state == S_MOVE1:
path = PutFootOnGround(model, controller, 0.05)
state = S_LOWER
elif state == S_LOWER:
ground_level = model.getFootPos()[Z]
path = TrapezoidalFootMove(model, controller,
array([1.5, .6, ground_level]), 0.2,
0.1)
state = S_MOVE2
elif state == S_MOVE2:
path = TrapezoidalFootMove(model, controller,
array([1.5, 0.6, -0.4]), 0.2, 0.1)
state = S_MOVE3
logger.info("State changed.", state=state)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def __init__(self, body_model,
max_acceleration=array([20.0, 20.0, 20.0]),
initial_velocities=None,
initial_positions=None,
time_source=global_time):
self.model = body_model
self.paths = []
for i in range(NUM_LEGS):
self.paths.append(
FootVelocity(
self.model.getLegs()[i],
max_acceleration=max_acceleration,
initial_velocity=tryGetElement(initial_velocities, i, None),
initial_position=tryGetElement(initial_positions, i, None),
time_source=tryGetElement(time_source, i, time_source)))
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state, lr
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Evaluate path and joint control
JA = model.getJointAngles()
controller.update(JA, array([0.0,0.0,0.0]))
flie.write("%f,%f,%f,%f,%f,%f\n" % (lr[0],lr[1],lr[2],JA[0],JA[1],JA[2]))
lr[j_idx] += delta
return lr
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state, lr
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Evaluate path and joint control
JA = model.getJointAngles()
controller.update(JA, array([0.0, 0.0, 0.0]))
flie.write("%f,%f,%f,%f,%f,%f\n" %
(lr[0], lr[1], lr[2], JA[0], JA[1], JA[2]))
lr[j_idx] += delta
return lr
def __init__(self, body_model,
max_acceleration=None,
initial_velocities=None,
initial_positions=None,
time_source=global_time):
# don't put a mutable as a default arg
if None is max_acceleration:
max_acceleration = array([20.0, 20.0, 20.0])
self.model = body_model
self.paths = []
for i in range(NUM_LEGS):
self.paths.append(
FootVelocity(self.model.getLegs()[i],
max_acceleration=max_acceleration,
initial_velocity=tryGetElement(initial_velocities, i, None),
initial_position=tryGetElement(initial_positions, i, None),
time_source=tryGetElement(time_source, i, time_source)))
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global points, path, index
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Init path. Do this after the first update.
if path is None:
path = Pause(model, controller, 1.0)
if path.isDone():
path = TrapezoidalFootMove(model, controller, array(points[index]), 0.5, 0.4)
index = (index + 1) % len(points)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global points, path, index
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Init path. Do this after the first update.
if path is None:
path = Pause(model, controller, 1.0)
if path.isDone():
path = TrapezoidalFootMove(model, controller, array(points[index]),
0.5, 0.4)
index = (index + 1) % len(points)
# Evaluate path and joint control
controller.update(model.getJointAngles(), path.update())
# Send commands
return controller.getLengthRateCommands()
def __init__(self, body_model):
self.model = body_model
self.fv_path = FeetVelocities(self.model)
self.fv_path.setVelocities([array([0.2, 0.0, 0.0])] * NUM_LEGS)
from ControlsKit import time_sources, LegModel, LimbController
from ControlsKit.math_utils import array, KP, HP, YAW
# Initialization
model = LegModel()
controller = LimbController()
path = None
j_idx = YAW
delta = 0.00005
lr = array([0.0, 0.0, 0.0])
file_name = "gimpy_calibration_of_joint_%d.csv" % j_idx
flie = open(file_name, "w")
# Body of control loop
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state, lr
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Evaluate path and joint control
JA = model.getJointAngles()
controller.update(JA, array([0.0, 0.0, 0.0]))
flie.write("%f,%f,%f,%f,%f,%f\n" %
(lr[0], lr[1], lr[2], JA[0], JA[1], JA[2]))
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
return array([0.0, 0.0, 0.0])
from ControlsKit import time_sources, LegModel, LimbController
from ControlsKit.math_utils import array, KP, HP, YAW
# Initialization
model = LegModel()
controller = LimbController()
path = None
j_idx = YAW
delta = 0.00005
lr = array([0.0, 0.0, 0.0])
file_name = "gimpy_calibration_of_joint_%d.csv" % j_idx
flie = open(file_name, "w")
# Body of control loop
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global path, state, lr
# Update model
time_sources.global_time.updateTime(time)
model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
model.updateFootOnGround()
# Evaluate path and joint control
JA = model.getJointAngles()
controller.update(JA, array([0.0,0.0,0.0]))
def update(self,
time,
yaw,
hip_pitch,
knee_pitch,
shock_depth,
command=None):
# Update model
time_sources.global_time.updateTime(time)
self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
self.model.updateFootOnGround()
# Init path. Do this after the first update.
if self.path is None:
self.path = Pause(self.model, controller, 1.0)
if self.path.isDone():
self.path = TrapezoidalFootMove(
self.model, controller, array(self.points[self.index][0:3]),
self.points[self.index][3], 0.4)
self.index = (self.index + 1) % len(self.points)
# Evaluate path and joint control
present_angles = self.model.getJointAngles()
target_angles = self.path.update()
controller.update(present_angles, target_angles)
control_lin_rates = [
self.joint_controllers[i].update(target_angles[i],
present_angles[i])
for i in range(3)
]
target_lin_rates = [
c.getDesiredLengthRate() for c in self.joint_controllers
]
measured_lin_rates = [
c.getLengthRate() for c in self.joint_controllers
]
if 0:
s = ''
m = self.pist_helpers[0].clay_pit_pos.mem
for i in range(len(m)):
if not i % 8:
s += '%2.2f ' % (m[i])
print s
commands = [
self.pist_helpers[i].update(control_lin_rates[i],
measured_lin_rates[i])
for i in range(3)
]
#commands = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]
if 0:
target_ang_rates = [c.desired_vel for c in self.joint_controllers]
measured_ang_rates = [
c.getAngleRate() for c in self.joint_controllers
]
s = 'Measured: '
for m in measured_ang_rates:
s += '%1.3f ' % (m * 180 / pi)
print s
s = 'Target: '
for m in target_ang_rates:
s += '%1.3f ' % (m * 180 / pi)
print s
s = 'M Lin'
for m in measured_lin_rates:
s += '%1.3f ' % m
print s
s = 'T Lin'
for m in target_lin_rates:
s += '%1.3f ' % m
print s
self.pist_helpers[0].clay_pit_pos.printpit()
return commands