def monitor():
global stepCounter, robotpose, tfBuffer, tfListener, foot_frame
global ROBOT_NAME, RIGHT_FOOT_FRAME_NAME, LEFT_FOOT_FRAME_NAME
global start_line_crossed, finish_line_crossed
global fig1, ax1, pltdata
time = rospy.Time.now()
foot_frame = LEFT_FOOT_FRAME_NAME
footWorld = tfBuffer.lookup_transform('world', foot_frame, rospy.Time())
footstep = FootstepDataRosMessage()
footstep.orientation = footWorld.transform.rotation
footstep.location = footWorld.transform.translation
f_x = footstep.location.x
r_t = robotpose.header.stamp.secs
r_x = robotpose.pose.pose.position.x
if (r_x >= 0.5 and not start_line_crossed):
start_line_crossed = time
elif (r_x >= 4.5 and not finish_line_crossed):
finish_line_crossed = time
os.system('clear')
print("time: %6.3f steps: %2d robot_x: %6.3f l_foot_x: %6.3f" %
(time.to_sec(), stepCounter, r_x, f_x))
if (start_line_crossed):
print(" Started: %6.3f" % (start_line_crossed.to_sec()))
if (finish_line_crossed):
print("Finished: %6.3f" % (finish_line_crossed.to_sec()))
print(" Elapsed: %6.3f" %
((finish_line_crossed - start_line_crossed).to_sec()))
def make_footstep(reference_pose, side, offset_posn, offset_quat):
step = FootstepDataRosMessage()
step.robot_side = side
old_world_to_point = numpy.dot(transformations.translation_matrix(offset_posn),
transformations.quaternion_matrix(offset_quat))
pelvis_to_old_world = numpy.dot(transformations.translation_matrix(reference_pose[0]),
transformations.quaternion_matrix(reference_pose[1]))
pelvis_tf_msg = tfBuffer.lookup_transform('world', 'pelvis', rospy.Time())
q = pelvis_tf_msg.transform.rotation
t = pelvis_tf_msg.transform.translation
new_world_to_pelvis = numpy.dot(transformations.translation_matrix((t.x, t.y, t.z)),
transformations.quaternion_matrix((q.x, q.y, q.z, q.w)))
new_world_to_point = new_world_to_pelvis.dot(pelvis_to_old_world).dot(old_world_to_point)
step.location.x, step.location.y, step.location.z = transformations.translation_from_matrix(new_world_to_point)
step.orientation.x, step.orientation.y, step.orientation.z, step.orientation.w = \
transformations.quaternion_from_matrix(new_world_to_point)
foot_COP_tf_msg = tfBuffer.lookup_transform('world', 'leftCOP_Frame', rospy.Time())
# Ensure z is always at foot height
step.location.z = foot_COP_tf_msg.transform.translation.z
return step
def createFootStepInPlace(self, step_side):
footstep = FootstepDataRosMessage()
footstep.robot_side = step_side
if step_side == FootstepDataRosMessage.LEFT:
foot_frame = self.LEFT_FOOT_FRAME_NAME
else:
foot_frame = self.RIGHT_FOOT_FRAME_NAME
footWorld = self.tfBuffer.lookup_transform('world', foot_frame,
rospy.Time())
#Get Pelvis orientation as we want to be with respect to the pelvis
pelvis_world = self.tfBuffer.lookup_transform('world',
self.PELVIS_FRAME_NAME,
rospy.Time())
quat_pelvis_world = pelvis_world.transform.rotation
quat_pelvis = numpy.array([
quat_pelvis_world.w, quat_pelvis_world.x, quat_pelvis_world.y,
quat_pelvis_world.z
])
quat_to_use = self.get_pelvis_xy_coplanar_quat(quat_pelvis)
quat = Quaternion()
quat.w = quat_to_use[0]
quat.x = quat_to_use[1]
quat.y = quat_to_use[2]
quat.z = quat_to_use[3]
footstep.orientation = quat #footWorld.transform.rotation
footstep.location = footWorld.transform.translation
return footstep
def getFootFootstepMsg(self, foot):
#msg = self.getEmptyFootsetListMsg()
msg = FootstepDataListRosMessage()
msg.default_transfer_time = 0.3 #was transfer_time 1.5
msg.default_swing_time = 0.7 #was swing_time 1.5
msg.execution_mode = 0
msg.unique_id = -1
ROSleft = FootstepDataRosMessage.LEFT
ROSright = FootstepDataRosMessage.RIGHT
footstep = FootstepDataRosMessage()
step_side = ROSleft if foot.side == Foot.LEFT else ROSright
footstep.robot_side = step_side
#loc,rot = getROSloc(side)
rot = quaternion_from_euler(0, 0, foot.t)
footstep.orientation.x = rot[0]
footstep.orientation.y = rot[1]
footstep.orientation.z = rot[2]
footstep.orientation.w = rot[3]
footstep.location.x = foot.x
footstep.location.y = foot.y
footstep.location.z = self.recentz
msg.footstep_data_list.append(footstep)
return msg
def createFootStepInPlace(stepSide):
footstep = FootstepDataRosMessage()
footstep.robot_side = stepSide
if stepSide == LEFT:
foot_frame = LEFT_FOOT_FRAME_NAME
else:
foot_frame = RIGHT_FOOT_FRAME_NAME
footWorld = tfBuffer.lookup_transform('world', foot_frame, rospy.Time())
footstep.orientation = footWorld.transform.rotation
footstep.location = footWorld.transform.translation
return footstep
def create_one_footstep(side, offset):
global lf_start_position
global lf_start_orientation
global rf_start_position
global rf_start_orientation
footstep = FootstepDataRosMessage()
footstep.robot_side = side
if side == LEFT:
footstep.orientation = deepcopy(lf_start_orientation)
footstep.location = deepcopy(lf_start_position)
else:
footstep.orientation = deepcopy(rf_start_orientation)
footstep.location = deepcopy(rf_start_position)
# JPW There are further parameters on feet that we
# should be able to play with. This is supposedly the 'fast'
# trajectory
footstep.trajectory_type = footstep.PUSH_RECOVERY
footstep.location.x += offset[0]
footstep.location.y += offset[1]
footstep.location.z += offset[2]
return footstep
def createFootStepInPlace(stepSide):
footstep = FootstepDataRosMessage()
footstep.robot_side = stepSide
if stepSide == LEFT:
foot_frame = LEFT_FOOT_FRAME_NAME
else:
foot_frame = RIGHT_FOOT_FRAME_NAME
footWorld = tfBuffer.lookup_transform('world', foot_frame, rospy.Time())
footstep.orientation = footWorld.transform.rotation
footstep.location = footWorld.transform.translation
return footstep
def setFeet(both_x=0.15, right_y=-0.12, foot_separation=0.25):
#both_x = 0.02, right_y=-0.12, foot_separation=0.25
rospy.loginfo('Setting feet to fixed position.')
right_footstep = FootstepDataRosMessage()
right_footstep.robot_side = RIGHT
right_footstep.orientation = FOOT_ORIENTATION
right_footstep.location = Vector3(x=both_x, y=right_y, z=0)
left_footstep = FootstepDataRosMessage()
left_footstep.robot_side = LEFT
left_footstep.orientation = FOOT_ORIENTATION
left_footstep.location = Vector3(x=both_x,
y=right_y + foot_separation,
z=0)
right_current = createFootStepInPlace(RIGHT)
if right_current.location.y > right_y:
first_footstep = right_footstep
second_footstep = left_footstep
else:
first_footstep = left_footstep
second_footstep = right_footstep
msg = FootstepDataListRosMessage()
# ----- Default Value: 1.5
msg.transfer_time = 1.5
msg.swing_time = 1.5
msg.execution_mode = FootstepDataListRosMessage.OVERRIDE
msg.unique_id = rospy.Time.now().nsecs
msg.footstep_data_list.append(first_footstep)
footStepListPublisher.publish(msg)
waitForFootstepCompletion()
msg.footstep_data_list[:] = []
msg.unique_id = rospy.Time.now().nsecs
msg.footstep_data_list.append(second_footstep)
footStepListPublisher.publish(msg)
waitForFootstepCompletion()
msg.footstep_data_list[:] = []
return left_footstep, right_footstep
def create_one_abs_footstep(self, side, location, orientation):
""" Simple create of a footstep to move a foot to a precise location."""
footstep = FootstepDataRosMessage()
footstep.robot_side = side
footstep.location = location
footstep.orientation = orientation
footstep.trajectory_type = FootstepDataRosMessage.DEFAULT
# Create report of what we are doing relative to left foot.
cur_pos = self.lf_start_position
cur_angle = compute_rot_angle(self.lf_start_orientation)
cur_pos_diff = [location.x - cur_pos.x, location.y - cur_pos.y, 0]
lf_rot_pos_diff = self.undo_transform_offset(cur_pos_diff)
new_angle = compute_rot_angle(orientation)
if side == FootstepDataRosMessage.LEFT: footstep_type = "left"
else: footstep_type = "right"
log("Moving {} foot relative to (starting) left foot to position {} and angle {}."
.format(footstep_type, fmt(lf_rot_pos_diff),
fmt(new_angle - cur_angle)))
return footstep
def create_one_footstep(self, side, offset, angle=None, world=False):
""" Create one footstep. """
footstep = FootstepDataRosMessage()
footstep.robot_side = side
up = offset[2]
intermediate_pt = None
world_intermediate = None
world_intermediate2 = None
# if up > 0.05 and side == FootstepDataRosMessage.RIGHT and self.USE_RIGHT_STEP_UP_TRAJECTORY:
if up > 0.05 and self.USE_STEP_UP_TRAJECTORY:
# Note, how we use 0.06 or 0.03 forward, which would intuitively seem wrong, but it produces the desired
# effect.
xoffset = 0.06 if side == FootstepDataRosMessage.LEFT else 0.03
intermediate_pt = [xoffset, 0, up + 0.05]
if side == FootstepDataRosMessage.LEFT:
footstep_type = "left"
footstep.orientation = deepcopy(self.lf_start_orientation)
footstep.location = deepcopy(self.lf_start_position)
if not world:
world_offset = self.transform_left_offset(offset)
if intermediate_pt is not None:
log("Using waypoints to move left foot to avoid stair")
lf_start_v = msg_v_to_v(self.lf_start_position)
world_intermediate = add_v(
lf_start_v,
self.transform_left_offset(intermediate_pt))
world_intermediate2 = add_v(lf_start_v, world_offset)
else:
world_offset = offset
else:
footstep_type = "right"
footstep.orientation = deepcopy(self.rf_start_orientation)
footstep.location = deepcopy(self.rf_start_position)
if not world:
world_offset = self.transform_right_offset(offset)
if intermediate_pt is not None:
log("Using waypoints to move right foot to avoid stair")
rf_start_v = msg_v_to_v(self.rf_start_position)
world_intermediate = add_v(
rf_start_v,
self.transform_right_offset(intermediate_pt))
world_intermediate2 = add_v(rf_start_v, world_offset)
else:
world_offset = offset
if world_intermediate is None:
footstep.trajectory_type = FootstepDataRosMessage.DEFAULT
else:
footstep.trajectory_type = FootstepDataRosMessage.CUSTOM
footstep.trajectory_waypoints = [
v_to_msg_v(world_intermediate),
v_to_msg_v(world_intermediate2)
]
reference_foot_angle = compute_rot_angle(self.lf_start_orientation)
if not world:
log("Preparing " + footstep_type + " foot (left foot rotated=" +
fmt(reference_foot_angle) + "), moving " + fmt(offset))
else:
log("Preparing " + footstep_type + " foot, move (world) " +
fmt(offset))
footstep.location.x += world_offset[0]
footstep.location.y += world_offset[1]
footstep.location.z += world_offset[2]
if angle is not None:
# Rotate about z-axis.
rotate = quaternion_about_axis(radians(angle), [0, 0, 1])
original = [
footstep.orientation.x, footstep.orientation.y,
footstep.orientation.z, footstep.orientation.w
]
new = quaternion_multiply(rotate, original)
log("Rotating " + footstep_type + " foot by " + fmt(angle))
footstep.orientation = Quaternion(new[0], new[1], new[2], new[3])
return footstep
def createRotationFootStepList(self, yaw):
left_footstep = FootstepDataRosMessage()
left_footstep.robot_side = self.LEFT_FOOT
right_footstep = FootstepDataRosMessage()
right_footstep.robot_side = self.RIGHT_FOOT
left_foot_world = self.tfBuffer.lookup_transform(
'world', self.LEFT_FOOT_FRAME_NAME, rospy.Time())
right_foot_world = self.tfBuffer.lookup_transform(
'world', self.RIGHT_FOOT_FRAME_NAME, rospy.Time())
intermediate_transform = Transform()
# create a virtual fram between the feet, this will be the center of the rotation
intermediate_transform.translation.x = (
left_foot_world.transform.translation.x + right_foot_world.transform.translation.x)/2.
intermediate_transform.translation.y = (
left_foot_world.transform.translation.y + right_foot_world.transform.translation.y)/2.
intermediate_transform.translation.z = (
left_foot_world.transform.translation.z + right_foot_world.transform.translation.z)/2.
# here we assume that feet have the same orientation so we can pick arbitrary left or right
intermediate_transform.rotation = left_foot_world.transform.rotation
left_footstep.location = left_foot_world.transform.translation
right_footstep.location = right_foot_world.transform.translation
# define the turning radius
radius = sqrt(
(
right_foot_world.transform.translation.x -
left_foot_world.transform.translation.x
)**2 + (
right_foot_world.transform.translation.y -
left_foot_world.transform.translation.y
)**2) / 2.
left_offset = [-radius*sin(yaw), radius*(1-cos(yaw)), 0]
right_offset = [radius*sin(yaw), -radius*(1-cos(yaw)), 0]
intermediate_euler = euler_from_quaternion([
intermediate_transform.rotation.x,
intermediate_transform.rotation.y,
intermediate_transform.rotation.z,
intermediate_transform.rotation.w])
resulting_quat = quaternion_from_euler(
intermediate_euler[0], intermediate_euler[1],
intermediate_euler[2] + yaw)
rot = quaternion_matrix([
resulting_quat[0], resulting_quat[1], resulting_quat[2], resulting_quat[3]])
left_transformedOffset = numpy.dot(rot[0:3, 0:3], left_offset)
right_transformedOffset = numpy.dot(rot[0:3, 0:3], right_offset)
quat_final = Quaternion(
resulting_quat[0], resulting_quat[1], resulting_quat[2], resulting_quat[3])
left_footstep.location.x += left_transformedOffset[0]
left_footstep.location.y += left_transformedOffset[1]
left_footstep.location.z += left_transformedOffset[2]
left_footstep.orientation = quat_final
right_footstep.location.x += right_transformedOffset[0]
right_footstep.location.y += right_transformedOffset[1]
right_footstep.location.z += right_transformedOffset[2]
right_footstep.orientation = quat_final
if yaw > 0:
return [left_footstep, right_footstep]
else:
return [right_footstep, left_footstep]
def createRotationFootStepList(self, yaw):
left_footstep = FootstepDataRosMessage()
left_footstep.robot_side = FootstepDataRosMessage.LEFT
right_footstep = FootstepDataRosMessage()
right_footstep.robot_side = FootstepDataRosMessage.RIGHT
left_foot_world = self.tfBuffer.lookup_transform(
'world', self.LEFT_FOOT_FRAME_NAME, rospy.Time())
right_foot_world = self.tfBuffer.lookup_transform(
'world', self.RIGHT_FOOT_FRAME_NAME, rospy.Time())
intermediate_transform = Transform()
# create a virtual fram between the feet, this will be the center of the rotation
intermediate_transform.translation.x = (
left_foot_world.transform.translation.x +
right_foot_world.transform.translation.x) / 2.
intermediate_transform.translation.y = (
left_foot_world.transform.translation.y +
right_foot_world.transform.translation.y) / 2.
intermediate_transform.translation.z = (
left_foot_world.transform.translation.z +
right_foot_world.transform.translation.z) / 2.
# here we assume that feet have the same orientation so we can pick arbitrary left or right
#intermediate_transform.rotation = left_foot_world.transform.rotation # Get Pelvis orientation
# We will use the pelvis's rotation as the initial frame of reference
pelvis_world = self.tfBuffer.lookup_transform('world',
self.PELVIS_FRAME_NAME,
rospy.Time())
quat_pelvis_world = pelvis_world.transform.rotation
quat_pelvis = numpy.array([
quat_pelvis_world.w, quat_pelvis_world.x, quat_pelvis_world.y,
quat_pelvis_world.z
])
quat_to_use = self.get_pelvis_xy_coplanar_quat(quat_pelvis)
quat = Quaternion()
quat.w = quat_to_use[0]
quat.x = quat_to_use[1]
quat.y = quat_to_use[2]
quat.z = quat_to_use[3]
intermediate_transform.rotation = quat
left_footstep.location = left_foot_world.transform.translation
right_footstep.location = right_foot_world.transform.translation
# define the turning radius
radius = sqrt((right_foot_world.transform.translation.x -
left_foot_world.transform.translation.x)**2 +
(right_foot_world.transform.translation.y -
left_foot_world.transform.translation.y)**2) / 2.
left_offset = [-radius * sin(yaw), radius * (1 - cos(yaw)), 0]
right_offset = [radius * sin(yaw), -radius * (1 - cos(yaw)), 0]
intermediate_euler = euler_from_quaternion([
intermediate_transform.rotation.x,
intermediate_transform.rotation.y,
intermediate_transform.rotation.z,
intermediate_transform.rotation.w
])
resulting_quat = quaternion_from_euler(intermediate_euler[0],
intermediate_euler[1],
intermediate_euler[2] + yaw)
rot = quaternion_matrix([
resulting_quat[0], resulting_quat[1], resulting_quat[2],
resulting_quat[3]
])
left_transformedOffset = numpy.dot(rot[0:3, 0:3], left_offset)
right_transformedOffset = numpy.dot(rot[0:3, 0:3], right_offset)
quat_final = Quaternion(resulting_quat[0], resulting_quat[1],
resulting_quat[2], resulting_quat[3])
left_footstep.location.x += left_transformedOffset[0]
left_footstep.location.y += left_transformedOffset[1]
if ON_REAL_ROBOT_USE:
left_footstep.location.z = 0.0
else:
left_footstep.location.z = -SOLE_FRAME_Z_OFFSET #+= left_transformedOffset[2]
left_footstep.orientation = quat_final
right_footstep.location.x += right_transformedOffset[0]
right_footstep.location.y += right_transformedOffset[1]
if ON_REAL_ROBOT_USE:
right_footstep.location.z = 0.0
else:
right_footstep.location.z = -SOLE_FRAME_Z_OFFSET #+= right_transformedOffset[2]
right_footstep.orientation = quat_final
if yaw > 0:
return [left_footstep, right_footstep]
else:
return [right_footstep, left_footstep]
def create_setup_footsteps(adjust_x=0.0, left_y=-0.08, stance_width=0.18):
footsteps = []
rightstep = FootstepDataRosMessage()
rightstep.robot_side = RIGHT
rightstep.orientation = deepcopy(rf_start_orientation)
rightstep.location = deepcopy(rf_start_position)
rightstep.trajectory_type = rightstep.PUSH_RECOVERY
rightstep.origin = rightstep.AT_SOLE_FRAME
rightstep.location.x = adjust_x
rightstep.location.y = left_y - stance_width
zarj_tf.transform_from_world(rightstep.location)
footsteps.append(rightstep)
leftstep = FootstepDataRosMessage()
leftstep.robot_side = LEFT
leftstep.orientation = deepcopy(lf_start_orientation)
leftstep.location = deepcopy(lf_start_position)
leftstep.trajectory_type = leftstep.PUSH_RECOVERY
leftstep.origin = leftstep.AT_SOLE_FRAME
leftstep.location.x = adjust_x
leftstep.location.y = left_y
zarj_tf.transform_from_world(leftstep.location)
footsteps.append(leftstep)
return footsteps
