Beispiel #1
0
 void ChangeIndicatorColor(Grasp grasp, Color color)
 {
     foreach (var child in grasp.GetComponentsInChildren <MeshRenderer>())
     {
         child.material.SetColor("_Color1", color);
     }
 }
Beispiel #2
0
        // Use this for initialization
        void Start()
        {
            if (!this._graspable_object)
            {
                this._graspable_object = this.GetComponent <GraspableObject>();
            }

            if (!this._gripper)
            {
                this._gripper = FindObjectOfType <ScriptedGripper>();
            }

            this._grasp            = this._graspable_object.GetOptimalGrasp(this._gripper).First;
            this._rigid_body       = this._grasp.GetComponentInParent <Rigidbody>();
            this._rigid_bodies     = this._graspable_object.GetComponentsInChildren <Rigidbody>();
            this._initial_position = this._rigid_body.transform.position;
            this._initial_rotation = this._rigid_body.transform.rotation;

            NeodroidUtilities.RegisterCollisionTriggerCallbacksOnChildren(
                this,
                this.transform,
                this.OnCollisionEnterChild,
                this.OnTriggerEnterChild,
                this.OnCollisionExitChild,
                this.OnTriggerExitChild,
                this.OnCollisionStayChild,
                this.OnTriggerStayChild,
                this._debugging);
        }
Beispiel #3
0
        void Start()
        {
            if (!this._gripper)
            {
                this._gripper = FindObjectOfType <ScriptedGripper>();
            }
            if (!this._target)
            {
                this._target = FindObjectOfType <Grasp>();
            }

            //print ("GPU supports depth format: " + SystemInfo.SupportsRenderTextureFormat (RenderTextureFormat.Depth));
            //print ("GPU supports shadowmap format: " + SystemInfo.SupportsRenderTextureFormat (RenderTextureFormat.Shadowmap));

            File.WriteAllText(this._file_path + this._file_path_gripper, "frame, x, y, z, rot_x, rot_y, rot_z\n");
            File.WriteAllText(this._file_path + this._file_path_target, "frame, x, y, z, rot_x, rot_y, rot_z\n");

            /*if (!File.Exists(_file_path + _file_path_pos_rot)) {
             * print("Created file/path: " + _file_path + _file_path_pos_rot);
             * //File.Create(_file_path + _file_path_pos_rot);
             * }
             * if (_deleteFileContent) {
             * File.WriteAllText(_file_path + _file_path_pos_rot, "");
             * _deleteFileContent = false;
             * }*/
        }
Beispiel #4
0
    Tuple<GraspableObject, Grasp> GetOptimalTargetAndGrasp() {
      var targets = FindObjectsOfType<GraspableObject>();
      if (targets.Length == 0) {
        return null;
      }

      var shortest_distance = float.MaxValue;
      GraspableObject optimal_target = null;
      Grasp optimal_grasp = null;
      foreach (var target in targets) {
        var pair = target.GetOptimalGrasp(this);
        if (pair == null || pair.Item1 == null || pair.Item1.IsObstructed()) {
          continue;
        }

        var target_grasp = pair.Item1;
        var distance = pair.Item2;
        if (distance < shortest_distance) {
          shortest_distance = distance;
          optimal_grasp = target_grasp;
          optimal_target = target;
        }
      }

      return new Tuple<GraspableObject, Grasp>(optimal_target, optimal_grasp);
    }
Beispiel #5
0
    public void FindTargetAndUpdatePath() {
      var pair = this.GetOptimalTargetAndGrasp();
      if (pair == null || pair.Item1 == null || pair.Item2 == null) {
        this._state.PathFindingState = PathFindingState.Returning_;
        this._path = this.FindPath(this.transform.position, this._reset_position);
        return;
      }

      this._target_game_object = pair.Item1;
      this._target_grasp = pair.Item2;
      this._approach_position = this._target_grasp.transform.position
                                - this._target_grasp.transform.forward * this._approach_distance;
      if (Vector3.Distance(this.transform.position, this._approach_position) > this._search_boundary) {
        return;
      }

      this._path = this.FindPath(this.transform.position, this._approach_position);
      this._intermediate_target = this._path.Next(this._step_size);
    }
