def to_footstep_t(self):
msg = drc.footstep_t()
msg.pos = drc.position_3d_t()
msg.pos.translation = drc.vector_3d_t()
msg.pos.translation.x, msg.pos.translation.y, msg.pos.translation.z = self.pos[:3]
msg.pos.rotation = drc.quaternion_t()
msg.pos.rotation.w, msg.pos.rotation.x, msg.pos.rotation.y, msg.pos.rotation.z = ut.rpy2quat(self.pos[3:])
msg.id = self.step_id
msg.is_right_foot = self.is_right_foot
msg.is_in_contact = self.is_in_contact
msg.fixed_x, msg.fixed_y, msg.fixed_z, msg.fixed_roll, msg.fixed_pitch, msg.fixed_yaw = self.pos_fixed
msg.num_terrain_pts = self.terrain_pts.shape[1]
if msg.num_terrain_pts > 0:
msg.terrain_path_dist = self.terrain_pts[0, :]
msg.terrain_height = self.terrain_pts[1, :]
msg.params = drc.footstep_params_t()
# TODO: this should probably be filled in
msg.params.bdi_step_duration = self.bdi_step_duration
msg.params.bdi_sway_duration = self.bdi_sway_duration
msg.params.bdi_lift_height = self.bdi_lift_height
msg.params.bdi_toe_off = self.bdi_toe_off
msg.params.bdi_knee_nominal = self.bdi_knee_nominal
msg.params.bdi_max_body_accel = self.bdi_max_body_accel
msg.params.bdi_max_foot_vel = self.bdi_max_foot_vel
msg.params.bdi_sway_end_dist = self.bdi_sway_end_dist
msg.params.bdi_step_end_dist = self.bdi_step_end_dist
msg.params.support_contact_groups = self.support_contact_groups
return msg
def to_footstep_t(self):
msg = drc.footstep_t()
msg.pos = bot_core.position_3d_t()
msg.pos.translation = bot_core.vector_3d_t()
msg.pos.translation.x, msg.pos.translation.y, msg.pos.translation.z = self.pos[:
3]
msg.pos.rotation = bot_core.quaternion_t()
msg.pos.rotation.w, msg.pos.rotation.x, msg.pos.rotation.y, msg.pos.rotation.z = ut.rpy2quat(
self.pos[3:])
msg.id = self.step_id
msg.is_right_foot = self.is_right_foot
msg.is_in_contact = self.is_in_contact
msg.fixed_x, msg.fixed_y, msg.fixed_z, msg.fixed_roll, msg.fixed_pitch, msg.fixed_yaw = self.pos_fixed
msg.num_terrain_pts = self.terrain_pts.shape[1]
if msg.num_terrain_pts > 0:
msg.terrain_path_dist = self.terrain_pts[0, :]
msg.terrain_height = self.terrain_pts[1, :]
msg.params = drc.footstep_params_t()
# TODO: this should probably be filled in
msg.params.bdi_step_duration = self.bdi_step_duration
msg.params.bdi_sway_duration = self.bdi_sway_duration
msg.params.bdi_lift_height = self.bdi_lift_height
msg.params.bdi_toe_off = self.bdi_toe_off
msg.params.bdi_knee_nominal = self.bdi_knee_nominal
msg.params.bdi_max_body_accel = self.bdi_max_body_accel
msg.params.bdi_max_foot_vel = self.bdi_max_foot_vel
msg.params.bdi_sway_end_dist = self.bdi_sway_end_dist
msg.params.bdi_step_end_dist = self.bdi_step_end_dist
msg.params.support_contact_groups = self.support_contact_groups
return msg
def makeStepMessages(self, stepFrames, leadingFoot, snapToTerrain=False):
assert leadingFoot in ('left', 'right')
isRightFootOffset = 0 if leadingFoot == 'left' else 1
footOriginToSole = -np.mean(FootstepsDriver.getContactPts(), axis=0)
stepMessages = []
for i, stepFrame in enumerate(stepFrames):
t = transformUtils.copyFrame(stepFrame)
t.PreMultiply()
t.Translate(footOriginToSole)
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(t)
step.is_right_foot = (i + isRightFootOffset) % 2
step.params = self.footstepsDriver.getDefaultStepParams()
step.fixed_x = True
step.fixed_y = True
step.fixed_z = True
step.fixed_roll = True
step.fixed_pitch = True
step.fixed_yaw = True
if snapToTerrain:
step.fixed_z = False
step.fixed_roll = False
step.fixed_pitch = False
stepMessages.append(step)
return stepMessages
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 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 createGoalSteps(self, model, pose):
distanceForward = 1.0
fr = model.getLinkFrame(_leftFootLink)
fl = model.getLinkFrame(_rightFootLink)
pelvisT = model.getLinkFrame(_pelvisLink)
xaxis = [1.0, 0.0, 0.0]
pelvisT.TransformVector(xaxis, xaxis)
xaxis = np.array(xaxis)
zaxis = np.array([0.0, 0.0, 1.0])
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
numGoalSteps = 3
is_right_foot = True
self.goalSteps = []
for i in range(numGoalSteps):
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
if is_right_foot:
t.Translate(fr.GetPosition())
else:
t.Translate(fl.GetPosition())
t.Translate(xaxis*distanceForward)
distanceForward += 0.15
is_right_foot = not is_right_foot
step = lcmdrc.footstep_t()
step.pos = positionMessageFromFrame(t)
step.is_right_foot = is_right_foot
step.params = self.getDefaultStepParams()
self.goalSteps.append(step)
request = self.constructFootstepPlanRequest(pose)
request.num_goal_steps = len(self.goalSteps)
request.goal_steps = self.goalSteps
self.sendFootstepPlanRequest(request)
def makeStepMessages(self, stepFrames, leadingFoot, snapToTerrain=False):
assert leadingFoot in ('left', 'right')
isRightFootOffset = 0 if leadingFoot == 'left' else 1
leftPoints, rightPoints = FootstepsDriver.getContactPts()
# note, assumes symmetrical feet. the loop below should be
# updated to alternate between left/right contact point sets
footOriginToSole = -np.mean(leftPoints, axis=0)
stepMessages = []
for i, stepFrame in enumerate(stepFrames):
t = transformUtils.copyFrame(stepFrame)
t.PreMultiply()
t.Translate(footOriginToSole)
step = lcmdrc.footstep_t()
step.pos = positionMessageFromFrame(t)
step.is_right_foot = (i + isRightFootOffset) % 2
step.params = self.footstepsDriver.getDefaultStepParams()
step.fixed_x = True
step.fixed_y = True
step.fixed_z = True
step.fixed_roll = True
step.fixed_pitch = True
step.fixed_yaw = True
if snapToTerrain:
step.fixed_z = False
step.fixed_roll = False
step.fixed_pitch = False
stepMessages.append(step)
return stepMessages
def makeStepMessages(self, stepFrames, leadingFoot, snapToTerrain=False):
assert leadingFoot in ('left', 'right')
isRightFootOffset = 0 if leadingFoot == 'left' else 1
leftPoints, rightPoints = FootstepsDriver.getContactPts()
# note, assumes symmetrical feet. the loop below should be
# updated to alternate between left/right contact point sets
footOriginToSole = -np.mean(leftPoints, axis=0)
stepMessages = []
for i, stepFrame in enumerate(stepFrames):
t = transformUtils.copyFrame(stepFrame)
t.PreMultiply()
t.Translate(footOriginToSole)
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(t)
step.is_right_foot = (i + isRightFootOffset) % 2
step.params = self.footstepsDriver.getDefaultStepParams()
step.fixed_x = True
step.fixed_y = True
step.fixed_z = True
step.fixed_roll = True
step.fixed_pitch = True
step.fixed_yaw = True
if snapToTerrain:
step.fixed_z = False
step.fixed_roll = False
step.fixed_pitch = False
stepMessages.append(step)
return stepMessages
def createGoalSteps(self, model, pose):
distanceForward = 1.0
fr = model.getLinkFrame(_leftFootLink)
fl = model.getLinkFrame(_rightFootLink)
pelvisT = model.getLinkFrame(_pelvisLink)
xaxis = [1.0, 0.0, 0.0]
pelvisT.TransformVector(xaxis, xaxis)
xaxis = np.array(xaxis)
zaxis = np.array([0.0, 0.0, 1.0])
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
numGoalSteps = 3
is_right_foot = True
self.goalSteps = []
for i in range(numGoalSteps):
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
if is_right_foot:
t.Translate(fr.GetPosition())
else:
t.Translate(fl.GetPosition())
t.Translate(xaxis*distanceForward)
distanceForward += 0.15
is_right_foot = not is_right_foot
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(t)
step.is_right_foot = is_right_foot
step.params = self.getDefaultStepParams()
self.goalSteps.append(step)
request = self.constructFootstepPlanRequest(pose)
request.num_goal_steps = len(self.goalSteps)
request.goal_steps = self.goalSteps
self.sendFootstepPlanRequest(request)
def sendPlanningRequest(self, footsteps, nextDoubleSupportPose):
goalSteps = []
#for i in range(flist_shape[0]):
for i, footstep in enumerate(footsteps):
#step_t = vtk.vtkTransform()
#step_t.PostMultiply()
#step_t.Concatenate(transformUtils.frameFromPositionAndRPY(flist[i,0:3] , flist[i,3:6]))
#step_t.Concatenate(foot_to_sole)
#step_t.Concatenate(frame_pt_to_centerline)
step_t = footstep.transform
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(step_t)
step.is_right_foot = footstep.is_right_foot # flist[i,6] # is_right_foot
step.params = self.footstepsPanel.driver.getDefaultStepParams()
# Visualization via triads
#vis.updateFrame(step_t, str(i), parent="navigation")
goalSteps.append(step)
#nextDoubleSupportPose = self.robotStateJointController.q
request = self.footstepsPanel.driver.constructFootstepPlanRequest(nextDoubleSupportPose)
request.num_goal_steps = len(goalSteps)
request.goal_steps = goalSteps
# force correct planning parameters:
request.params.leading_foot = goalSteps[0].is_right_foot
request.params.planning_mode = 1
request.params.nom_forward_step = 0.38
request.params.map_mode = 1 # 2 footplane, 0 h+n, 1 h+zup, 3 hori
request.params.max_num_steps = len(goalSteps)
request.params.min_num_steps = len(goalSteps)
lcmUtils.publish('FOOTSTEP_PLAN_REQUEST', request)
def sendPlanningRequest(self, footsteps, nextDoubleSupportPose):
goalSteps = []
#for i in range(flist_shape[0]):
for i, footstep in enumerate(footsteps):
#step_t = vtk.vtkTransform()
#step_t.PostMultiply()
#step_t.Concatenate(transformUtils.frameFromPositionAndRPY(flist[i,0:3] , flist[i,3:6]))
#step_t.Concatenate(foot_to_sole)
#step_t.Concatenate(frame_pt_to_centerline)
step_t = footstep.transform
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(step_t)
step.is_right_foot = footstep.is_right_foot # flist[i,6] # is_right_foot
step.params = self.footstepsPanel.driver.getDefaultStepParams()
# Visualization via triads
#vis.updateFrame(step_t, str(i), parent="navigation")
goalSteps.append(step)
#nextDoubleSupportPose = self.robotStateJointController.q
request = self.footstepsPanel.driver.constructFootstepPlanRequest(nextDoubleSupportPose)
request.num_goal_steps = len(goalSteps)
request.goal_steps = goalSteps
# force correct planning parameters:
request.params.leading_foot = goalSteps[0].is_right_foot
request.params.planning_mode = 1
request.params.nom_forward_step = 0.38
request.params.map_mode = 1 # 2 footplane, 0 h+n, 1 h+zup, 3 hori
request.params.max_num_steps = len(goalSteps)
request.params.min_num_steps = len(goalSteps)
lcmUtils.publish('FOOTSTEP_PLAN_REQUEST', request)
def pointPickerStoredFootsteps(self,p1, p2):
yaw = math.atan2( p2[1] - p1[1] , p2[0] - p1[0] )*180/math.pi + 90
frame_p1 = transformUtils.frameFromPositionAndRPY(p1, [0,0,yaw])
blockl = 0.3937
blockh = 0.142875
sep = 0.11
frame_pt_to_centerline = transformUtils.frameFromPositionAndRPY( [0, -blockl/2, 0], [0,0,0])
frame_pt_to_centerline.PostMultiply()
frame_pt_to_centerline.Concatenate(frame_p1)
vis.updateFrame(frame_pt_to_centerline, "corner", parent="navigation")
# up/down 1 block (Original)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.06 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*1.5 +0.03 ,-.1 , 0 , 0 , 0 , 0]])
# up/down 1 block (Newer)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.04 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*1.5 + 0.00 ,-.1 , 0 , 0 , 0 , 0]])
# up 3 blocks
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] ,
# [ blockl*2.5 + 0.0 ,-.1 , 3*blockh, 0 , 0 , 0]])
# up and down 3 blocks (original)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] ,
# [ blockl*2.5 + 0.03 ,-.1 , 3*blockh, 0 , 0 , 0],
# [ blockl*3.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*3.5 + 0.03 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*4.5 - 0.03 , .1 , 1*blockh, 0 , 0 , 0] ,
# [ blockl*4.5 + 0.03 ,-.1 , 1*blockh, 0 , 0 , 0],
# [ blockl*5.5 - 0.03 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*5.5 + 0.03 ,-.1 , 0 , 0 , 0 , 0]])
# up and down 3 blocks (used in video)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.0 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r],
# [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*5.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half
# [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner
# [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order
# [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r], # top
# [ blockl*2.5 - 0.06 , sep , 3*blockh, 0 , 0 , 0, 0], # top
# [ blockl*1.5 + 0.04 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]])
# up and down 2 blocks (used in vicon april 2014)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r], # start
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r], # 1
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r], # 2
# [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # 1d
# [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half
# [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner
# [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order
# [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]])
# up 6 blocks (used in journal paper in oct 2014)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 ,-sep , 0 , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 - 0.02 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 - 0.02 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 - 0.02 ,-sep , 3*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 4*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 - 0.02 ,-sep , 4*blockh, 0 , 0 , 0, r],
# [ blockl*4.5 - 0.03 , sep , 5*blockh, 0 , 0 , 0, 0],
# [ blockl*4.5 - 0.02 ,-sep , 5*blockh, 0 , 0 , 0, r],
# [ blockl*5.5 - 0.03 , sep , 6*blockh, 0 , 0 , 0, 0],
# [ blockl*5.5 - 0.02 ,-sep , 6*blockh, 0 , 0 , 0, r]])
# continuous walking - first two blocks up
r =1
flist = np.array( [[ blockl*-0.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0],
[ blockl*-0.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r],
[ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
[ blockl*0.5 + 0.03 ,-sep , blockh , 0 , 0 , 0, r]])
contact_pts = self.footstepDriver.getContactPts()
contact_pts_mid = np.mean(contact_pts, axis=0) # mid point on foot relative to foot frame
foot_to_sole = transformUtils.frameFromPositionAndRPY( contact_pts_mid, [0,0,0]).GetLinearInverse()
flist_shape = flist.shape
self.goalSteps = []
for i in range(flist_shape[0]):
step_t = vtk.vtkTransform()
step_t.PostMultiply()
step_t.Concatenate(transformUtils.frameFromPositionAndRPY(flist[i,0:3] , flist[i,3:6]))
step_t.Concatenate(foot_to_sole)
step_t.Concatenate(frame_pt_to_centerline)
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(step_t)
step.is_right_foot = flist[i,6] # is_right_foot
step.params = self.footstepDriver.getDefaultStepParams()
# Visualization via triads
#vis.updateFrame(step_t, str(i), parent="navigation")
self.goalSteps.append(step)
startPose = self.jointController.q
request = self.footstepDriver.constructFootstepPlanRequest(startPose)
request.num_goal_steps = len(self.goalSteps)
request.goal_steps = self.goalSteps
lcmUtils.publish('FOOTSTEP_PLAN_REQUEST', request)
def pointPickerStoredFootsteps(self, p1, p2):
yaw = math.atan2(p2[1] - p1[1], p2[0] - p1[0]) * 180 / math.pi + 90
frame_p1 = transformUtils.frameFromPositionAndRPY(p1, [0, 0, yaw])
blockl = 0.3937
blockh = 0.142875
sep = 0.11
frame_pt_to_centerline = transformUtils.frameFromPositionAndRPY(
[0, -blockl / 2, 0], [0, 0, 0])
frame_pt_to_centerline.PostMultiply()
frame_pt_to_centerline.Concatenate(frame_p1)
vis.updateFrame(frame_pt_to_centerline, "corner", parent="navigation")
# up/down 1 block (Original)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.06 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*1.5 +0.03 ,-.1 , 0 , 0 , 0 , 0]])
# up/down 1 block (Newer)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.04 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*1.5 + 0.00 ,-.1 , 0 , 0 , 0 , 0]])
# up 3 blocks
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] ,
# [ blockl*2.5 + 0.0 ,-.1 , 3*blockh, 0 , 0 , 0]])
# up and down 3 blocks (original)
# flist = np.array( [[ blockl*-0.5 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*-0.5 , -.1 , 0 , 0 , 0 , 0] ,
# [ blockl*0.5 - 0.03 , .1 , blockh , 0 , 0 , 0] ,
# [ blockl*0.5 + 0.0 ,-.1 , blockh , 0 , 0 , 0] ,
# [ blockl*1.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*1.5 + 0.0 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*2.5 - 0.03 , .1 , 3*blockh, 0 , 0 , 0] ,
# [ blockl*2.5 + 0.03 ,-.1 , 3*blockh, 0 , 0 , 0],
# [ blockl*3.5 - 0.03 , .1 , 2*blockh, 0 , 0 , 0] ,
# [ blockl*3.5 + 0.03 ,-.1 , 2*blockh, 0 , 0 , 0],
# [ blockl*4.5 - 0.03 , .1 , 1*blockh, 0 , 0 , 0] ,
# [ blockl*4.5 + 0.03 ,-.1 , 1*blockh, 0 , 0 , 0],
# [ blockl*5.5 - 0.03 , .1 , 0 , 0 , 0 , 0] ,
# [ blockl*5.5 + 0.03 ,-.1 , 0 , 0 , 0 , 0]])
# up and down 3 blocks (used in video)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.0 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r],
# [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*5.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half
# [ blockl*5.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner
# [ blockl*4.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order
# [ blockl*4.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 3*blockh, 0 , 0 , 0, r], # top
# [ blockl*2.5 - 0.06 , sep , 3*blockh, 0 , 0 , 0, 0], # top
# [ blockl*1.5 + 0.04 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]])
# up and down 2 blocks (used in vicon april 2014)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 , -sep , 0 , 0 , 0 , 0, r], # start
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.0 ,-sep , blockh , 0 , 0 , 0, r], # 1
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r], # 2
# [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # 1d
# [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*3.5 + 0.03 ,-sep , 0 , 0 , 0 , 0, r], # half
# [ blockl*3.5 - 0.03 , sep , 0 , 0 , 0 , 0, 0], # extra step for planner
# [ blockl*2.5 + 0.03 ,-sep , 1*blockh, 0 , 0 , 0, r], # invert order
# [ blockl*2.5 - 0.03 , sep , 1*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 + 0.03 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*1.5 - 0.06 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*0.5 + 0.04 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.06 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*-0.5+ 0.04 , -sep , 0 , 0 , 0 , 0, r],
# [ blockl*-0.5 - 0.03, sep , 0 , 0 , 0 , 0, 0]])
# up 6 blocks (used in journal paper in oct 2014)
# r =1
# flist = np.array( [[ blockl*-0.5 , sep , 0 , 0 , 0 , 0, 0],
# [ blockl*-0.5 ,-sep , 0 , 0 , 0 , 0, r],
# [ blockl*0.5 - 0.03 , sep , blockh , 0 , 0 , 0, 0],
# [ blockl*0.5 - 0.02 ,-sep , blockh , 0 , 0 , 0, r],
# [ blockl*1.5 - 0.03 , sep , 2*blockh, 0 , 0 , 0, 0],
# [ blockl*1.5 - 0.02 ,-sep , 2*blockh, 0 , 0 , 0, r],
# [ blockl*2.5 - 0.03 , sep , 3*blockh, 0 , 0 , 0, 0],
# [ blockl*2.5 - 0.02 ,-sep , 3*blockh, 0 , 0 , 0, r],
# [ blockl*3.5 - 0.03 , sep , 4*blockh, 0 , 0 , 0, 0],
# [ blockl*3.5 - 0.02 ,-sep , 4*blockh, 0 , 0 , 0, r],
# [ blockl*4.5 - 0.03 , sep , 5*blockh, 0 , 0 , 0, 0],
# [ blockl*4.5 - 0.02 ,-sep , 5*blockh, 0 , 0 , 0, r],
# [ blockl*5.5 - 0.03 , sep , 6*blockh, 0 , 0 , 0, 0],
# [ blockl*5.5 - 0.02 ,-sep , 6*blockh, 0 , 0 , 0, r]])
# continuous walking - first two blocks up
r = 1
flist = np.array([[blockl * -0.5 - 0.03, sep, 0, 0, 0, 0, 0],
[blockl * -0.5 + 0.03, -sep, 0, 0, 0, 0, r],
[blockl * 0.5 - 0.03, sep, blockh, 0, 0, 0, 0],
[blockl * 0.5 + 0.03, -sep, blockh, 0, 0, 0, r]])
contact_pts = self.footstepDriver.getContactPts()
contact_pts_mid = np.mean(
contact_pts, axis=0) # mid point on foot relative to foot frame
foot_to_sole = transformUtils.frameFromPositionAndRPY(
contact_pts_mid, [0, 0, 0]).GetLinearInverse()
flist_shape = flist.shape
self.goalSteps = []
for i in range(flist_shape[0]):
step_t = vtk.vtkTransform()
step_t.PostMultiply()
step_t.Concatenate(
transformUtils.frameFromPositionAndRPY(flist[i, 0:3],
flist[i, 3:6]))
step_t.Concatenate(foot_to_sole)
step_t.Concatenate(frame_pt_to_centerline)
step = lcmdrc.footstep_t()
step.pos = transformUtils.positionMessageFromFrame(step_t)
step.is_right_foot = flist[i, 6] # is_right_foot
step.params = self.footstepDriver.getDefaultStepParams()
# Visualization via triads
#vis.updateFrame(step_t, str(i), parent="navigation")
self.goalSteps.append(step)
startPose = self.jointController.q
request = self.footstepDriver.constructFootstepPlanRequest(startPose)
request.num_goal_steps = len(self.goalSteps)
request.goal_steps = self.goalSteps
lcmUtils.publish('FOOTSTEP_PLAN_REQUEST', request)
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())
