def transformPlanToBDIFrame(self, plan):
if (self.pose_bdi is None):
# print "haven't received POSE_BDI"
return
# TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame
# instead of using FK here
t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation)
t_bodybdi.PostMultiply()
current_pose = self.jointController.q
t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , transformUtils.rollPitchYawToQuaternion(current_pose[3:6]) )
t_bodymain.PostMultiply()
# iterate and transform
self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy
for i, footstep in enumerate(self.bdi_plan.footsteps):
step = footstep.pos
t_step = frameFromPositionMessage(step)
t_body_to_step = vtk.vtkTransform()
t_body_to_step.DeepCopy(t_step)
t_body_to_step.PostMultiply()
t_body_to_step.Concatenate(t_bodymain.GetLinearInverse())
t_stepbdi = vtk.vtkTransform()
t_stepbdi.DeepCopy(t_body_to_step)
t_stepbdi.PostMultiply()
t_stepbdi.Concatenate(t_bodybdi)
footstep.pos = positionMessageFromFrame(t_stepbdi)
if (self.showBDIPlan is True):
self.drawBDIFootstepPlan()
def requestIRISRegion(self, tform, uid, bounding_box_width=2):
start = np.asarray(tform.GetPosition())
A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width)
msg = lcmdrc.iris_region_request_t()
msg.utime = getUtime()
msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
msg.num_seed_poses = 1
msg.seed_poses = [positionMessageFromFrame(tform)]
msg.region_id = [uid]
msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
lcmUtils.publish(self.request_channel, msg)
def requestIRISRegion(self, tform, uid, bounding_box_width=2):
start = np.asarray(tform.GetPosition())
A_bounds, b_bounds = self.getXYBounds(start, bounding_box_width)
msg = lcmdrc.iris_region_request_t()
msg.utime = getUtime()
msg.initial_state = robotstate.drakePoseToRobotState(self.jointController.q)
msg.map_mode = self.map_mode_map[self.params.properties.map_mode]
msg.num_seed_poses = 1
msg.seed_poses = [positionMessageFromFrame(tform)]
msg.region_id = [uid]
msg.xy_bounds = [irisUtils.encodeLinCon(A_bounds, b_bounds)]
lcmUtils.publish(self.request_channel, msg)
def constructFootstepPlanRequest(self, pose, goalFrame=None):
msg = lcmdrc.footstep_plan_request_t()
msg.utime = getUtime()
state_msg = robotstate.drakePoseToRobotState(pose)
msg.initial_state = state_msg
if goalFrame is None:
goalFrame = vtk.vtkTransform()
msg.goal_pos = positionMessageFromFrame(goalFrame)
msg = self.applyParams(msg)
msg = self.applySafeRegions(msg)
return msg
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 onStepModified(self, ndx, frameObj):
self.lastFootstepPlan.footsteps[ndx+2].pos = positionMessageFromFrame(frameObj.transform)
self.lastFootstepPlan.footsteps[ndx+2].fixed_x = True
self.lastFootstepPlan.footsteps[ndx+2].fixed_y = True
self.lastFootstepPlan.footsteps[ndx+2].fixed_yaw = True
self.sendUpdatePlanRequest()
def to_iris_region_t(self):
msg = lcmdrc.iris_region_t()
msg.lin_con = self.to_lin_con_t()
msg.seed_pose = positionMessageFromFrame(self.tform)
return msg
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 = 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 to_iris_region_t(self):
msg = lcmdrc.iris_region_t()
msg.lin_con = self.to_lin_con_t()
msg.seed_pose = positionMessageFromFrame(self.tform)
return msg
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 = 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)
