def test_walk_4(self):
self.walker.setPose(Transformation([0.3275415, 0.2841, 0.321], [0.04060593, 0.0120126, 0.86708929, -0.4963497]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([-0.12015226, -0.19813691, 0.321], [0, 0, 0.95993011, -0.28023953]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def bezierPositionAtRatio(self, start_transform, end_transform, r):
# If the distance is small, use turn in place strategy, otherwise cubic bezier
p1 = start_transform
if self.isWalkingBackwards():
p2 = np.matmul(start_transform, Transformation([- PathSection.speed * PathSectionBezier.turn_duration, 0., 0.]))
p3 = np.matmul(end_transform, Transformation([PathSection.speed * PathSectionBezier.turn_duration, 0., 0.]))
else:
p2 = np.matmul(start_transform, Transformation([PathSection.speed * PathSectionBezier.turn_duration, 0., 0.]))
p3 = np.matmul(end_transform, Transformation([-PathSection.speed * PathSectionBezier.turn_duration, 0., 0.]))
p4 = end_transform
p1_pos = p1.get_position()
p2_pos = p2.get_position()
p3_pos = p3.get_position()
p4_pos = p4.get_position()
position = np.array([0.0, 0.0, 0.0])
for d in range(0, 3):
bez_param = [p1_pos[d], p2_pos[d], p3_pos[d], p4_pos[d]]
# Cubic bezier
for i in range(0, 4):
position[d] = position[d] + bez_param[i] * comb(3, i, exact=True, repetition=False) * (
(1 - r) ** (3 - i)) * (r ** i)
return Transformation(position)
def test_small_movement_4(self):
self.walker.setPose(Transformation([0.2489, -0.163, 0.0], [0.0284, -0.003, 0.9939, 0.01986]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([0.0503, 0.06323, 0], [0, 0, 1, 0]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_walk_6(self):
self.walker.setPose(Transformation([2.008, -0.646, 0.0], [0.0149, -0.0474, 0.99985, -0.0072]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([0.00736, 0.0356, 0.0], [0, 0, 0.998, 0.0176]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_walk_2(self):
self.walker.setPose(Transformation([-0.7384, -0.008, 0], [0.00000, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([0.0198, -0.0199, 0], [0.00000, 0, 0, 1]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_walk_side(self):
self.walker.setPose(Transformation([0, 0, 0], [0.00000, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([0, -1, 0], [0.00000, 0, 0, 1]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_walk_7(self):
self.walker.setPose(Transformation([2.082603318747387, 0.04499586647232634, 0.0], [0.07888602209666294, -0.03018659995378454, 0.9054426772657052, 0.41597995490997813]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([2.5901226468203067, 0.7938447967981127, 0.0], [0, 0, -0.9987013856398979, 0.050946465244882694]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def get_foot_pressure_sensors(self, floor):
"""
Checks if 4 corners of the each feet are in contact with ground
Indicies for looking from above on the feet plates:
Left Right
4-------5 0-------1
| ^ | | ^ | ^
| | | | | | | : forward direction
| | | |
6-------7 2-------3
:param floor: PyBullet body id of the plane the robot is walking on.
:return: boolean array of 8 contact points on both feet, True: that point is touching the ground False: otherwise
"""
locations = [False] * 8
right_pts = pb.getContactPoints(bodyA=self.body, bodyB=floor, linkIndexA=Links.RIGHT_LEG_6)
left_pts = pb.getContactPoints(bodyA=self.body, bodyB=floor, linkIndexA=Links.LEFT_LEG_6)
right_center = np.array(pb.getLinkState(self.body, linkIndex=Links.RIGHT_LEG_6)[4])
left_center = np.array(pb.getLinkState(self.body, linkIndex=Links.LEFT_LEG_6)[4])
right_tr = tr.get_rotation_matrix_from_transformation(
tr(quaternion=pb.getLinkState(self.body, linkIndex=Links.RIGHT_LEG_6)[5]))
left_tr = tr.get_rotation_matrix_from_transformation(
tr(quaternion=pb.getLinkState(self.body, linkIndex=Links.LEFT_LEG_6)[5]))
for point in right_pts:
index = np.signbit(np.matmul(right_tr, point[5] - right_center))[0:2]
locations[index[1] + index[0] * 2] = True
for point in left_pts:
index = np.signbit(np.matmul(left_tr, point[5] - left_center))[0:2]
locations[index[1] + (index[0] * 2) + 4] = True
return locations
def test_calculate_accuracy_matrix(self):
x_range = np.arange(-1, 1, 0.2)
y_range = np.arange(-1, 1, 0.2)
ang_range = np.arange(-np.pi, np.pi, np.pi/6)
x, y, ang = np.meshgrid(x_range, y_range, ang_range)
fake_localization_pose_subscriber = rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped, self.amcl_pose_callback)
for i in x_range:
for j in y_range:
for k in ang_range:
b = String()
b.data = ""
self.resetPublisher.publish(b)
self.walker.setPose(Transformation([0, 0, 0], [0, 0, 0, 1]))
self.walker.ready()
t = Transformation.get_transform_from_euler([k, 0, 0])
t.set_position([i, j, 0])
self.walker.setGoal(t)
# self.walker.updateGoal()
# self.walker.soccerbot.robot_path.show()
self.walker.wait(100)
self.walker.run(True)
pass
def test_imu_feedback(self):
import pybullet as pb
self.walker.setPose(Transformation([0, 0, 0], [0, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([2, 0, 0], [0, 0, 0, 1]))
pitches = []
times = []
t = 0
while t <= self.walker.soccerbot.robot_path.duration():
if self.walker.soccerbot.current_step_time <= t <= self.walker.soccerbot.robot_path.duration():
self.walker.soccerbot.stepPath(t, verbose=True)
pitch = self.walker.soccerbot.get_imu().get_orientation_euler()[1]
pitches.append(pitch)
times.append(t)
self.walker.soccerbot.apply_imu_feedback(self.walker.soccerbot.get_imu())
forces = self.walker.soccerbot.apply_foot_pressure_sensor_feedback(self.walker.ramp.plane)
pb.setJointMotorControlArray(bodyIndex=self.walker.soccerbot.body, controlMode=pb.POSITION_CONTROL,
jointIndices=list(range(0, 20, 1)),
targetPositions=self.walker.soccerbot.get_angles(),
forces=forces
)
self.walker.soccerbot.current_step_time = self.walker.soccerbot.current_step_time + self.walker.soccerbot.robot_path.step_size
pb.stepSimulation()
t = t + self.walker.PYBULLET_STEP
plt.plot(times, pitches)
plt.xlabel("Time (t)")
plt.ylabel("Forward pitch of robot in radians")
plt.grid(which='minor')
plt.show()
def test_small_movement_5(self):
self.walker.setPose(Transformation([0.3096807057334623, 0.09374110438873018, 0.0], [0.03189331238935847, -0.0065516868290173, 0.9990119776602083, 0.03024831426656374]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([0.14076394628045208, -0.034574636811865296, 0], [0, 0, -0.9999956132297835, -0.002962013029887055]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_walk_3(self):
self.walker.setPose(Transformation([-2.404, -1.0135, 0], [0, 0, -0.9979391070307153, 0.064168050139]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([-2.26, -1.27, 0], [0, 0, 0.997836202477347, 0.06574886330262358]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_walk_5(self):
self.walker.setPose(Transformation([0.716, -0.4188, 0.0], [0.0149, -0.085, 0.9685, 0.2483]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([0.0859, -0.016, 0.0], [0, 0, 0.998, 0.0176]))
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_do_nothing(self):
self.walker.setPose(Transformation([0, 0, 0], [0.00000, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
goal = Transformation.get_transform_from_euler([0, 0, 0])
self.walker.setGoal(goal)
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_turn_in_place(self):
self.walker.setPose(Transformation([0, 0, 0], [0.00000, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
goal = Transformation.get_transform_from_euler([np.pi, 0, 0])
self.walker.setGoal(goal)
self.walker.run()
def test_small_movement_3(self):
self.walker.setPose(Transformation([0, 0, 0], [0.00000, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
goal = Transformation.get_transform_from_euler([-np.pi/2, 0, 0])
goal.set_position([-0.2, -0.2, 0])
self.walker.setGoal(goal)
self.walker.run()
def poseAtRatio(self, r):
diff_position = self.end_transform.get_position(
)[0:2] - self.start_transform.get_position()[0:2]
start_angle = self.start_transform.get_orientation_euler()[0]
intermediate_angle = np.arctan2(diff_position[1], diff_position[0])
if self.isWalkingBackwards():
intermediate_angle = wrapToPi(intermediate_angle + np.pi)
final_angle = self.end_transform.get_orientation_euler()[0]
step_1_duration = abs(
wrapToPi(intermediate_angle -
start_angle)) / PathSection.angular_speed
step_2_duration = np.linalg.norm(diff_position) / PathSection.speed
step_3_duration = abs(
wrapToPi(intermediate_angle -
final_angle)) / PathSection.angular_speed
total_duration = step_1_duration + step_2_duration + step_3_duration
t = r * total_duration
if t == 0:
pose = deepcopy(self.start_transform)
return pose
elif t < step_1_duration != 0:
# First turn
pose = deepcopy(self.start_transform)
percentage = t / step_1_duration
angle = start_angle + wrapToPi(intermediate_angle -
start_angle) * percentage
pose.set_orientation(
Transformation.get_quaternion_from_euler([angle, 0, 0]))
return pose
elif step_1_duration < t <= step_1_duration + step_2_duration != 0:
# Then go straight
pose = deepcopy(self.start_transform)
percentage = (t - step_1_duration) / step_2_duration
position = diff_position * percentage + self.start_transform.get_position(
)[0:2]
pose.set_position(
np.concatenate((position, [pose.get_position()[2]])))
pose.set_orientation(
Transformation.get_quaternion_from_euler(
[intermediate_angle, 0, 0]))
return pose
elif step_1_duration + step_2_duration < t <= step_1_duration + step_2_duration + step_3_duration != 0:
# Then turn
pose = deepcopy(self.end_transform)
percentage = (t - step_1_duration -
step_2_duration) / step_3_duration
angle = intermediate_angle + wrapToPi(
final_angle - intermediate_angle) * percentage
pose.set_orientation(
Transformation.get_quaternion_from_euler([angle, 0, 0]))
return pose
else:
pose = deepcopy(self.end_transform)
return pose
def test_small_movement_0(self):
self.walker.setPose(Transformation([0, 0, 0], [0.00000, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
goal = Transformation.get_transform_from_euler([np.pi/5, 0, 0])
goal.set_position([0.05, 0.05, 0])
self.walker.setGoal(goal)
# self.walker.soccerbot.robot_path.show()
self.walker.run()
def test_camera_find_camera_coordinate_2(self):
q = Transformation.get_quaternion_from_euler([0, 0, 0])
p = Transformation([0, 0, 0.5], q)
c = Camera("robot1")
c.pose = p
c.resolution_x = 360
c.resolution_y = 240
p3 = c.findCameraCoordinate([0.5, 0, 0.5])
self.assertAlmostEqual(p3[0], 360 / 2)
self.assertAlmostEqual(p3[1], 240 / 2)
def test_camera_find_floor_coordinate(self):
q = Transformation.get_quaternion_from_euler([0, math.pi / 4, 0])
p = Transformation([0, 0, 0.5], q)
c = Camera("robot1")
c.pose = p
c.resolution_x = 360
c.resolution_y = 240
p2 = c.findFloorCoordinate([360 / 2, 240 / 2])
self.assertAlmostEqual(p2[0], 0.5)
self.assertAlmostEqual(p2[1], 0)
self.assertAlmostEqual(p2[2], 0)
def inverseKinematicsRightFoot(self, transformation):
"""
#TODO
:param transformation: #TODO
:return: Motor angles for the right foot
"""
transformation[0:3, 3] = transformation[0:3, 3] - self.torso_to_right_hip[0:3, 3]
invconf = np.linalg.inv(transformation)
d3 = self.DH[2, 0]
d4 = self.DH[3, 0]
Xd = invconf[0, 3]
Yd = invconf[1, 3]
Zd = invconf[2, 3]
if (np.linalg.norm([Xd, Yd, Zd]) > (d3 + d4)):
print("IK Position Unreachable: Desired Distance: " + str(
np.linalg.norm([Xd, Yd, Zd])) + ", Limited Distance: " + str(d3 + d4))
assert (np.linalg.norm([Xd, Yd, Zd]) <= (d3 + d4))
theta6 = -np.arctan2(Yd, Zd)
tmp1 = Zd / np.cos(theta6)
tmp2 = Xd
D = ((((((tmp1 ** 2) + (tmp2 ** 2)) - ((d3 ** 2) + (d4 ** 2))) / 2) / d3) / d4)
tmp3 = np.arctan2(D, -np.sqrt(1 - (D ** 2)))
tmpX = (tmp3 - (np.pi / 2))
if tmpX < 0:
tmpX = tmpX + (2 * np.pi)
theta4 = -(np.unwrap([tmpX])[0])
assert (theta4 < 4.6)
alp = np.arctan2(tmp1, tmp2)
beta = np.arctan2(-d3 * np.cos(tmp3), d4 + (d3 * np.sin(tmp3)))
theta5 = np.pi / 2 - (alp - beta)
H34 = tr.get_transform_from_dh(self.DH[3, 0], self.DH[3, 1], self.DH[3, 2], theta4)
H45 = tr.get_transform_from_dh(self.DH[4, 0], self.DH[4, 1], self.DH[4, 2], theta5)
H56 = tr.get_transform_from_dh(self.DH[5, 0], self.DH[5, 1], self.DH[5, 2], theta6)
H36 = np.matmul(H34, np.matmul(H45, H56))
final_rotation = tr.get_transform_from_euler([0, np.pi / 2, np.pi])
H03 = np.matmul(np.matmul(transformation, final_rotation), np.linalg.inv(H36))
assert (np.linalg.norm(H03[0:3, 3]) - d3 < 0.03)
angles = tr.get_euler_from_rotation_matrix(np.linalg.inv(H03[0:3, 0:3]), orientation='ZYX')
theta3 = np.pi / 2 - angles[0]
theta1 = -angles[1]
theta2 = (angles[2] + np.pi / 2)
return [theta1, theta2, theta3, theta4, theta5, theta6]
def test_terminate_walk(self):
import rospy
self.walker.setPose(Transformation([0.5, 0, 0], [0, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([2.0, 1.0, 0], [0, 0, 0, 1]))
def send_terminate_walk(_):
self.walker.terminate_walk = True
pass
self.send_terminate_walk = rospy.Timer(rospy.Duration(5), send_terminate_walk, oneshot=True)
self.walker.run()
def test_dynamic_walking_1(self):
import rospy
self.walker.setPose(Transformation([0.5, 0, 0], [0, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([1.5, 0, 0], [0, 0, 0, 1]))
def send_alternative_trajectory(_):
self.walker.setGoal(Transformation([2.5, 0.5, 0], [0, 0, 0, 1]))
pass
self.send_alternative_trajectory = rospy.Timer(rospy.Duration(5), send_alternative_trajectory, oneshot=True)
self.walker.run()
def test_path_combination_long_to_short(self):
import rospy
self.walker.setPose(Transformation([0.5, 0, 0], [0, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([1.0, 0, 0], [0, 0, 0, 1]))
def send_alternative_trajectory(_):
self.walker.setGoal(Transformation([1, 0.1, 0], [0, 0, 0, 1]))
pass
self.send_alternative_trajectory = rospy.Timer(rospy.Duration(3), send_alternative_trajectory, oneshot=True)
self.walker.run()
def __init__(self, robot_name: str):
self.robot_name = robot_name
self.pose = Transformation()
self.resolution_x = None
self.resolution_y = None
self.camera_info = None
self.diagonal_fov = 1.523 # 1.57 # 1.523 # 1.57 #1.523 # 1.39626 # 1.523 # 1.723# 1.39626 # 1.5231001536981417 # old: 1.57
self.focal_length = 3.67 # 3.67
self.camera_info_subscriber = Subscriber(
"/" + robot_name + "/camera/camera_info", CameraInfo,
self.cameraInfoCallback)
self.tf_listener = TransformListener()
def test_imu_feedback_webots(self):
import pybullet as pb
from soccerbot_controller import SoccerbotController
self.walker.setPose(Transformation([0, 0, 0], [0, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.soccerbot.ready() # TODO Cancel walking
self.walker.soccerbot.reset_head()
self.walker.soccerbot.publishAngles()
print("Getting ready")
self.walker.wait(150)
# Reset robot position and goal
self.walker.soccerbot.createPathToGoal(Transformation([0.5, 0, 0], [0, 0, 0, 1]))
pitches = []
times = []
t = 0
r = rospy.Rate(1/SoccerbotController.PYBULLET_STEP)
while t <= self.walker.soccerbot.robot_path.duration():
if self.walker.soccerbot.current_step_time <= t <= self.walker.soccerbot.robot_path.duration():
self.walker.soccerbot.stepPath(t, verbose=True)
pitch = self.walker.soccerbot.get_imu().get_orientation_euler()[1]
f = self.walker.soccerbot.apply_imu_feedback(t, self.walker.soccerbot.get_imu())
times.append(t)
pitches.append((pitch, f))
forces = self.walker.soccerbot.apply_foot_pressure_sensor_feedback(self.walker.ramp.plane)
pb.setJointMotorControlArray(bodyIndex=self.walker.soccerbot.body, controlMode=pb.POSITION_CONTROL,
jointIndices=list(range(0, 20, 1)),
targetPositions=self.walker.soccerbot.get_angles(),
forces=forces
)
self.walker.soccerbot.current_step_time = self.walker.soccerbot.current_step_time + self.walker.soccerbot.robot_path.step_size
pb.stepSimulation()
self.walker.soccerbot.publishAngles()
t = t + self.walker.PYBULLET_STEP
r.sleep()
plt.plot(times, pitches)
plt.xlabel("Time (t)")
plt.ylabel("Forward pitch of robot in radians")
plt.grid(which='minor')
plt.show()
def setPose(self, pose: tr):
try:
last_hip_height = self.pose.get_position()[2]
except:
self.pose = pose
last_hip_height = Soccerbot.standing_hip_height
self.pose.set_position([pose.get_position()[0], pose.get_position()[1], last_hip_height])
# Remove the roll and yaw from the pose
[r, p, y] = pose.get_orientation_euler()
q_new = tr.get_quaternion_from_euler([r, 0, 0])
self.pose.set_orientation(q_new)
if os.getenv('ENABLE_PYBULLET', False):
pb.resetBasePositionAndOrientation(self.body, self.pose.get_position(), self.pose.get_orientation())
def test_path_combination_basic(self):
plt.figure()
height = 0.321
start_transform = Transformation([0.5, 0, height], [0, 0, 0, 1])
end_transform = Transformation([0.9, 0, height], [0, 0, 0, 1])
path = Path(start_transform, end_transform)
path.show()
t = 2
end_transform_new = Transformation([1.3, 0, height], [0, 0, 0, 1])
path.dynamicallyUpdateGoalPosition(t, end_transform_new)
path.show()
plt.show()
def calculateBallFromBoundingBoxes(
self, ball_radius: float,
bounding_boxes: [float]) -> Transformation:
# bounding boxes [(y1, z1), (y2, z2)]
r = ball_radius
y1 = bounding_boxes[0][0]
z1 = bounding_boxes[0][1]
y2 = bounding_boxes[1][0]
z2 = bounding_boxes[1][1]
# Assuming the ball is a sphere, the bounding box must be a square, averaging the borders
ym = (y1 + y2) / 2
zm = (z1 + z2) / 2
length = (z2 - z1)
width = (y2 - y1)
y1 = ym - (width / 2)
z1 = zm - (length / 2)
y2 = ym + (width / 2)
z2 = zm + (length / 2)
y1w, z1w = self.imageToWorldFrame(y1, z1)
y2w, z2w = self.imageToWorldFrame(y2, z2)
y1w = -y1w
z1w = -z1w
y2w = -y2w
z2w = -z2w
f = self.focal_length
thetay1 = math.atan2(y1w, f)
thetay2 = math.atan2(y2w, f)
thetayy = (thetay2 - thetay1) / 2
thetay = thetay1 + thetayy
dy = r / math.sin(thetayy)
xy = (math.cos(thetay) * dy, math.sin(thetay) * dy)
thetaz1 = math.atan2(z1w, f)
thetaz2 = math.atan2(z2w, f)
thetazz = (thetaz2 - thetaz1) / 2
thetaz = thetaz1 + thetazz
dz = r / math.sin(thetazz)
xz = (math.cos(thetaz) * dz, math.sin(thetaz) * dz)
ball_x = xy[0]
ball_y = xy[1]
ball_z = xz[1]
tr = Transformation([ball_x, -ball_y, -ball_z])
tr_cam = self.pose @ tr
return tr_cam
def test_calculate_bounding_boxes_from_ball(self):
for cam_angle in [0, 0.1, -0.1]:
q = Transformation.get_quaternion_from_euler([cam_angle, 0, 0])
for cam_position in [[0, 0, 0], [0, 0, 0.1], [0, 0, -0.1]]:
p = Transformation(cam_position, q)
c = Camera("robot1")
c.pose = p
c.resolution_x = 360
c.resolution_y = 240
positions = [[0.5, 0, 0.1], [0.5, 0, 0], [0.5, 0, 0.1]]
for position in positions:
ball_pose = Transformation(position)
ball_radius = 0.1
bounding_boxes = c.calculateBoundingBoxesFromBall(
ball_pose, ball_radius)
# [[135.87634651355825, 75.87634651355823], [224.12365348644175, 164.12365348644175]]
position = c.calculateBallFromBoundingBoxes(
ball_radius, bounding_boxes)
self.assertAlmostEqual(position.get_position()[0],
ball_pose.get_position()[0],
delta=0.001)
self.assertAlmostEqual(position.get_position()[1],
ball_pose.get_position()[1],
delta=0.001)
self.assertAlmostEqual(position.get_position()[2],
ball_pose.get_position()[2],
delta=0.001)
