private Node GetOrCreateConstraint(agx.Constraint constraint)
{
return(GetOrCreateNode(NodeType.Constraint,
constraint.getUuid(),
true,
() => m_constraints.Add(constraint.getUuid(), constraint)));
}
public static void AddToAssembly(agx.Constraint joint)
{
if (robotAssembly == null)
{
robotAssembly = new agxSDK.Assembly();
}
robotAssembly.add(joint);
}
/// <summary>
/// Finds constraint type given native instance.
/// </summary>
/// <param name="native">Native instance.</param>
/// <returns>ConstraintType of the native instance.</returns>
public static ConstraintType FindType(agx.Constraint native)
{
return(native == null ? ConstraintType.Unknown :
native.asHinge() != null ? ConstraintType.Hinge :
native.asPrismatic() != null ? ConstraintType.Prismatic :
native.asLockJoint() != null ? ConstraintType.LockJoint :
native.asCylindricalJoint() != null ? ConstraintType.CylindricalJoint :
native.asBallJoint() != null ? ConstraintType.BallJoint :
native.asDistanceJoint() != null ? ConstraintType.DistanceJoint :
native.asAngularLockJoint() != null ? ConstraintType.AngularLockJoint :
native.asPlaneJoint() != null ? ConstraintType.PlaneJoint :
ConstraintType.Unknown);
}
private void Gripper(bool grip)
{
if (GrippingBody == null || FollowBody == null || waitForKeyUp)
{
return;
}
if (grip)
{
GrippedBody.SyncNativeTransform();
GrippingBody.SyncNativeTransform(); // Is this needed...?
if (Vector3.Distance(GrippedBody.transform.position, GrippingBody.transform.position) > MaxGripDistance)
{
Debug.Log("Couldn't reach to grip: " + (GrippedBody.transform.position - GrippingBody.transform.position).magnitude + ", pos1: " + GrippedBody.transform.position + ", pos2: " + GrippingBody.transform.position);
return;
}
if (lockJoint == null)
{
previouslyClose = false;
agx.Frame grippedFrame = new agx.Frame();
agx.Frame grippingFrame = new agx.Frame();
Vector3 offset = GrippingBody.transform.rotation * grippingPositionOffset;
grippedFrame.setLocalTranslate(GrippedBody.Native.getCmLocalTranslate());
grippingFrame.setLocalTranslate(new agx.Vec3(offset.x, offset.y, offset.z));
grippedFrame.setLocalRotate(new agx.EulerAngles(grippingRotationOffset.x * Mathf.Deg2Rad, grippingRotationOffset.y * Mathf.Deg2Rad, grippingRotationOffset.z * Mathf.Deg2Rad));
lockJoint = new agx.LockJoint(GrippedBody.Native, grippedFrame, GrippingBody.Native, grippingFrame);
lockJoint.setCompliance(0.00001f);
lockJoint.setForceRange(new agx.RangeReal(-200, 200));
lockJoint.setDamping(1);
GetSimulation().add(lockJoint);
}
}
else
{
if (lockJoint != null)
{
lockJoint.setEnable(false);
lockJoint = null;
VeryCloseEvent(false);
}
}
gripped = !gripped;
}
/// <summary>
/// Internal method which constructs this constraint given elementary constraints
/// in the native instance. Throws if an elementary constraint fails to initialize.
/// </summary>
/// <param name="native">Native instance.</param>
/// <param name="onObjectCreated">Optional callback when elementary constraint has been created.</param>
public void TryAddElementaryConstraints(agx.Constraint native,
Action <UnityEngine.Object> onObjectCreated = null)
{
if (native == null)
{
throw new ArgumentNullException("native", "Native constraint is null.");
}
m_elementaryConstraints.Clear();
for (uint i = 0; i < native.getNumElementaryConstraints(); ++i)
{
if (native.getElementaryConstraint(i).getName() == "")
{
throw new Exception("Native elementary constraint doesn't have a name.");
}
var ec = ElementaryConstraint.Create(gameObject, native.getElementaryConstraint(i));
if (ec == null)
{
throw new Exception("Failed to configure elementary constraint with name: " + native.getElementaryConstraint(i).getName() + ".");
}
onObjectCreated?.Invoke(ec);
m_elementaryConstraints.Add(ec);
}
for (uint i = 0; i < native.getNumSecondaryConstraints(); ++i)
{
if (native.getSecondaryConstraint(i).getName() == "")
{
throw new Exception("Native secondary constraint doesn't have a name.");
}
var sc = ElementaryConstraint.Create(gameObject, native.getSecondaryConstraint(i));
if (sc == null)
{
throw new Exception("Failed to configure elementary controller constraint with name: " + native.getElementaryConstraint(i).getName() + ".");
}
onObjectCreated?.Invoke(sc);
m_elementaryConstraints.Add(sc);
}
}
public void Create_Hinge(string type, AgX_Frame left, AgX_Frame right, double lowerLimit, double upperLimit)
{
this.type = type;
//Hinge is locked between the two objects.
hinge_Frame.setCenter((left.GetAgxObject().getPosition() + right.GetAgxObject().getPosition()).Divide(2));
if (left.GetRotation().x == 0) //pitch or yaw module
{
hinge_Frame.setAxis(new agx.Vec3(1, 0, 0)); //axis along the x direction
}
else
{
hinge_Frame.setAxis(new agx.Vec3(0, 1, 0)); //axis along the x direction
}
joint = new agx.Hinge(hinge_Frame, left.GetAgxObject(), right.GetAgxObject());
joint.asHinge().getMotor1D().setEnable(true);
joint.asHinge().getRange1D().setEnable(true);
joint.asHinge().getRange1D().setRange(lowerLimit, upperLimit);
}
public void Create_Hinge(string type, AgX_Frame left, AgX_Frame right, double leftLimit, double rightLimit)
{
this.type = type;
//Hinge is locked between the two objects.
hinge_Frame.setCenter((left.GetAgxObject().getPosition() + right.GetAgxObject().getPosition()).Divide(2));
if (left.Get_Rotation().x == 0)
{
hinge_Frame.setAxis(new agx.Vec3(1, 0, 0)); //axis along the x direction
//UnityEngine.Debug.Log("z in agxint: " + left.Get_Rotation().x);
}
else
{
hinge_Frame.setAxis(new agx.Vec3(0, 1, 0)); //axis along the x direction
}
Joint = new agx.Hinge(hinge_Frame, left.GetAgxObject(), right.GetAgxObject());
//Joint.asHinge().getLock1D().setEnable(true);
Joint.asHinge().getMotor1D().setEnable(true);
Joint.asHinge().getRange1D().setEnable(true);
//Might want to have this as a modifyable parameter:
Joint.asHinge().getRange1D().setRange(leftLimit, rightLimit /*-Math.PI / 2, Math.PI / 2*/);
//Joint.asHinge().getMotor1D().setSpeed(0.2f);
}
public static bool IsValid(agx.Constraint constraint)
{
return(constraint != null &&
constraint.getNumBodies() > 0ul);
}
private void handleKeyboard()
{
bool steering_key_down = false;
// Handle keyboard
if (Input.GetKey(KeyUp))
{
if (!m_motor_key_forward_down)
{
m_vehicle.forward();
}
m_motor_key_forward_down = true;
}
else
{
m_motor_key_forward_down = false;
}
if (Input.GetKey(KeyDown))
{
if (!m_motor_key_backward_down)
{
m_vehicle.reverse();
}
m_motor_key_backward_down = true;
}
else
{
m_motor_key_backward_down = false;
}
if (Input.GetKey(KeyLeft))
{
steering_key_down = true;
updateForceRange();
m_steeringHinge.getMotor1D().setSpeed(-SteeringSpeed);
}
if (Input.GetKey(KeyRight))
{
steering_key_down = true;
updateForceRange();
m_steeringHinge.getMotor1D().setSpeed(SteeringSpeed);
}
if (!steering_key_down)
{
m_steeringHinge.getMotor1D().setSpeed(0);
}
if (!m_motor_key_backward_down && !m_motor_key_forward_down)
{
m_vehicle.brake(true);
}
bool fast = Input.GetKey(KeyCode.LeftShift);
if (m_motor_key_forward_down && fast)
{
m_vehicle.Gearbox.setGear(3);
}
if (Input.GetKey(KeyReset) && !m_resetKeyDown)
{
m_resetKeyDown = true;
var pos = m_chassieRigidBody.getPosition();
m_moveBody = new agx.RigidBody();
GetSimulation().add(m_moveBody);
m_moveBody.setMotionControl(agx.RigidBody.MotionControl.KINEMATICS);
m_moveBody.setPosition(pos + new agx.Vec3(0, 2, 0));
m_moveBody.setVelocity(0, 2, 0);
var f1 = new agx.Frame();
var f2 = new agx.Frame();
agx.Constraint.calculateFramesFromWorld(pos, new agx.Vec3(0, 1, 0), m_moveBody, f1, m_chassieRigidBody, f2);
m_moveConstraint = new agx.BallJoint(m_moveBody, f1, m_chassieRigidBody, f2);
GetSimulation().add(m_moveConstraint);
}
else if (m_resetKeyDown)
{
GetSimulation().remove(m_moveBody);
GetSimulation().remove(m_moveConstraint);
m_resetKeyDown = false;
}
}
public void DistanceSensorLock(AgX_Primitive sm, AgX_Primitive ds, Vector3 lockPosition)
{
joint = new agx.LockJoint(sm.GetAgxObject(), ds.GetAgxObject(), Operations.ToAgxVec3(lockPosition));
joint.setEnableComputeForces(true);
}
public void Create_Lock(string type, AgX_Primitive s_mod, AgX_Frame left)
{
//THIS IS NOT THE MIDDLE OF THE LOCK FRAME (frames are longer than sensors)
joint = new agx.LockJoint(s_mod.GetAgxObject(), left.GetAgxObject(), (left.GetAgxObject().getPosition() + s_mod.GetAgxObject().getPosition()).Divide(2));
}
//Sensory module locks:
public void Create_Lock(string type, AgX_Frame right, AgX_Primitive s_mod)
{
//Creates a joint with a specified middle position for the lockframe.
//THIS IS NOT THE MIDDLE OF THE LOCK FRAME (frames are longer than sensors)
joint = new agx.LockJoint(right.GetAgxObject(), s_mod.GetAgxObject(), (right.GetAgxObject().getPosition() + s_mod.GetAgxObject().getPosition()).Divide(2));
}
public void Create_Lock(string type, AgX_Frame left, AgX_Frame right)
{
//connects right frame of left robot (LEFT) to left frame of right robot (RIGHT)
joint = new agx.LockJoint(left.GetAgxObject(), right.GetAgxObject(), (left.GetAgxObject().getPosition() + right.GetAgxObject().getPosition()).Divide(2));
}
protected override bool Initialize()
{
RemoveInvalidRollers();
var tracks = FindTracks();
if (tracks.Length == 0)
{
Debug.LogWarning("Belt: No tracks found.", this);
return(false);
}
if (tracks.Any(track => track.GetInitialized <Track>() == null) ||
// Tracks has zero nodes when the license isn't loaded or
// doesn't include the tracks module.
tracks.Any(Track => Track.Native.getNumNodes() == 0))
{
Debug.LogError("Belt: One or more tracks failed to initialize.", this);
return(false);
}
m_tracks = tracks;
if (m_tracks.Length > 1)
{
ulong numNodes = m_tracks.First().Native.getNumNodes();
ulong stride = System.Math.Min(System.Math.Max((ulong)ConnectingConstraintStride, 1),
numNodes);
while (stride > 1 && (numNodes % stride) != 0)
{
--stride;
}
if ((int)stride != ConnectingConstraintStride)
{
Debug.Log($"Belt: Connecting constraint stride changed from {ConnectingConstraintStride} to {stride} " +
$"to match node count: {m_tracks.First().Native.getNumNodes()}.", this);
}
for (int i = 0; i < m_tracks.Length; ++i)
{
m_tracks[i].Native.addGroup(GetGroupName(i));
}
ConnectingConstraints = new agx.Constraint[m_tracks.Length - 1, (int)(numNodes / stride)];
for (int i = 1; i < m_tracks.Length; ++i)
{
var prev = m_tracks[i - 1];
var curr = m_tracks[i];
GetSimulation().getSpace().setEnablePair(GetGroupName(i - 1),
GetGroupName(i),
false);
var connectingConstraintIndex = 0;
for (ulong nodeIndex = 0; nodeIndex < prev.Native.getNumNodes(); ++nodeIndex)
{
if ((nodeIndex % stride) != 0)
{
continue;
}
var prevTrackNode = prev.Native.getNode(nodeIndex);
var currTrackNode = curr.Native.getNode(nodeIndex);
var constraintCenter = 0.5 * (prevTrackNode.getCenterPosition() +
currTrackNode.getCenterPosition());
var constraintAxis = prevTrackNode.getDirection().normal();
var otherAxis = (constraintCenter - prevTrackNode.getCenterPosition()).normal();
// Constraint frame we're about to create:
// x or TRANSLATIONAL_1: Along otherAxis, i.e., separation of the two nodes.
// y or TRANSLATIONAL_2: Up/down between the two nodes.
// z or TRANSLATIONAL_3: Along the track.
var prevTrackNodeFrame = new agx.Frame();
var currTrackNodeFrame = new agx.Frame();
agx.Constraint.calculateFramesFromWorld(constraintCenter,
constraintAxis,
otherAxis,
prevTrackNode.getRigidBody(),
prevTrackNodeFrame,
currTrackNode.getRigidBody(),
currTrackNodeFrame);
ConnectingConstraints[i - 1, connectingConstraintIndex++] = new agx.Hinge(prevTrackNode.getRigidBody(),
prevTrackNodeFrame,
currTrackNode.getRigidBody(),
currTrackNodeFrame);
}
}
foreach (var constraint in ConnectingConstraints)
{
if (constraint == null)
{
Debug.LogWarning("Unexpected null connecting constraint.", this);
continue;
}
constraint.setName("connecting");
// Properties will be applied during BeltProperties.Register
// when all properties are synchronized for this belt after
// Initialize is done.
GetSimulation().add(constraint);
}
}
return(true);
}
