Ejemplo n.º 1
0
    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)
        }
Ejemplo n.º 2
0
    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
Ejemplo n.º 3
0
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)
Ejemplo n.º 4
0
 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")
Ejemplo n.º 5
0
 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)
Ejemplo n.º 6
0
    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)
Ejemplo n.º 7
0
    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)
Ejemplo n.º 8
0
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
Ejemplo n.º 9
0
    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)
Ejemplo n.º 10
0
    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)
Ejemplo n.º 11
0
    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]))
Ejemplo n.º 12
0
 def constructJoints(self, jointFile):
     f = open(jointFile, 'r')
     tempjoint = []
     next(f)
     for line in f:
         tempjoint.append(Joint(line))
     f.close()
     return tempjoint
Ejemplo n.º 13
0
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
Ejemplo n.º 14
0
    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)
Ejemplo n.º 15
0
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)
Ejemplo n.º 16
0
Archivo: body.py Proyecto: sainy/robot
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()
Ejemplo n.º 17
0
Archivo: body.py Proyecto: sainy/robot
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()
Ejemplo n.º 18
0
        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()
Ejemplo n.º 19
0
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")
Ejemplo n.º 20
0
	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
Ejemplo n.º 21
0
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
Ejemplo n.º 22
0
Archivo: body.py Proyecto: sainy/robot
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()
Ejemplo n.º 23
0
    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")
Ejemplo n.º 24
0
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
Ejemplo n.º 25
0
    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()
Ejemplo n.º 26
0
    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
Ejemplo n.º 27
0
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
Ejemplo n.º 28
0
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
Ejemplo n.º 29
0
    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
Ejemplo n.º 30
0
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