def setup_joints(self):
pi = pigpio.pi()
# pi.set_mode(constants.GPIO_GRABBER, pigpio.OUTPUT)
# pi.set_mode(constants.GPIO_ELBOW, pigpio.OUTPUT)
# pi.set_mode(constants.GPIO_WRIST, pigpio.OUTPUT)
self.joints = {
ServoName.GRABBER:
Joint(ServoName.GRABBER.value, constants.GPIO_GRABBER,
constants.GRABBER_POS_INIT, constants.GRABBER_POS_MIN,
constants.GRABBER_POS_MAX, constants.SERVO_POS_DELTA,
self.curr_control_type, self.locked, self.held,
self.last_pressed_button_joint,
self.last_pressed_button_command, pi, False),
ServoName.ELBOW:
Joint(ServoName.ELBOW.value, constants.GPIO_ELBOW,
constants.ELBOW_POS_INIT, constants.ELBOW_POS_MIN,
constants.ELBOW_POS_MAX, constants.SERVO_POS_DELTA,
self.curr_control_type, self.locked, self.held,
self.last_pressed_button_joint,
self.last_pressed_button_command, pi, False),
ServoName.WRIST:
Joint(ServoName.WRIST.value, constants.GPIO_WRIST,
constants.WRIST_POS_INIT, constants.WRIST_POS_MIN,
constants.WRIST_POS_MAX, constants.SERVO_POS_DELTA,
self.curr_control_type, self.locked, self.held,
self.last_pressed_button_joint,
self.last_pressed_button_command, pi, True)
}
def draw_left_arm(self,
left_shoulder_lateral_radians=0,
left_shoulder_frontal_radians=0,
left_elbow_lateral_radians=0,
color='c-',
draw=True):
left_shoulder_lateral = Joint(
dh_params=(-half_pi + left_shoulder_lateral_radians,
shoulder_length, 0, -half_pi),
initial_transformation=self._get_torso_corner_transformation(
left_shoulder_position, 'x', -half_pi))
left_shoulder_frontal = Joint(
prev_joint=left_shoulder_lateral,
dh_params=(half_pi - left_shoulder_frontal_radians, 0,
-upper_arm_length, -half_pi))
left_elbow_lateral = Joint(prev_joint=left_shoulder_frontal,
dh_params=(-left_elbow_lateral_radians, 0,
0, 0))
left_wrist_t = left_elbow_lateral.link_t.dot(
get_translate_matrix(-lower_arm_length, 0, 0))
left_wrist = apply_matrix_to_origin(left_wrist_t)
if draw:
self.draw_link(left_shoulder_lateral, color)
self.draw_link(left_shoulder_frontal, color)
self._painter.draw_line(left_elbow_lateral.vertex,
left_wrist,
color=color)
return left_shoulder_lateral, left_shoulder_frontal, left_elbow_lateral, left_wrist
def main():
"Function to contsruct an arm and input movements"
parts = [
Joint(name='r1', z_axis=[0, 0, 1]),
Bone(length=5),
Joint(name='r2', z_axis=[0, 1, 0])
]
robot_arm = Arm(parts=parts)
def __init__(self, leg_name, offset, servos):
self.leg_name = leg_name
if not path.exists("config/" + leg_name):
makedirs(leg_name)
self.offset = offset
self.upper_hip = Joint(servos["upper_hip"], "config/" + leg_name + "/upper_hip.json")
self.hip = Segment("config/" + leg_name + "/hip.json")
self.lower_hip = Joint(servos["lower_hip"], "config/" + leg_name + "/lower_hip.json")
self.femur = Segment("config/" + leg_name + "/femur.json")
self.knee = Joint(servos["knee"], "config/" + leg_name + "/knee.json")
self.tibia = Segment("config/" + leg_name + "/tibia.json")
def create_empty_data_struct(self):
COF = Joint(None, None, "COF")
COM = Joint(None, None, "COF")
C7 = Joint(None, None, "COF")
angle_lower = Angle(None, None, "angle_lower")
angle_trunk = Angle(None, None, "angle_trunk")
phase = OLD_PHASE_OUT_PHASE(None, None, None, name="phase")
return Move(cof=COF,
com=COM,
c7=C7,
angle_l=angle_lower,
angle_t=angle_trunk,
phase=phase)
def draw_neck_and_head(self,
neck_transversal_radians=0,
neck_lateral_radians=0):
neck_transversal = Joint(
dh_params=(0 + neck_transversal_radians, neck_length, 0, half_pi),
initial_transformation=get_translate_matrix(0, 0, torso_height))
neck_lateral = Joint(prev_joint=neck_transversal,
dh_params=(-neck_lateral_radians, 0, 0, 0))
self.draw_link(neck_transversal)
self._draw_head(head_t=neck_lateral.link_t)
def __init__(self, filename, scale):
self.file = open(filename, 'r')
# Read hierarchy
self.__expectKeyword('HIERARCHY')
items = self.__expectKeyword('ROOT')
self.root = Joint(items[1])
self.__readJoint(self.root, scale)
# Read motion
self.__expectKeyword('MOTION')
items = self.__expectKeyword('Frames:')
self.frames = int(items[1])
items = self.__expectKeyword('Frame') # Time:
self.frameTime = float(items[2])
for i in range(self.frames):
line = self.file.readline()
items = line.split()
data = [float(item) for item in items]
data = self.__getChannelData(self.root, data)
def get_joints_from_table(table):
joints = []
if not (table_ok):
raise ValueError('Antecedants should be sorted in the table')
for j, row in enumerate(table):
if (row[0] == 0):
antc = None
else:
antc = joints[row[0] - 1]
# TODO: redefine sameas by starting with index 0 (-1 <=> None)
if (row[1] == 0):
sameas = None
else:
sameas = joints[row[1] - 1]
jnt = Joint(
j=j + 1,
antc=antc,
sameas=sameas,
mu=row[2],
sigma=row[3],
gamma=row[4],
b=row[5],
alpha=row[6],
d=row[7],
theta=row[8],
r=row[9],
)
joints.append(jnt)
# TODO: add recursion detection for antecedants
return joints
def __init__(self, line_flag=False):
# log setting
program = os.path.basename(__name__)
self.logger = logging.getLogger(program)
logging.basicConfig(
format='%(asctime)s : %(name)s : %(levelname)s : %(message)s')
# load config file
f = open(Experiment.CONFIG_YAML, 'r')
self.config = yaml.load(f)
f.close()
self.english_corpus_dir = self.config['english_corpus_dir']
self.japanese_corpus_dir = self.config['japanese_corpus_dir']
self.japanese_original_corpus_dir = self.config[
'japanese_original_corpus_dir']
self.img_features_npy = self.config['img_features_npy']
self.img_original_dir = self.config['img_original_dir']
self.img_correspondence_path = self.config['img_correspondence_path']
self.joint = Joint(self.english_corpus_dir, self.img_features_npy,
self.japanese_corpus_dir, self.img_original_dir,
self.img_correspondence_path,
self.japanese_original_corpus_dir,
Experiment.PCA_COMPRESS_WORD_DIM,
Experiment.PCA_COMPRESS_IMG_DIM, line_flag)
self.logger.info("<Initilalizing Experiment>")
if Experiment.SEED_NUM is not None:
self.logger.info("seed: %s", Experiment.SEED_NUM)
np.random.seed(Experiment.SEED_NUM)
def createSkeleton(self, node, parent=None):
"""Create the skeleton hierarchy.
This method creates the skeleton recursively. Each invocation
creates one joint.
"""
order = self.rotationOrder(node.channels)
# Create a new Joint object
j = Joint(name=node.name,
pos=vec3(node.offset),
rotationorder=order,
parent=parent)
# Store the joint in the node so that later the motion can be applied
node.joint = j
vtx = ValueTable(type="double")
vty = ValueTable(type="double")
vtz = ValueTable(type="double")
vtx.output_slot.connect(j.anglex_slot)
vty.output_slot.connect(j.angley_slot)
vtz.output_slot.connect(j.anglez_slot)
node.vtx = vtx
node.vty = vty
node.vtz = vtz
if node.isRoot():
vtpos = ValueTable(type="vec3")
vtpos.output_slot.connect(j.pos_slot)
node.vtpos = vtpos
for c in node.children:
self.createSkeleton(c, j)
def __readJoint(self, joint, scale=0.25):
self.__expectKeyword('{')
self.__calcPosition(joint, scale)
items = self.__expectKeyword('CHANNELS')
joint.channels = items[2:]
joint.parseChannels(items[2:])
if int(items[1]) != len(joint.channels):
RuntimeError('Expected %d channels found %d' %
(items[1], len(joint.channels)))
# Read child joints
while 1:
line = self.file.readline()
items = line.split()
if items[0] == 'JOINT':
child = Joint(items[1])
joint.children.append(child)
child.parent = joint
self.__readJoint(child, scale)
elif items[0] == 'End': # Site
child = Joint('End effector')
joint.children.append(child)
child.channels = []
child.parent = joint
self.__expectKeyword('{')
self.__calcPosition(child, scale)
self.__expectKeyword('}')
elif items[0] == '}':
break
else:
raise RuntimeError('Expected %s found %s' %
('JOINT, End Site or }', items[0]))
def constructJoints(self, jointFile):
f = open(jointFile, 'r')
tempjoint = []
next(f)
for line in f:
tempjoint.append(Joint(line))
f.close()
return tempjoint
def build_joint(joint_id: id, reverse_direction: int):
servo = Servo(joint_id, False)
joint = Joint(servo,
min_angle=0,
max_angle=180,
reverse_direction=reverse_direction,
instant_move=False)
return joint
def onHierarchy(self, links):
# At the end, this list contains the leaves which need
# a dummy joint at the end.
leaves = []
for parentname, childnames in links:
leaves += childnames
if parentname in leaves:
leaves.remove(parentname)
# Create the second joint of the bone called parentname
# (which is the first joint of the bone called data["name"])
parent = self.joints[parentname]
data = self.bones[parentname]
dir = data["direction"].normalize()
length = data["length"]
for childname in childnames:
data = self.bones[childname]
axis = data["axis"]
ao = data["axis_order"]
axis_order = ao[2] + ao[1] + ao[0]
j = Joint(name=data["name"],
rotationorder=axis_order,
radius=0.3,
pos=length * dir.normalize(),
parent=parent)
exec "fromEuler = mat3.fromEuler%s" % axis_order.upper()
R = fromEuler(radians(axis.x), radians(axis.y),
radians(axis.z))
j.setOffsetTransform(mat4(1).setMat3(R))
j.freezePivot()
self.joints[j.name] = j
# Create end joints
for name in leaves:
parent = self.joints[name]
data = self.bones[name]
try:
dir = data["direction"].normalize()
except:
dir = vec3(0)
length = data["length"]
Joint(name=name + "_end",
rotationorder="xyz",
radius=0.3,
pos=length * dir,
parent=parent)
def coord_to_joint(coord, person_id, conf):
# type: (Tuple[int, float, float, float], int, float) -> Joint
"""
Given a `coord`, i.e. a tuple (jtype, x3d, y3d, z3d) and a person ID,
returns the corresponding `Joint` object
"""
return Joint(np.array(
[-1, person_id, coord[0], -1, -1, coord[1], coord[2], coord[3], 0, 0]),
confidence=conf)
class LeftLeg:
l_toe = Joint("left toe", init_pos={"x": -9, "y": 10, "z": 0})
l_sole = Joint("left sole",
init_pos={
"x": -6,
"y": 5,
"z": 0
},
dw_joint=[l_toe])
l_ankle = Joint("left ankle",
init_pos={
"x": -3,
"y": -1,
"z": 0
},
dw_joint=[l_sole])
l_knee = Joint("left knee",
init_pos={
"x": -4,
"y": 0,
"z": 40
},
dw_joint=[l_ankle])
l_thigh = Joint("left thigh",
init_pos={
"x": -3,
"y": 0,
"z": 95
},
dw_joint=[l_knee])
def __init__(self):
ll = {"name": "LeftLeg"}
ll["controller"] = [
self.l_toe, self.l_sole, self.l_ankle, self.l_knee, self.l_thigh
]
Store.register(ll)
def printInfo(self):
self.l_toe.printInfo()
self.l_sole.printInfo()
self.l_ankle.printInfo()
self.l_knee.printInfo()
self.l_thigh.printInfo()
class RightLeg:
r_toe = Joint("right toe", init_pos={"x": 9, "y": 10, "z": 0})
r_sole = Joint("right sole",
init_pos={
"x": 6,
"y": 5,
"z": 0
},
dw_joint=[r_toe])
r_ankle = Joint("right ankle",
init_pos={
"x": 3,
"y": -1,
"z": 0
},
dw_joint=[r_sole])
r_knee = Joint("right knee",
init_pos={
"x": 4,
"y": 0,
"z": 40
},
dw_joint=[r_ankle])
r_thigh = Joint("right thigh",
init_pos={
"x": 3,
"y": 0,
"z": 95
},
dw_joint=[r_knee])
def __init__(self):
rl = {"name": "RightLeg"}
rl["controller"] = [
self.r_toe, self.r_sole, self.r_ankle, self.r_knee, self.r_thigh
]
Store.register(rl)
def printInfo(self):
self.r_toe.printInfo()
self.r_sole.printInfo()
self.r_ankle.printInfo()
self.r_knee.printInfo()
self.r_thigh.printInfo()
def addNew(val):
joint = Joint(1, self.numJoints)
joint.initJoint = jointInit
self.joints.append(joint)
self.numJoints += 1
self.link_slide.valmax = self.numJoints - 1
self.link_slide.ax.set_xlim(self.link_slide.valmin,
self.link_slide.valmax)
update_link()
def get_pose(frame_data, person_id):
# type: (np.ndarray, int) -> Pose
"""
:param frame_data: data of the current frame
:param person_id: person identifier
:return: list of joints in the current frame with the required person ID
"""
pose = [Joint(j) for j in frame_data[frame_data[:, 1] == person_id]]
pose.sort(key=(lambda j: j.type))
return Pose(pose, "JTA")
def __init__(self, robot, motion_proxy, am_proxy):
self.posture = Posture(robot, motion_proxy)
self.motion_proxy = motion_proxy
self.am_proxy = am_proxy
self.manager_proxy = ALProxy("ALBehaviorManager", robot.ip, robot.port)
self.master_joint = Joint(motion_proxy)
self.unrefined_movement = Unrefined_Control(master_joint, motion_proxy)
self.is_aware = True
self.robot_upper_joints = ['Head', 'LShoulder', 'LElbow', 'LWrist', 'RShoulder', 'RElbow', 'RWrist','LHand', 'RHand']
self.previous_behaviour = None
pass
def refine_pose(pose, refiner):
# type: (Sequence[Sequence[float]], Refiner) -> Optional[List[np.ndarray]]
"""
:param pose: list of joints where each joint is in the form [jtype, x3d, y3d, z3d]
:param refiner: pose refiner model
:return: refined pose -> list of 14 ordered joints where each joint is in the form [x3d, y3d, z3d]
>> see `Joint.NAMES` for joint order
"""
# convert `pose` list into a `Pose` object
joints = []
for jtype in range(14):
_joint = [j for j in pose if j[0] == jtype]
if len(_joint) == 1:
_, x, y, z = _joint[0][0], _joint[0][1], _joint[0][2], _joint[0][3]
joint = np.array([-1, -1, jtype, -1, -1, x, y, z, 0, 0])
joint = Joint(joint)
joints.append(joint)
else:
joint = np.array([-1, -1, jtype, -1, -1, -1, -1, -1, 1, 1])
joint = Joint(joint)
joints.append(joint)
pose = Pose(joints)
# convert `Pose` object into a fountain
rr_pose_pred = pose.to_rr_pose(MAX_LS)
for jtype in range(1, 14):
if not pose[jtype].visible:
rr_pose_pred[jtype - 1] = np.array([-1, -1, -1])
rr_pose_pred = torch.tensor(rr_pose_pred).unsqueeze(0).float()
# refine fountain with `refiner` model
refined_rr_pose_pred = refiner.forward(rr_pose_pred).numpy().squeeze()
if pose[0].type == 0:
refined_pose_pred = Pose.from_rr_pose(refined_rr_pose_pred,
head_pos3d=pose[0].pos3d,
max_ls=MAX_LS)
return refined_pose_pred
else:
return None
class Body:
"""this class is used to initial the whole body.
each key point in the body is definited at this place."""
Left_hand = LeftHand()
Right_hand = RightHand()
Left_leg = LeftLeg()
Right_leg = RightLeg()
head = Joint("The head top", init_pos={"x": 0, "y": 0, "z": 175})
neck = Joint("body's neck",
init_pos={
"x": 0,
"y": 0,
"z": 150
},
dw_joint=[head, Left_hand.l_hand, Right_hand.r_hand])
back = Joint("the back of body",
init_pos={
"x": 0,
"y": 0,
"z": 125
},
dw_joint=[neck])
waist = Joint("the waist of body",
init_pos={
"x": 0,
"y": 0,
"z": 110
},
dw_joint=[back, Left_leg.l_thigh, Right_leg.r_thigh])
def __init__(self):
body = {"name": "The whole body"}
body["controller"] = [self.head, self.neck, self.back, self.waist]
Store.register(body)
def printInfo(self):
self.waist.printInfo()
def load_model(self):
n = len(self)
x_mid_cheville = (np.ones(n) * (self.raw.ankle_r + self.raw.ankle_l) /
2)
y_mid_cheville = (np.ones(n) *
(self.raw.ankle_r_y + self.raw.ankle_l_y) / 2)
t0 = Joint(x_mid_cheville, y_mid_cheville, 'translation vector'
) #vector de translation vers l'origine du model
self.P0 = Joint(x_mid_cheville, y_mid_cheville, 'P0') - t0
self.ankle_l = Joint(self.raw.ankle_l, self.raw.ankle_l_y,
'ankle_l') - t0
self.ankle_r = Joint(self.raw.ankle_r, self.raw.ankle_r_y,
'ankle_r') - t0
self.c7 = Joint(self.raw["C7"], self.raw["C7_y"], "C7") - t0
self.pelvis = Joint(self.raw["COM"], self.raw["COM_y"], "COM") - t0
self.cof = Joint(self.raw["COF"], np.zeros(n), "COF") - t0
self.cof_rel = CenterOfForce(self.raw["COF_REL"], None, "COF_REL")
self.cof_rel.x_arr *= 2 # THIS CORRECTS THE ERROR IN COP_K_REL. the problem is in Unity, in the choplo project. gameManager line 295.
self.hip_left = Joint(self.raw['HipLeft_y'], self.raw['HipLeft_z'],
'hip_l') - t0
self.hip_right = Joint(self.raw['HipRight_y'], self.raw['HipRight_z'],
'hip_r') - t0
self.left_leg = TotalLeg(self.hip_left, self.ankle_l, "left")
self.right_leg = TotalLeg(self.hip_right, self.ankle_r, "right")
self.hat = HAT(self.pelvis, self.c7)
self.com = CenterOfMasse(
"center_of_mass_legsANDhat",
segments=[self.hat, self.left_leg, self.right_leg]).to_joint()
self.ddot_com = Joint(self.cof.x_arr - self.com.x_arr,
np.zeros(len(self.raw)), "ddot_com")
def get_joints_from_person_coords(coords_one_person_frame):
coords_one_person_frame = coords_one_person_frame[[
'frame_no_cam', 'person_id', 'joint_type', 'x_2D_joint', 'y_2D_joint',
'x_3D_joint', 'y_3D_joint', 'z_3D_joint', 'joint_occluded',
'joint_self_occluded', 'x_top_left_BB', 'y_top_left_BB',
'x_bottom_right_BB', 'y_bottom_right_BB', 'x_2D_person', 'y_2D_person',
'wears_glasses', 'ped_type'
]]
person_joints = []
for person_joint_coords in coords_one_person_frame.values.tolist():
person_joint = Joint(person_joint_coords)
person_joints.append(person_joint)
return person_joints
def __init__(self, filename):
self.name = None
self.joints = None
self.root = None
self.units = None
self.name2joint = {}
self.root = Joint(-1, "root", [0, 0, 0], [0, 0, 0], 1, "rx ry rz")
self.name2joint["root"] = self.root
def __init_name(name):
self.name = name
def __init_units(units):
self.units = units
def __init_joints(joint_data):
self.joints = [Joint.from_dict(entry) for entry in joint_data]
for joint in self.joints:
self.name2joint[joint.name] = joint
def __init_root(root_data):
self.root.direction = np.array(
[float(i) for i in root_data["position"]])
self.root.theta_degrees = np.array(
[float(i) for i in root_data["orientation"]])
def __init_hierarchy(hierarchy):
for h in hierarchy:
parent_name, dep_names = h
for dep_name in dep_names:
self.name2joint[dep_name].parent = self.name2joint[
parent_name]
reader = ASFReader(filename)
reader.onName = __init_name
reader.onUnits = __init_units
reader.onRoot = __init_root
reader.onBonedata = __init_joints
reader.read()
reader = ASFReader(filename)
reader.onHierarchy = __init_hierarchy
reader.read()
def draw_left_lower_limp(self,
left_hip_transversal_radians=0,
left_hip_frontal_radians=0,
left_hip_lateral_radians=0,
left_knee_lateral_radians=0,
left_ankle_lateral_radians=0,
left_ankle_frontal_radians=0,
color='c-',
draw=True,
get_lines=False):
left_hip_transversal = Joint(
dh_params=(-half_pi + left_hip_transversal_radians, hip_length, 0,
-half_pi),
initial_transformation=self._get_torso_corner_transformation(
left_hip_top_position, 'x', -pi))
hip_frontal = Joint(prev_joint=left_hip_transversal,
dh_params=(-half_pi - left_hip_frontal_radians, 0,
0, -half_pi))
hip_lateral = Joint(prev_joint=hip_frontal,
dh_params=(0 + left_hip_lateral_radians, 0,
upper_leg_length, 0))
knee_lateral = Joint(prev_joint=hip_lateral,
dh_params=(0 + left_knee_lateral_radians, 0,
lower_leg_length, 0))
ankle_lateral = Joint(prev_joint=knee_lateral,
dh_params=(0 - left_ankle_lateral_radians, 0, 0,
half_pi))
ankle_frontal = Joint(prev_joint=ankle_lateral,
dh_params=(half_pi + left_ankle_frontal_radians,
0, 0, -half_pi))
if draw or get_lines:
leg_links = self.draw_link(left_hip_transversal, color),\
self.draw_link(hip_lateral, color),\
self.draw_link(knee_lateral, color)
if get_lines:
ankle_foot_links = self._draw_foot(ankle_t=ankle_frontal.link_t,
color=color,
draw=draw,
get_lines=get_lines)
return tuple(list(leg_links) + list(ankle_foot_links))
else:
foot_center = self._draw_foot(ankle_t=ankle_frontal.link_t,
color=color,
draw=draw)
return left_hip_transversal, hip_frontal, hip_lateral, knee_lateral, ankle_lateral, ankle_frontal, foot_center
def demo_2() -> World:
world = World(gravity=GRAVITY, iterations=IMPULSE_ITERATIONS)
body_1 = Body([100.0, 10.0])
body_1.position = np.array([0.0, 0.5 * body_1.width[1]])
body_1.friction = 0.2
body_1.rotation = 0.0
world.add_body(body_1)
body_2 = Body([5.0, 5.0], 100.0)
body_2.position = np.array([45.0, 60.0])
body_2.friction = 0.2
body_2.rotation = 0.0
world.add_body(body_2)
joint = Joint(body_1, body_2, body_2.position * [0, 1])
world.add_joint(joint)
return world
def demo_7() -> World:
world = World(gravity=GRAVITY, iterations=IMPULSE_ITERATIONS)
Ground = Body([100.0, 10.0])
Ground.position = np.array([0.0, 0.5 * Ground.width[1]])
world.add_body(Ground)
PLANK_COUNT = 15
PLANK_MASS = 50.0
PLANK_WIDTH = 5.0
GAP = 1.25
period = PLANK_WIDTH + GAP
deck_length = period * PLANK_COUNT - GAP
deck_startx = -deck_length / 2
joint_startx = -deck_length / 2 - GAP / 2
planks = []
for i in range(PLANK_COUNT):
p = Body([PLANK_WIDTH, 0.25 * 5], PLANK_MASS)
p.position = np.array(
[deck_startx + PLANK_WIDTH / 2 + period * i, 25.0])
planks.append(p)
world.add_body(p)
# Joint Tuning
FREQUENCY_HERTZ = 2.0
frequency_radians = 2.0 * np.pi * FREQUENCY_HERTZ
DAMPING_RATIO = 0.7
damping_coeffcient = 2.0 * PLANK_MASS * DAMPING_RATIO * frequency_radians
stiffness = PLANK_MASS * frequency_radians**2
softness = 1 / (damping_coeffcient + TIMESTEP * stiffness)
bias_factor = TIMESTEP * stiffness / (damping_coeffcient +
TIMESTEP * stiffness)
limbs = [Ground] + planks + [Ground]
for i, (limb1, limb2) in enumerate(zip(limbs, limbs[1:])):
j = Joint(limb1, limb2, np.array([joint_startx + period * i, 25.0]))
j.softness = softness
j.bias_factor = bias_factor
world.add_joint(j)
return world
def __init__(self, interruptCallback, log):
"""The interruptCallack function must be provided so the caller can receive interrupts from the sensors"""
# Remember the callback function so we can call it when an interrupt condition is met
self.interruptCallback = interruptCallback
## Set up the joint on the given Servo Bonnet pin
self.joint = Joint(4)
# Set up the distance sensor on the given RPi pin
self.distanceSensor = DistanceSensor(14, log)
self._distanceSensorThread = None # thread on which to run continuous monitoring
self._runningDistanceSensor = False # flag to indicate if continuous monitoring is running
self._pauseDistanceSensor = False # flag to indicate if continuous monitoring is paused
self.lastDistance = None # last distance measured
# Set up the thermal sensor (uses I2C to the RPi)
# The thermal sensor reads a 8x8 matrix (64 readings)
self.thermalSensor = ThermalSensor()
self.thermalSensorThread = None # thread on which to run continuous monitoring
self._runningThermalSensor = False # flag to indicate if continuous monitoring is running
self._pauseThermalSensor = False # flag to indicate if continuous monitoring is paused
self.lastMaxTemperature = None # last max temperature read from the 8x8 matrix
self.lastMinTemperature = None # last min temperature read from the 8x8 matrix
# Set up the PIR, which is on the tail
self.tail = MotionSensor(13)
# Settings - can be overridden by the body
self.joint.midAngle = 90 # midpoint angle for servo (facing forwards)
self.joint.highAngle = 135 # high angle for servo (rightmost angle)
self.joint.lowAngle = 45 # lowest angle for servo (leftmost angle)
self.trackDelta = 10 # change in angle each iteration when tracking movement
self.shortDistance = 20 # distance in cm to obstacle below which triggers a short-distance interrupt
self.longDistance = 1000 # distance in cm to obstacle above which triggers a long-distance interrupt
self.humanDetectMinTemperature = 26 # temperature in C which triggers human-detect interrupt
self.humanDetectMaxTemperature = 30 # temperature in C which triggers human-detect interrupt
self.colMovementThreshold = 5 # total temperature change in a matrix column above which we say we saw movement
self.movementWaitSeconds = 2 # how long to wait between thermal readings to detect movement
self.log = log
def demo_6() -> World:
world = World(gravity=GRAVITY, iterations=IMPULSE_ITERATIONS)
Ground = Body([100.0, 10.0])
Ground.position = np.array([0.0, 0.5 * Ground.width[1]])
world.add_body(Ground)
teeter = Body([60.0, 1.25], 100.0)
teeter.position = np.array([0.0, 15.0])
world.add_body(teeter)
small = Body([2.5, 2.5], 25.0)
small.position = np.array([-27.5, 20.0])
world.add_body(small)
big = Body([5.0, 5.0], 100.0)
big.position = np.array([27.5, 85.0])
world.add_body(big)
j = Joint(Ground, teeter, teeter.position)
world.add_joint(j)
return world