Beispiel #6
0
        //Main function
        //return grip vector/transform with highest score
        public Pair <Grasp, float> GetOptimalGrasp(ScriptedGripper gripper)
        {
            var grasps = this.GetGrasps();
            var unobstructed_grasps = new List <Grasp>();

            foreach (var grasp in grasps)
            {
                if (!grasp.IsObstructed())
                {
                    unobstructed_grasps.Add(grasp);
                    this.ChangeIndicatorColor(grasp, Color.yellow);
                }
                else
                {
                    this.ChangeIndicatorColor(grasp, Color.red);
                }
            }

            Grasp optimal_grasp     = null;
            var   shortest_distance = float.MaxValue;

            foreach (var grasp in unobstructed_grasps)
            {
                var distance = Vector3.Distance(grasp.transform.position, gripper.transform.position);
                if (distance <= shortest_distance)
                {
                    shortest_distance = distance;
                    optimal_grasp     = grasp;
                }
            }

            if (optimal_grasp != null)
            {
                this.ChangeIndicatorColor(optimal_grasp, Color.green);
                return(new Pair <Grasp, float>(optimal_grasp, shortest_distance));
            }

            return(null);
        }
        // Use this for initialization
        void Start()
        {
            if (!this._graspable_object)
            {
                this._graspable_object = this.GetComponent <GraspableObject>();
            }

            if (!this._grasping)
            {
                this._grasping = FindObjectOfType <ScriptedGrasping>();
            }

            this._grasp        = this._graspable_object.GetOptimalGrasp(grasping: this._grasping).Item1;
            this._rigid_body   = this._grasp.GetComponentInParent <Rigidbody>();
            this._rigid_bodies = this._graspable_object.GetComponentsInChildren <Rigidbody>();
            var transform1 = this._rigid_body.transform;

            this._initial_position = transform1.position;
            this._initial_rotation = transform1.rotation;

            NeodroidRegistrationUtilities
            .RegisterCollisionTriggerCallbacksOnChildren <ChildCollider3DSensor, Collider, Collision>(caller: this,
                                                                                                      parent: this
                                                                                                      .transform,
                                                                                                      on_collision_enter_child: this
                                                                                                      .OnCollisionEnterChild,
                                                                                                      on_trigger_enter_child: this
                                                                                                      .OnTriggerEnterChild,
                                                                                                      on_collision_exit_child: this
                                                                                                      .OnCollisionExitChild,
                                                                                                      on_trigger_exit_child: this
                                                                                                      .OnTriggerExitChild,
                                                                                                      on_collision_stay_child: this
                                                                                                      .OnCollisionStayChild,
                                                                                                      on_trigger_stay_child: this
                                                                                                      .OnTriggerStayChild,
                                                                                                      debug: this
                                                                                                      ._debugging);
        }
Beispiel #8
0
 IEnumerator Schedule(Grasp chunk)
 {
     yield return new WaitForSeconds(chunk.Start);
     DebugManager.Instance.OnChunkStart(chunk);
     float duration = chunk.End;
     bool left = chunk.Mode == Behaviour.Lexemes.Mode.LEFT_HAND;
     ActorMotion.Arm which = left ? _motion.Left : _motion.Right;
     yield return StartCoroutine(Grasp(duration, chunk.Target, left));
     chunk.Callback(which);
 }
Beispiel #9
0
 public PickupResult(MoveItErrorCodes error_code, RobotState trajectory_start, RobotTrajectory[] trajectory_stages, string[] trajectory_descriptions, Grasp grasp, double planning_time)
 {
     this.error_code              = error_code;
     this.trajectory_start        = trajectory_start;
     this.trajectory_stages       = trajectory_stages;
     this.trajectory_descriptions = trajectory_descriptions;
     this.grasp         = grasp;
     this.planning_time = planning_time;
 }