def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty('Name')
if name in ('footstep widget', 'footstep widget frame'):
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
return True
match = re.match('^step (\d+)$', name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName('footstep widget')
if existingWidget:
previousStep = existingWidget.stepIndex
print 'have existing widget for step:', stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
print 'returning because widget was for selected step'
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty('Color', obj.getProperty('Color'))
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
def onFootWidgetChanged(frame):
footstepsDriver.onStepModified(stepIndex - 1, frame)
frameObj.connectFrameModified(onFootWidgetChanged)
return True
def onFrameModified(self, frame):
pos, rpy = self.frame.transform.GetPosition(
), transformUtils.rollPitchYawFromTransform(self.frame.transform)
q = self.jointController.q.copy()
q[:3] = pos
q[3:6] = rpy
self.jointController.setPose('moved_pose', q)
def onRobotStateModelChanged(self, model=None):
if self.lfootFrame is None:
return
newPelvisToWorld = self.getPelvisEstimate()
q = robotStateJointController.q.copy()
q[0:3] = newPelvisToWorld.GetPosition()
q[3:6] = transformUtils.rollPitchYawFromTransform(newPelvisToWorld)
playbackJointController.setPose('lock_pose', q)
def onRobotStateModelChanged(self, model=None):
if self.lfootFrame is None:
return
newPelvisToWorld = self.getPelvisEstimate()
q = robotStateJointController.q.copy()
q[0:3] = newPelvisToWorld.GetPosition()
q[3:6] = transformUtils.rollPitchYawFromTransform(newPelvisToWorld)
playbackJointController.setPose('lock_pose', q)
def testEuler():
'''
Test some euler conversions
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
mat = transformUtils.getNumpyFromTransform(frame)
rpy = transformUtils.rollPitchYawFromTransform(frame)
rpy2 = transformations.euler_from_matrix(mat)
print rpy
print rpy2
assert np.allclose(rpy, rpy2)
def getFeetMidPoint(model, useWorldZ=True):
'''
Returns a frame in world coordinate system that is the average of the left
and right foot reference point positions in world frame, the average of the
left and right foot yaw in world frame, and Z axis aligned with world Z.
The foot reference point is the average of the foot contact points in the foot frame.
'''
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
contact_pts_mid_left = np.mean(contact_pts_left, axis=0) # mid point on foot relative to foot frame
contact_pts_mid_right = np.mean(contact_pts_right, axis=0) # mid point on foot relative to foot frame
t_lf_mid = model.getLinkFrame(_leftFootLink)
t_lf_mid.PreMultiply()
t_lf_mid.Translate(contact_pts_mid_left)
t_rf_mid = model.getLinkFrame(_rightFootLink)
t_rf_mid.PreMultiply()
t_rf_mid.Translate(contact_pts_mid_right)
if (_robotType == 0): # Atlas
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
elif (_robotType == 1):
# Valkyrie v1 Foot orientation is silly
l_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [180.0, 82.5, 0])
l_foot_sole.PostMultiply()
l_foot_sole.Concatenate(t_lf_mid)
r_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [0.0, -82.5, 0])
r_foot_sole.PostMultiply()
r_foot_sole.Concatenate(t_rf_mid)
t_feet_mid = transformUtils.frameInterpolate(l_foot_sole, r_foot_sole, 0.5)
elif (_robotType == 2):
# Valkyrie v2 is better
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
if useWorldZ:
rpy = [0.0, 0.0, np.degrees(transformUtils.rollPitchYawFromTransform(t_feet_mid)[2])]
return transformUtils.frameFromPositionAndRPY(t_feet_mid.GetPosition(), rpy)
else:
return t_feet_mid
def getFeetMidPoint(model, useWorldZ=True):
'''
Returns a frame in world coordinate system that is the average of the left
and right foot reference point positions in world frame, the average of the
left and right foot yaw in world frame, and Z axis aligned with world Z.
The foot reference point is the average of the foot contact points in the foot frame.
'''
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
contact_pts_mid_left = np.mean(contact_pts_left, axis=0) # mid point on foot relative to foot frame
contact_pts_mid_right = np.mean(contact_pts_right, axis=0) # mid point on foot relative to foot frame
t_lf_mid = model.getLinkFrame(_leftFootLink)
t_lf_mid.PreMultiply()
t_lf_mid.Translate(contact_pts_mid_left)
t_rf_mid = model.getLinkFrame(_rightFootLink)
t_rf_mid.PreMultiply()
t_rf_mid.Translate(contact_pts_mid_right)
if (_robotType == 0): # Atlas
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
elif (_robotType == 1):
# Valkyrie v1 Foot orientation is silly
l_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [180.0, 82.5, 0])
l_foot_sole.PostMultiply()
l_foot_sole.Concatenate(t_lf_mid)
r_foot_sole = transformUtils.frameFromPositionAndRPY([0,0,0], [0.0, -82.5, 0])
r_foot_sole.PostMultiply()
r_foot_sole.Concatenate(t_rf_mid)
t_feet_mid = transformUtils.frameInterpolate(l_foot_sole, r_foot_sole, 0.5)
elif (_robotType == 2):
# Valkyrie v2 is better
t_feet_mid = transformUtils.frameInterpolate(t_lf_mid, t_rf_mid, 0.5)
if useWorldZ:
rpy = [0.0, 0.0, np.degrees(transformUtils.rollPitchYawFromTransform(t_feet_mid)[2])]
return transformUtils.frameFromPositionAndRPY(t_feet_mid.GetPosition(), rpy)
else:
return t_feet_mid
def testEulerBotpy():
'''
Test some quaternion and euler conversions with botpy
'''
quat = transformations.random_quaternion()
rpy = transformations.euler_from_quaternion(quat)
rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
quat2 = botpy.roll_pitch_yaw_to_quat(rpy)
mat = transformations.quaternion_matrix(quat)
frame = transformUtils.getTransformFromNumpy(mat)
rpy3 = transformUtils.rollPitchYawFromTransform(frame)
print quat, quat2
print rpy, rpy2, rpy3
assert isQuatEqual(quat, quat2)
assert np.allclose(rpy, rpy2)
assert np.allclose(rpy2, rpy3)
def poseFromAffordance(aff):
t = aff.getChildFrame().transform
position, quat = transformUtils.poseFromTransform(t)
rpy = transformUtils.rollPitchYawFromTransform(t)
return urdf.Pose(position, rpy)
def onFrameModified(self, frame):
pos, rpy = self.frame.transform.GetPosition(), transformUtils.rollPitchYawFromTransform(self.frame.transform)
q = self.jointController.q.copy()
q[:3] = pos
q[3:6] = rpy
self.jointController.setPose('moved_pose', q)
def poseFromAffordance(aff):
t = aff.getChildFrame().transform
position, quat = transformUtils.poseFromTransform(t)
rpy = transformUtils.rollPitchYawFromTransform(t)
return urdf.Pose(position, rpy)
