def generate_plan(self, behavior):
plan = drc.footstep_plan_t()
plan.utime = 0
plan.params = drc.footstep_plan_params_t()
plan.params.behavior = behavior
plan.footsteps = []
plan.num_steps = 10
for j in range(plan.num_steps):
goal = drc.footstep_t()
goal.utime = 0
goal.pos = drc.position_3d_t()
goal.pos.translation = drc.vector_3d_t()
goal.params = drc.footstep_params_t()
if 1 <= j <= 2: # this works in Python!
goal.pos.translation.x = 0
goal.pos.translation.y = 0
goal.pos.translation.z = 0
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = 0
goal.pos.rotation.y = 0
goal.pos.rotation.z = 0
goal.pos.rotation.w = 1
else:
goal.pos.translation.x = 0.15 * j + (0.5 -
random.random()) * 0.15
goal.pos.translation.y = random.random() * 0.15 * j + (
0.5 - random.random()) * 0.2
goal.pos.translation.z = (0.5 - random.random()) * 0.2
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = random.random()
goal.pos.rotation.y = random.random()
goal.pos.rotation.z = random.random()
goal.pos.rotation.w = random.random()
goal.id = j + 1
goal.is_right_foot = j % 2 == 0
goal.is_in_contact = True
goal.fixed_x = True
goal.fixed_y = True
goal.fixed_z = True
goal.fixed_roll = True
goal.fixed_pitch = True
goal.fixed_yaw = True
goal.params.step_speed = 1.5
goal.params.step_height = random.random() * 0.25
goal.params.bdi_step_duration = 2.0
goal.params.bdi_sway_duration = 0
goal.params.bdi_lift_height = random.random() * 0.15
goal.params.bdi_toe_off = 0
goal.params.bdi_knee_nominal = 0
goal.num_terrain_pts = 3
goal.terrain_path_dist = [0, 0.1, 0.2]
goal.terrain_height = [0, random.random() * 0.15, 0]
plan.footsteps.append(goal)
return plan
def encode_footstep_plan(footsteps, params):
plan = drc.footstep_plan_t()
plan.num_steps = len(footsteps)
plan.footsteps = [step.to_footstep_t() for step in footsteps]
if params is not None:
plan.params = params
else:
plan.params = drc.footstep_plan_params_t()
plan.iris_region_assignments = [-1 for f in footsteps]
return plan
def encode_footstep_plan(footsteps, params):
plan = drc.footstep_plan_t()
plan.num_steps = len(footsteps)
plan.footsteps = [step.to_footstep_t() for step in footsteps]
if params is not None:
plan.params = params
else:
plan.params = drc.footstep_plan_params_t()
plan.iris_region_assignments = [-1 for f in footsteps]
return plan
def generate_plan(self, behavior):
plan = drc.footstep_plan_t()
plan.utime = 0
plan.params = drc.footstep_plan_params_t()
plan.params.behavior = behavior
plan.footsteps = []
plan.num_steps = 10
for j in range(plan.num_steps):
goal = drc.footstep_t()
goal.utime = 0
goal.pos = drc.position_3d_t();
goal.pos.translation = drc.vector_3d_t()
goal.params = drc.footstep_params_t()
if 1 <= j <= 2: # this works in Python!
goal.pos.translation.x = 0
goal.pos.translation.y = 0
goal.pos.translation.z = 0
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = 0
goal.pos.rotation.y = 0
goal.pos.rotation.z = 0
goal.pos.rotation.w = 1
else:
goal.pos.translation.x = 0.15 * j + (0.5 - random.random()) * 0.15
goal.pos.translation.y = random.random() * 0.15 * j + (0.5 - random.random()) * 0.2
goal.pos.translation.z = (0.5 - random.random()) * 0.2
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = random.random()
goal.pos.rotation.y = random.random()
goal.pos.rotation.z = random.random()
goal.pos.rotation.w = random.random()
goal.id = j+1
goal.is_right_foot = j % 2 == 0
goal.is_in_contact = True
goal.fixed_x = True
goal.fixed_y = True
goal.fixed_z = True
goal.fixed_roll = True
goal.fixed_pitch= True
goal.fixed_yaw = True
goal.params.step_speed = 1.5
goal.params.step_height = random.random() * 0.25
goal.params.bdi_step_duration = 2.0
goal.params.bdi_sway_duration = 0
goal.params.bdi_lift_height = random.random() * 0.15
goal.params.bdi_toe_off = 0
goal.params.bdi_knee_nominal = 0
goal.num_terrain_pts = 3
goal.terrain_path_dist = [0, 0.1, 0.2]
goal.terrain_height = [0, random.random() * 0.15, 0]
plan.footsteps.append(goal)
return plan
def request_handler(channel, data):
request = drc.footstep_plan_request_t.decode(data)
# ----- Send LCM message based on received info -----
# Create Footstep planner and get solution
f = FootstepPlanner(2)
# Create Atlas object
print("----------------------------------")
pm = PackageMap()
model = os.path.abspath(
os.path.join(os.sep, "home", "rahuly", "Data", "drake-new",
"drake-distro", "drake", "examples", "atlas", "urdf",
"atlas_minimal_contact.urdf"))
pm.PopulateUpstreamToDrake(model)
robot = rbtree.RigidBodyTree(
model,
package_map=pm,
floating_base_type=rbtree.FloatingBaseType.kQuaternion)
t = request.initial_state.pose.translation
r = request.initial_state.pose.rotation
q = [t.x, t.y, t.z] + [r.w, r.x, r.y, r.z] + list(
request.initial_state.joint_position)
q = tuple(q)
v = [0] * 6 + list(request.initial_state.joint_velocity)
v = tuple(v)
kinsol = robot.doKinematics(q, v)
r_startpos = robot.transformPoints(
kinsol, np.zeros((3, 1)),
robot.findFrame("r_foot_sole").get_frame_index(), 0)
l_startpos = robot.transformPoints(
kinsol, np.zeros((3, 1)),
robot.findFrame("l_foot_sole").get_frame_index(), 0)
print("r_startpos: " + str(r_startpos))
print("l_startpos: " + str(l_startpos))
print("----------------------------------")
f.setStartRL(r_startpos, l_startpos)
f.setGoal(
[request.goal_pos.translation.x, request.goal_pos.translation.y])
f.setReachable([(0, -0.2), (0.5, 0), (0, 0.2), (-0.3, 0)], 0.3)
# f.setNominal(0.5)
f.solveProgram()
# Package solution into footstep_plan_t
plan = drc.footstep_plan_t()
# - Footsteps
plan.num_steps = 2 * f.numFootsteps
for fNum in range(0, f.numFootsteps):
# -- Right Footstep
rstep = drc.footstep_t()
rstep.pos.rotation.w = 1
rspos = np.array([[f.footsteps[2 * fNum][0]],
[f.footsteps[2 * fNum][1]], [0]])
# rs = np.array([f.footsteps[2*fNum][0], f.footsteps[2*fNum][1], 0])
rs = robot.transformPoints(
kinsol, rspos,
robot.FindBody("r_foot").get_body_index(),
robot.findFrame("r_foot_sole").get_frame_index())
print("Right: " + str(rs))
rstep.pos.translation.x = rs[0]
rstep.pos.translation.y = rs[1]
rstep.pos.translation.z = rs[2]
# pos.rotation ??
rstep.id = 2 * fNum
rstep.is_right_foot = True
rstep.params = request.default_step_params
plan.footsteps.append(rstep)
# -- Left Footstep
lstep = drc.footstep_t()
lstep.pos.rotation.w = 1
lspos = np.array([[f.footsteps[2 * fNum + 1][0]],
[f.footsteps[2 * fNum + 1][1]], [0]])
# ls = np.array([f.footsteps[2*fNum+1][0], f.footsteps[2*fNum+1][1], 0])
ls = robot.transformPoints(
kinsol, lspos,
robot.FindBody("l_foot").get_body_index(),
robot.findFrame("l_foot_sole").get_frame_index())
print("Left: " + str(ls))
lstep.pos.translation.x = ls[0]
lstep.pos.translation.y = ls[1]
lstep.pos.translation.z = ls[2]
# pos.rotation ??
lstep.id = 2 * fNum + 1
lstep.is_right_foot = False
lstep.params = request.default_step_params
plan.footsteps.append(lstep)
# - IRIS Regions
plan.num_iris_regions = request.num_iris_regions
plan.iris_regions = request.iris_regions
plan.iris_region_assignments = [-1] * plan.num_steps
# - Params: AFTER WE HAVE REQUESTS WORKING
plan.params = request.params
# Send footstep_plan_t
lc.publish("FOOTSTEP_PLAN", plan.encode())