def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
global state, ja, sw
# 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 ja is None:
ja = model.getJointAngles()
ja[HP] = -0.7
ja[KP] = 1.5
sw = time_sources.StopWatch()
print "SW1"
print "between"
if sw.getTime() > 5.0:
if ja[KP] == 1.5:
ja[KP] = 1.8
else:
ja[KP] = 1.5
sw = time_sources.StopWatch()
# Evaluate path and joint control
controller.update(model.getJointAngles(), ja)
# Send commands
lr = controller.getLengthRateCommands()
lr[YAW] = 0.0
lr[HP] = 0.0
return lr
def __init__(self, leg_model, limb_controller, duration):
self.model = leg_model
self.controller = limb_controller
self.initial_angles = self.model.getJointAngles()
self.duration = duration
self.sw = time_sources.StopWatch()
self.done = False
def __init__(self, leg_model, limb_controller, velocity, accel_duration=0.1):
logger.info("New trajectory.",
traj_name="PutFootOnGround",
velocity=velocity,
accel_duration=accel_duration)
self.model = leg_model
self.controller = limb_controller
self.vel = velocity
self.accel_duration = accel_duration
self.done = self.model.isFootOnGround()
self.target_foot_pos = self.model.footPosFromLegState(
[self.controller.getDesiredPosAngle(), self.model.getShockDepth()])
self.stop_watch = time_sources.StopWatch(active=False)
if not self.done:
self.stop_watch.smoothStart(self.accel_duration)
def __init__(self,
leg_model,
limb_controller,
joint_idx,
direction,
velocity=0.05,
accel_duration=0.1):
self.model = leg_model
self.controller = limb_controller
self.joint = joint_idx
self.vel = direction * velocity
self.target_angles = self.model.getJointAngles()
self.sw = time_sources.StopWatch()
self.sw.smoothStart(accel_duration)
self.moving = False
self.done = False
def __init__(self, body_model, body_controller, leg_index, delta_angle,
max_velocity, acceleration):
logger.info("New path.",
path_name="RotateFootAboutOrigin",
delta_angle=delta_angle,
max_velocity=max_velocity,
acceleration=acceleration)
self.file_name = "rotate_foot_about_origin_data_leg%d.csv" % leg_index
self.file = open(self.file_name, "w")
self.body_model = body_model
self.leg_index = leg_index
self.leg_model = self.body_model.getLegs()[self.leg_index]
self.body_controller = body_controller
self.limb_controller = self.body_controller.getLimbControllers()[
self.leg_index]
self.target_foot_pos = self.leg_model.getFootPos()
self.delta_angle = delta_angle
self.max_ang_vel = max_velocity
self.ang_vel = 0.0
self.ang_acc = acceleration
self.body_coord = self.body_model.transformLeg2Body(
self.leg_index,
self.leg_model.footPosFromLegState(
[self.limb_controller.getDesiredPosAngle(),
0])) # self.leg_model.getShockDepth()]))
#self.body_coord = self.body_model.transformLeg2Body(self.leg_index,[2,0,-1])
self.init_angle = arctan2(self.body_coord[1], self.body_coord[0])
self.last_commanded_angle = self.init_angle
self.target_angle = self.init_angle
self.radius = norm([self.body_coord[0], self.body_coord[1]])
self.init_height = self.body_coord[2]
# Unit vector pointing towards the destination
self.dir = sign(self.delta_angle)
self.done = False
self.sw = time_sources.StopWatch()
self.sw.smoothStart(1) # self.accel_duration)
def __init__(self,
leg_model,
limb_controller,
joint_idx,
duration,
direction,
velocity=0.1,
accel_duration=0.1):
assert abs(direction) == 1
self.model = leg_model
self.controller = limb_controller
self.joint = joint_idx
self.duration = duration
self.vel = direction * velocity
self.accel_duration = accel_duration
self.target_angles = self.model.getJointAngles()
self.stopping = False
self.sw = time_sources.StopWatch()
self.sw.smoothStart(self.accel_duration)
def __init__(self, leg_model, limb_controller, way_points):
self.model = leg_model
self.controller = limb_controller
self.done = False
self.time_array = way_points[0]
self.spatial_array = way_points[1:, :]
start_error = self.spatial_array[:, 0] - self.model.getFootPos()
#adjust all values to be relative to actual start position
for i in range(0, way_points.shape[1]):
self.spatial_array[:, i] -= start_error
self.f = interpolate.interp1d(self.time_array,
self.spatial_array,
kind='cubic')
self.target_foot_pos = self.f(0)
self.stop_watch = time_sources.StopWatch(
active=True, time_source=time_sources.global_time)