def bdi_step_queue_out(self):
bdi_step_queue_out = [s.copy() for s in self.bdi_step_queue_in]
for step in bdi_step_queue_out:
# Transform to BDI coordinate frame
T1 = ut.mk_transform(step.pos[:3], step.pos[3:])
T2 = ut.mk_transform(self.T_local_to_localbdi.trans,
ut.quat2rpy(self.T_local_to_localbdi.quat))
T = T2.dot(T1)
step.pos[:3] = T[:3, 3]
step.pos[3:] = ut.rotmat2rpy(T[:3, :3])
self.lc.publish(
'BDI_ADJUSTED_FOOTSTEP_PLAN',
encode_footstep_plan(bdi_step_queue_out,
self.last_params).encode())
for step in bdi_step_queue_out:
# Express pos of the center of the foot, as expected by BDI
R = ut.rpy2rotmat(step.pos[3:])
offs = R.dot(ATLAS_FRAME_OFFSET)
# import pdb; pdb.set_trace()
step.pos[:3] += offs
for i in reversed(range(2, len(bdi_step_queue_out))):
bdi_step_queue_out[i].pos[2] -= bdi_step_queue_out[i - 1].pos[2]
return [
s.to_bdi_spec(self.behavior, j + 1)
for j, s in enumerate(bdi_step_queue_out[2:])
]
def from_goal_msg(goal_msg):
rpy = quat2rpy([
goal_msg.pos.rotation.w, goal_msg.pos.rotation.x,
goal_msg.pos.rotation.y, goal_msg.pos.rotation.z
])
goal = FootGoal(pos=pl.hstack([
goal_msg.pos.translation.x, goal_msg.pos.translation.y,
goal_msg.pos.translation.z, rpy
]),
step_speed=goal_msg.step_speed,
step_height=goal_msg.step_height,
step_id=goal_msg.id,
pos_fixed=[
goal_msg.fixed_x, goal_msg.fixed_y,
goal_msg.fixed_z, goal_msg.fixed_roll,
goal_msg.fixed_pitch, goal_msg.fixed_yaw
],
is_right_foot=goal_msg.is_right_foot,
is_in_contact=goal_msg.is_in_contact,
bdi_step_duration=goal_msg.bdi_step_duration,
bdi_sway_duration=goal_msg.bdi_sway_duration,
bdi_lift_height=goal_msg.bdi_lift_height,
bdi_toe_off=goal_msg.bdi_toe_off,
bdi_knee_nominal=goal_msg.bdi_knee_nominal,
bdi_max_body_accel=goal_msg.bdi_max_body_accel,
bdi_max_foot_vel=goal_msg.bdi_max_foot_vel,
bdi_sway_end_dist=goal_msg.bdi_sway_end_dist,
bdi_step_end_dist=goal_msg.bdi_step_end_dist,
support_contact_groups=goal_msg.support_contact_groups,
terrain_pts=pl.vstack([
goal_msg.terrain_path_dist, goal_msg.terrain_height
]))
if any(pl.isnan(goal.pos[[0, 1, 5]])):
raise ValueError("I don't know how to handle NaN in x, y, or yaw")
else:
goal.pos[pl.find(pl.isnan(goal.pos))] = 0
return goal
def from_footstep_msg(goal_msg):
rpy = quat2rpy(
[goal_msg.pos.rotation.w, goal_msg.pos.rotation.x, goal_msg.pos.rotation.y, goal_msg.pos.rotation.z]
)
goal = FootGoal(
pos=pl.hstack([goal_msg.pos.translation.x, goal_msg.pos.translation.y, goal_msg.pos.translation.z, rpy]),
step_speed=goal_msg.params.step_speed,
step_height=goal_msg.params.step_height,
step_id=goal_msg.id,
pos_fixed=[
goal_msg.fixed_x,
goal_msg.fixed_y,
goal_msg.fixed_z,
goal_msg.fixed_roll,
goal_msg.fixed_pitch,
goal_msg.fixed_yaw,
],
is_right_foot=goal_msg.is_right_foot,
is_in_contact=goal_msg.is_in_contact,
bdi_step_duration=goal_msg.params.bdi_step_duration,
bdi_sway_duration=goal_msg.params.bdi_sway_duration,
bdi_lift_height=goal_msg.params.bdi_lift_height,
bdi_toe_off=goal_msg.params.bdi_toe_off,
bdi_knee_nominal=goal_msg.params.bdi_knee_nominal,
bdi_max_body_accel=goal_msg.params.bdi_max_body_accel,
bdi_max_foot_vel=goal_msg.params.bdi_max_foot_vel,
bdi_sway_end_dist=goal_msg.params.bdi_sway_end_dist,
bdi_step_end_dist=goal_msg.params.bdi_step_end_dist,
support_contact_groups=goal_msg.params.support_contact_groups,
terrain_pts=pl.vstack([goal_msg.terrain_path_dist, goal_msg.terrain_height]),
)
if any(pl.isnan(goal.pos[[0, 1, 5]])):
raise ValueError("I don't know how to handle NaN in x, y, or yaw")
else:
goal.pos[pl.find(pl.isnan(goal.pos))] = 0
return goal
def bdi_step_queue_out(self):
bdi_step_queue_out = [s.copy() for s in self.bdi_step_queue_in]
for step in bdi_step_queue_out:
# Transform to BDI coordinate frame
T1 = ut.mk_transform(step.pos[:3], step.pos[3:])
T2 = ut.mk_transform(self.T_local_to_localbdi.trans, ut.quat2rpy(self.T_local_to_localbdi.quat))
T = T2.dot(T1)
step.pos[:3] = T[:3,3]
step.pos[3:] = ut.rotmat2rpy(T[:3,:3])
self.lc.publish('BDI_ADJUSTED_FOOTSTEP_PLAN', encode_footstep_plan(bdi_step_queue_out, self.last_params).encode())
for step in bdi_step_queue_out:
# Express pos of the center of the foot, as expected by BDI
R = ut.rpy2rotmat(step.pos[3:])
offs = R.dot(ATLAS_FRAME_OFFSET)
# import pdb; pdb.set_trace()
step.pos[:3] += offs
for i in reversed(range(2, len(bdi_step_queue_out))):
bdi_step_queue_out[i].pos[2] -= bdi_step_queue_out[i-1].pos[2]
return [s.to_bdi_spec(self.behavior, j+1) for j, s in enumerate(bdi_step_queue_out[2:])]
