// debug for static arm collisions from collision listener
public void GetMidLevelArmCollisions()
{
IK_Robot_Arm_Controller arm = getArm();
CollisionListener collisionListener = arm.GetComponentInChildren <CollisionListener>();
if (collisionListener != null)
{
List <Dictionary <string, string> > collisions = new List <Dictionary <string, string> >();
foreach (var sc in collisionListener.StaticCollisions())
{
Dictionary <string, string> element = new Dictionary <string, string>();
if (sc.simObjPhysics != null)
{
element["objectType"] = "simObjPhysics";
element["name"] = sc.simObjPhysics.objectID;
}
else
{
element["objectType"] = "gameObject";
element["name"] = sc.gameObject.name;
}
collisions.Add(element);
}
actionFinished(true, collisions);
}
}
// currently not finished action. New logic needs to account for the
// hierarchy of rigidbodies of each arm joint and how to detect collision
// between a given arm joint an other arm joints.
public void RotateMidLevelHand(ServerAction action)
{
IK_Robot_Arm_Controller arm = getArm();
Quaternion target = new Quaternion();
// rotate around axis aliged x, y, z with magnitude based on vector3
if (action.degrees == 0)
{
// use euler angles
target = Quaternion.Euler(action.rotation);
}
else
{
// rotate action.degrees about axis
target = Quaternion.AngleAxis(action.degrees, action.rotation);
}
arm.rotateHand(
controller: this,
targetQuat: target,
degreesPerSecond: action.speed,
disableRendering: action.disableRendering,
fixedDeltaTime: action.fixedDeltaTime.GetValueOrDefault(Time.fixedDeltaTime),
returnToStartPositionIfFailed: action.returnToStart
);
}
public void ReleaseObject()
{
IK_Robot_Arm_Controller arm = getArm();
arm.DropObject();
// TODO: only return after object(s) dropped have finished moving
// currently this will return the frame the object is released
actionFinished(true);
}
// debug for static arm collisions from collision listener
public void DebugMidLevelArmCollisions()
{
IK_Robot_Arm_Controller arm = getArm();
List <CollisionListener.StaticCollision> scs = arm.collisionListener.StaticCollisions();
Debug.Log("Total current active static arm collisions: " + scs.Count);
foreach (CollisionListener.StaticCollision sc in scs)
{
Debug.Log("Arm static collision: " + sc.name);
}
actionFinished(true);
}
protected IK_Robot_Arm_Controller getArm()
{
IK_Robot_Arm_Controller arm = GetComponentInChildren <IK_Robot_Arm_Controller>();
if (arm == null)
{
throw new InvalidOperationException(
"Agent does not have kinematic arm or is not enabled.\n" +
$"Make sure there is a '{typeof(IK_Robot_Arm_Controller).Name}' component as a child of this agent."
);
}
return(arm);
}
// note this does not reposition the center point of the magnet orb
// so expanding the radius too much will cause it to clip backward into the wrist joint
public void SetHandSphereRadius(float radius)
{
if (radius < 0.04f || radius > 0.5f)
{
throw new ArgumentOutOfRangeException(
$"radius={radius} of hand cannot be less than 0.04m nor greater than 0.5m"
);
}
IK_Robot_Arm_Controller arm = getArm();
arm.SetHandSphereRadius(radius);
actionFinished(true);
}
private static void continuousMoveFinish <T>(
PhysicsRemoteFPSAgentController controller,
CollisionListener collisionListener,
Transform moveTransform,
System.Action <Transform, T> setProp,
T target,
T resetProp
)
{
bool actionSuccess = true;
string debugMessage = "";
IK_Robot_Arm_Controller arm = controller.GetComponentInChildren <IK_Robot_Arm_Controller>();
var staticCollisions = collisionListener.StaticCollisions();
if (staticCollisions.Count > 0)
{
var sc = staticCollisions[0];
// decide if we want to return to original property or last known property before collision
setProp(moveTransform, resetProp);
// if we hit a sim object
if (sc.isSimObj)
{
debugMessage = "Collided with static sim object: '" + sc.simObjPhysics.name + "', could not reach target: '" + target + "'.";
}
// if we hit a structural object that isn't a sim object but still has static collision
if (!sc.isSimObj)
{
debugMessage = "Collided with static structure in scene: '" + sc.gameObject.name + "', could not reach target: '" + target + "'.";
}
actionSuccess = false;
}
controller.errorMessage = debugMessage;
controller.actionFinished(actionSuccess, debugMessage);
}
public void MoveArm(
Vector3 position,
float speed = 1,
float?fixedDeltaTime = null,
bool returnToStart = true,
string coordinateSpace = "armBase",
bool restrictMovement = false,
bool disableRendering = true
)
{
IK_Robot_Arm_Controller arm = getArm();
arm.moveArmTarget(
controller: this,
target: position,
unitsPerSecond: speed,
fixedDeltaTime: fixedDeltaTime.GetValueOrDefault(Time.fixedDeltaTime),
returnToStart: returnToStart,
coordinateSpace: coordinateSpace,
restrictTargetPosition: restrictMovement,
disableRendering: disableRendering
);
}
// constrain arm's y position based on the agent's current capsule collider center and extents
// valid Y height from action.y is [0, 1.0] to represent the relative min and max heights of the
// arm constrained by the agent's capsule
public void MoveArmBase(
float y,
float speed = 1,
float?fixedDeltaTime = null,
bool returnToStart = true,
bool disableRendering = true
)
{
if (y < 0 || y > 1)
{
throw new ArgumentOutOfRangeException($"y={y} value must be [0, 1.0].");
}
IK_Robot_Arm_Controller arm = getArm();
arm.moveArmBase(
controller: this,
height: y,
unitsPerSecond: speed,
fixedDeltaTime: fixedDeltaTime.GetValueOrDefault(Time.fixedDeltaTime),
returnToStartPositionIfFailed: returnToStart,
disableRendering: disableRendering
);
}
public void moveArmTarget(
PhysicsRemoteFPSAgentController controller,
Vector3 target,
float unitsPerSecond,
float fixedDeltaTime = 0.02f,
bool returnToStart = false,
string coordinateSpace = "arm",
bool restrictTargetPosition = false,
bool disableRendering = false
)
{
// clearing out colliders here since OnTriggerExit is not consistently called in Editor
collisionListener.Reset();
IK_Robot_Arm_Controller arm = this;
// Move arm based on hand space or arm origin space
// Vector3 targetWorldPos = handCameraSpace ? handCameraTransform.TransformPoint(target) : arm.transform.TransformPoint(target);
Vector3 targetWorldPos = Vector3.zero;
switch (coordinateSpace)
{
case "world":
// world space, can be used to move directly toward positions
// returned by sim objects
targetWorldPos = target;
break;
case "wrist":
// space relative to base of the wrist, where the camera is
targetWorldPos = handCameraTransform.TransformPoint(target);
break;
case "armBase":
// space relative to the root of the arm, joint 1
targetWorldPos = arm.transform.TransformPoint(target);
break;
default:
throw new ArgumentException("Invalid coordinateSpace: " + coordinateSpace);
}
// TODO Remove this after restrict movement is finalized
Vector3 targetShoulderSpace = (this.transform.InverseTransformPoint(targetWorldPos) - new Vector3(0, 0, originToShoulderLength));
#if UNITY_EDITOR
Debug.Log(
$"pos target {target} world {targetWorldPos} remaining {targetShoulderSpace.z}\n" +
$"magnitude {targetShoulderSpace.magnitude} extendedArmLength {extendedArmLength}"
);
#endif
if (restrictTargetPosition && !validArmTargetPosition(targetWorldPos))
{
targetShoulderSpace = (
this.transform.InverseTransformPoint(targetWorldPos)
- new Vector3(0, 0, originToShoulderLength)
);
throw new InvalidOperationException(
$"Invalid target: Position '{target}' in space '{coordinateSpace}' is behind shoulder."
);
}
Vector3 originalPos = armTarget.position;
Vector3 targetDirectionWorld = (targetWorldPos - originalPos).normalized;
IEnumerator moveCall = ContinuousMovement.move(
controller,
collisionListener,
armTarget,
targetWorldPos,
disableRendering ? fixedDeltaTime : Time.fixedDeltaTime,
unitsPerSecond,
returnToStart,
false
);
if (disableRendering)
{
controller.unrollSimulatePhysics(
moveCall,
fixedDeltaTime
);
}
else
{
StartCoroutine(
moveCall
);
}
}
// perhaps this should fail if no object is picked up?
// currently action success happens as long as the arm is
// enabled because it is a successful "attempt" to pickup something
public void PickupObject(List <string> objectIdCandidates = null)
{
IK_Robot_Arm_Controller arm = getArm();
actionFinished(arm.PickupObject(objectIdCandidates, ref errorMessage), errorMessage);
}
