Changing the Baxter Source Code

Below are instructions to modify the Baxter Source Code:

In the file /baxter_interface/src/baxter_interface/limb.py, replace the `move_to_joint_positions' function with the following:

def move_to_joint_positions(self, positions, _rate=100, timeout=15.0,
                            threshold=settings.JOINT_ANGLE_TOLERANCE,
                            test=None):
    """
    (Blocking) Commands the limb to the provided positions.

    Waits until the reported joint state matches that specified.

    This function uses a low-pass filter to smooth the movement.

    @type positions: dict({str:float})
    @param positions: joint_name:angle command
    @type timeout: float
    @param timeout: seconds to wait for move to finish [15]
    @type threshold: float
    @param threshold: position threshold in radians across each joint when
    move is considered successful [0.008726646]
    @param test: optional function returning True if motion must be aborted
    """
    cmd = self.joint_angles()

    def filtered_cmd():
        # First Order Filter - 0.2 Hz Cutoff
        for joint in positions.keys():
            cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
        return cmd

    def genf(joint, angle):
        def joint_diff():
            return abs(angle - self._joint_angle[joint])
        return joint_diff

    diffs = [genf(j, a) for j, a in positions.items() if
             j in self._joint_angle]

    self.set_joint_positions(filtered_cmd())
    baxter_dataflow.wait_for(
        test=lambda: callable(test) and test() == True or \
                     (all(diff() < threshold for diff in diffs)),
        timeout=timeout,
        timeout_msg=("%s limb failed to reach commanded joint positions" %
                     (self.name.capitalize(),)),
        rate=_rate,
        raise_on_error=False,
        body=lambda: self.set_joint_positions(filtered_cmd())
        )