def go()

in experiments/grasp_stability/robot.py [0:0]

• 41 lines of code
• 10 McCabe index (conditional complexity)

    def go(self, pos, ori=None, width=None, wait=False, gripForce=20):
        if ori is None:
            ori = self.ori

        if width is None:
            width = self.width

        ori_q = pb.getQuaternionFromEuler(ori)

        jointPose = pb.calculateInverseKinematics(self.robotID, self.eefID, pos, ori_q)
        jointPose = np.array(jointPose)

        jointPose[self.gripperControlID[0]] = -width / 2
        jointPose[self.gripperControlID[1]] = width / 2

        maxForces = np.ones(len(jointPose)) * 200
        maxForces[self.gripperControlID] = gripForce

        # Select the relavant joints for arm and gripper
        jointPose = jointPose[self.armControlID + self.gripperControlID]
        maxForces = maxForces[self.armControlID + self.gripperControlID]

        pb.setJointMotorControlArray(
            self.robotID,
            tuple(self.armJoints + self.gripperJoints),
            pb.POSITION_CONTROL,
            targetPositions=jointPose,
            forces=maxForces,
        )

        self.pos = pos
        if ori is not None:
            self.ori = ori
        if width is not None:
            self.width = width

        if wait:
            last_err = 1e6
            while True:
                pb.stepSimulation()
                ee_pose = self.get_ee_pose()
                w = self.get_gripper_width()
                err = (
                    np.sum(np.abs(np.array(ee_pose[0]) - pos))
                    + np.sum(np.abs(np.array(ee_pose[1]) - ori_q))
                    + np.abs(w - width)
                )
                diff_err = last_err - err
                last_err = err

                if np.abs(diff_err) < self.tol:
                    break