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 update(self):
if not self.isDone():
delta = time_sources.global_time.getDelta()
# if the remaining distance <= the time it would take to slow
# down times the average speed during such a deceleration (ie
# the distance it would take to stop)
remaining_angle = self.delta_angle + self.init_angle - self.last_commanded_angle
self.target_angle = self.last_commanded_angle
if norm(remaining_angle) <= .5 * self.ang_vel**2 / self.ang_acc:
self.ang_vel -= self.ang_acc * delta
else:
self.ang_vel += self.ang_acc * delta
self.ang_vel = min(self.ang_vel, self.max_ang_vel)
self.target_angle += self.dir * self.ang_vel * delta
self.last_commanded_angle = self.target_angle
self.target_foot_pos = [self.radius*cos(self.target_angle), self.radius*sin(self.target_angle), self.init_height]
if not sign(remaining_angle) == self.dir:
self.done = True
self.target_foot_pos = [self.radius*cos(self.delta_angle + self.init_angle), self.radius*sin(self.delta_angle + self.init_angle), self.init_height]
a = self.body_model.transformLeg2Body(self.leg_index,self.leg_model.getFootPos())
b = self.target_foot_pos
self.file.write("%f,%f,%f,%f,%f,%f\n" % (a[0],a[1],a[2],b[0],b[1],b[2]))
return self.leg_model.jointAnglesFromFootPos(self.body_model.transformBody2Leg(self.leg_index, self.target_foot_pos))
def update(self):
if not self.isDone():# and (self.start_on_ground or not self.model.isFootOnGround()):
delta = time_sources.global_time.getDelta()
# if the remaining distance <= the time it would take to slow
# down times the average speed during such a deceleration (ie
# the distance it would take to stop)
# rearranged multiplies to avoid confusing order of operations
# readability issues
remaining_vector = self.final_foot_pos - self.target_foot_pos
if norm(remaining_vector) <= .5 * self.vel**2 / self.acc:
self.vel -= self.acc * delta
else:
self.vel += self.acc * delta
self.vel = min(self.vel, self.max_vel)
self.target_foot_pos += self.dir * self.vel * delta
if norm(self.final_foot_pos-self.target_foot_pos)<0.02:
self.done = True
return self.model.jointAnglesFromFootPos(self.target_foot_pos, 0)
def update(self):
#if not self.isDone() and (self.start_on_ground or not self.model.isFootOnGround()):
if not self.isDone():
delta = time_sources.global_time.getDelta()
# if the remaining distance <= the time it would take to slow
# down times the average speed during such a deceleration (ie
# the distance it would take to stop)
# rearranged multiplies to avoid confusing order of operations
# readability issues
remaining_vector = self.final_foot_pos - self.target_foot_pos
if norm(remaining_vector) <= .5 * self.vel**2 / self.acc:
self.vel -= self.acc * delta
else:
self.vel += self.acc * delta
self.vel = min(self.vel, self.max_vel)
self.target_foot_pos += self.dir * self.vel * delta
if norm(self.final_foot_pos - self.target_foot_pos) < 0.02:
self.done = True
else:
self.done = True
return self.model.jointAnglesFromFootPos(self.target_foot_pos, 0)
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 update(self):
if not self.isDone():
delta = time_sources.global_time.getDelta()
# if the remaining distance <= the time it would take to slow
# down times the average speed during such a deceleration (ie
# the distance it would take to stop)
remaining_angle = self.delta_angle + self.init_angle - self.last_commanded_angle
self.target_angle = self.last_commanded_angle
if norm(remaining_angle) <= .5 * self.ang_vel**2 / self.ang_acc:
self.ang_vel -= self.ang_acc * delta
else:
self.ang_vel += self.ang_acc * delta
self.ang_vel = min(self.ang_vel, self.max_ang_vel)
self.target_angle += self.dir * self.ang_vel * delta
self.last_commanded_angle = self.target_angle
self.target_foot_pos = [
self.radius * cos(self.target_angle),
self.radius * sin(self.target_angle), self.init_height
]
if not sign(remaining_angle) == self.dir:
self.done = True
self.target_foot_pos = [
self.radius * cos(self.delta_angle + self.init_angle),
self.radius * sin(self.delta_angle + self.init_angle),
self.init_height
]
a = self.body_model.transformLeg2Body(self.leg_index,
self.leg_model.getFootPos())
b = self.target_foot_pos
self.file.write("%f,%f,%f,%f,%f,%f\n" %
(a[0], a[1], a[2], b[0], b[1], b[2]))
return self.leg_model.jointAnglesFromFootPos(
self.body_model.transformBody2Leg(self.leg_index,
self.target_foot_pos))
def update(self):
if not self.isDone():
delta = time_sources.global_time.getDelta()
# if the remaining distance <= the time it would take to slow
# down times the average speed during such a deceleration (ie
# the distance it would take to stop)
# rearranged multiplies to avoid confusing order of operations
# readability issues
remaining_vector = self.final_angles - self.target_angles
if norm(remaining_vector) <= .5 * self.vel**2 / self.acc:
self.vel -= self.acc * delta
else:
self.vel += self.acc * delta
self.vel = min(self.vel, self.max_vel)
self.target_angles += self.dir * self.vel * delta
if not arraysAreEqual(self.getNormalizedRemaining(), self.dir):
self.done = True
self.target_angles = self.final_angles
return self.target_angles
def update(self):
if not self.isDone():
delta = time_sources.global_time.getDelta()
# if the remaining distance <= the time it would take to slow
# down times the average speed during such a deceleration (ie
# the distance it would take to stop)
# rearranged multiplies to avoid confusing order of operations
# readability issues
remaining_vector = self.final_angles - self.target_angles
if norm(remaining_vector) <= .5 * self.vel**2 / self.acc:
self.vel -= self.acc * delta
else:
self.vel += self.acc * delta
self.vel = min(self.vel, self.max_vel)
self.target_angles += self.dir * self.vel * delta
if not arraysAreEqual(self.getNormalizedRemaining(), self.dir):
self.done = True
self.target_angles = self.final_angles
return self.target_angles
