void ChangeIndicatorColor(Grasp grasp, Color color)
{
foreach (var child in grasp.GetComponentsInChildren <MeshRenderer>())
{
child.material.SetColor("_Color1", color);
}
}
// 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);
}
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;
* }*/
}
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);
}
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);
}
//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);
}
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);
}
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;
}
