public Robot(Vector3Int _startingCell, int _currentlyAttached, int _startingStance, int _startingDirection)
{
legARailJoint = new RobotJoint(railBasicSpeed, legARailResetPos);
legAVerticalJoint = new RobotJoint(legAVerticalResetSpeed, legAVerticalResetPos);
legARotationJoint = new RobotJoint(legARotationResetSpeed, legARotationResetPos);
legBRailJoint = new RobotJoint(railBasicSpeed, legBRailResetPos);
legBVerticalJoint = new RobotJoint(legBVerticalResetSpeed, legBVerticalResetPos);
legBRotationJoint = new RobotJoint(legBRotationResetSpeed, legBRotationResetPos);
legCRailJoint = new RobotJoint(railBasicSpeed, legCRailResetPos);
legCRotationJoint = new RobotJoint(legCRotationResetSpeed, legCRotationResetPos);
legCGripJoint = new RobotJoint(legCGripResetSpeed, legCGripResetPos);
nozzleVerticalJoint = new RobotJoint(nozzleVerticalResetSpeed, nozzleVerticalResetPos);
nozzleRotationJoint = new RobotJoint(nozzleRotationResetSpeed, nozzleRotationResetPos);
nozzleMortarJoint = new RobotJoint(nozzleMortarResetSpeed, nozzleMortarResetPos);
allJoints.Add(legARailJoint);
allJoints.Add(legAVerticalJoint);
allJoints.Add(legARotationJoint);
allJoints.Add(legBRailJoint);
allJoints.Add(legBVerticalJoint);
allJoints.Add(legBRotationJoint);
allJoints.Add(legCRailJoint);
allJoints.Add(legCRotationJoint);
allJoints.Add(legCGripJoint);
allJoints.Add(nozzleVerticalJoint);
allJoints.Add(nozzleRotationJoint);
allJoints.Add(nozzleMortarJoint);
// set the short 'memory' of previous steps
steppingDown = false;
steppingUp = false;
previousStepLength = _startingStance;
brickCurrentlyCarried = noBrick; // set the brick
currentlyAttached = _currentlyAttached; // set the leg currently attached and 'spawn' the robot according to that position and rotation
if (currentlyAttached == legA)
{
leadingLeg = legB;
legAFootPos = new Vector3(_startingCell.x * gridXZDim, _startingCell.y * gridYDim, _startingCell.z * gridXZDim);
legAFootRot = Quaternion.Euler(0, _startingDirection, 0);
}
else
{
leadingLeg = legA;
legBFootPos = new Vector3(_startingCell.x * gridXZDim, _startingCell.y * gridYDim, _startingCell.z * gridXZDim);
legBFootRot = Quaternion.Euler(0, _startingDirection, 0);
}
currentDirection = _startingDirection;
normalDirection = true;
}
public void Generate()
{
Clean();
Transform[] bones = new Transform[nJoints];
Matrix4x4[] bindPoses = new Matrix4x4[nJoints];
Transform curr = transform;
curr.localPosition = Vector3.zero;
for (int i = 0; i < bones.Length; i++)
{
RobotJoint joint = Instantiate(bonePrefab);
joint.transform.GetChild(0).localScale = Vector3.one * sectionLength;
//TMP
switch (i % 3)
{
case 0:
joint.axis = Vector3.up;
break;
case 1:
joint.axis = Vector3.forward;
break;
case 2:
joint.axis = Vector3.right;
break;
}
bones[i] = joint.transform;
bones[i].name = "RobotJoint" + i;
bones[i].SetParent(curr, true);
bones[i].localRotation = curr.localRotation;
bones[i].localScale = curr.localScale;
bones[i].localPosition = curr.up * sectionLength;
bindPoses[i] = bones[i].worldToLocalMatrix * transform.localToWorldMatrix;
curr = bones[i];
}
renderer.bones = bones;
}
static void Main(string[] args)
{
//initialize robot joints
robotJoints[RIGHT_SHOULDER_PITCH] = new RobotJoint()
{
MaxAngle = 180
, MinAngle = 0
, InvertKinectAngle = false
, KinectAngleOffset = 0
, StartingAngle = 0
, Angle = 0
};
robotJoints[RIGHT_SHOULDER_YAW] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = true
,
KinectAngleOffset = 0
,
StartingAngle = 90
,
Angle = 90
};
robotJoints[RIGHT_ELBOW_YAW] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = false
,
KinectAngleOffset = 0
,
StartingAngle = 90
,
Angle = 90
};
robotJoints[RIGHT_ELBOW_PITCH] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = true
,
KinectAngleOffset = 0
,
StartingAngle = 90
,
Angle = 90
};
robotJoints[LEFT_SHOULDER_PITCH] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = true
,
KinectAngleOffset = 0
,
StartingAngle = 180
,
Angle = 180
};
robotJoints[LEFT_SHOULDER_YAW] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = false
,
KinectAngleOffset = 0
,
StartingAngle = 90
,
Angle = 90
};
robotJoints[LEFT_ELBOW_YAW] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = true
,
KinectAngleOffset = 0
,
StartingAngle = 90
,
Angle = 90
};
robotJoints[LEFT_ELBOW_PITCH] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = false
,
KinectAngleOffset = 0
,
StartingAngle = 90
,
Angle = 90
};
robotJoints[RIGHT_WRIST] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = false
,
KinectAngleOffset = 0
,
StartingAngle = 0
,
Angle = 0
};
robotJoints[RIGHT_GRIPPER] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = false
,
KinectAngleOffset = 0
,
StartingAngle = 0
,
Angle = 0
};
robotJoints[LEFT_GRIPPER] = new RobotJoint()
{
MaxAngle = 180
,
MinAngle = 0
,
InvertKinectAngle = false
,
KinectAngleOffset = 0
,
StartingAngle = 180
,
Angle = 180
};
for (var i = 0; i < 20; i++)
{
command_bytes[i] = 90;
}
command_bytes[LEFT_GRIPPER] = 90;
command_bytes[LEFT_ELBOW_PITCH] = 135;
command_bytes[LEFT_ELBOW_YAW] = 90;
command_bytes[LEFT_SHOULDER_PITCH] = 180;
command_bytes[LEFT_SHOULDER_YAW] = 90;
command_bytes[HEAD_PITCH] = 90;
command_bytes[HEAD_YAW] = 90;
command_bytes[RIGHT_WRIST] = 90;
command_bytes[RIGHT_GRIPPER] = 0;
command_bytes[RIGHT_ELBOW_PITCH] = 55;
command_bytes[RIGHT_ELBOW_YAW] = 90;
command_bytes[RIGHT_SHOULDER_PITCH] = 0;
command_bytes[RIGHT_SHOULDER_YAW] = 90;
command_bytes[RIGHT_WHEEL] = ConvertSignedIntToByte(0);
command_bytes[LEFT_WHEEL] = ConvertSignedIntToByte(0);
// Kinect sensor initialization
_sensor = KinectSensor.GetDefault();
if (_sensor != null)
{
_sensor.Open();
}
_reader = _sensor.OpenMultiSourceFrameReader(FrameSourceTypes.Color |
FrameSourceTypes.Depth |
FrameSourceTypes.Infrared |
FrameSourceTypes.Body);
_reader.MultiSourceFrameArrived += Reader_MultiSourceFrameArrived;
// Instatiate this class
new SerialPortProgram();
while (true)
{
try
{
command_bytes[RIGHT_SHOULDER_PITCH] = (byte)robotJoints[RIGHT_SHOULDER_PITCH].Angle;
command_bytes[RIGHT_SHOULDER_YAW] = (byte)robotJoints[RIGHT_SHOULDER_YAW].Angle;
command_bytes[RIGHT_ELBOW_YAW] = (byte)robotJoints[RIGHT_ELBOW_YAW].Angle;
command_bytes[RIGHT_ELBOW_PITCH] = (byte)robotJoints[RIGHT_ELBOW_PITCH].Angle;
command_bytes[LEFT_SHOULDER_PITCH] = (byte)robotJoints[LEFT_SHOULDER_PITCH].Angle;
command_bytes[LEFT_SHOULDER_YAW] = (byte)robotJoints[LEFT_SHOULDER_YAW].Angle;
command_bytes[LEFT_ELBOW_YAW] = (byte)robotJoints[LEFT_ELBOW_YAW].Angle;
command_bytes[LEFT_ELBOW_PITCH] = (byte)robotJoints[LEFT_ELBOW_PITCH].Angle;
command_bytes[RIGHT_WRIST] = (byte)(180 - robotJoints[RIGHT_SHOULDER_PITCH].Angle); //robotJoints[RIGHT_WRIST].Angle;
command_bytes[LEFT_WRIST] = (byte)(180 - robotJoints[LEFT_SHOULDER_PITCH].Angle); //robotJoints[RIGHT_WRIST].Angle;
command_bytes[LEFT_GRIPPER] = (byte)robotJoints[LEFT_GRIPPER].Angle;
command_bytes[RIGHT_WHEEL] = ConvertSignedIntToByte(right_wheel_speed);
command_bytes[LEFT_WHEEL] = ConvertSignedIntToByte(left_wheel_speed);
command_bytes[RIGHT_GRIPPER] = (byte)robotJoints[RIGHT_GRIPPER].Angle;
Console.Write(
"\rRWS:" + ((int)command_bytes[RIGHT_WHEEL] - 127)
+ "\tLSP:" + ((int)command_bytes[LEFT_WHEEL] - 127)
+ "\tLX:" + String.Format("{0:0.000}%", lean_x)
+ "\tLY:" + String.Format("{0:0.000}%", lean_y)
// + "\tREY:" + (int)command_bytes[RIGHT_ELBOW_YAW]
// + "\tREP:" + (int)command_bytes[RIGHT_ELBOW_PITCH]
// + "\tLSP:" + (int)command_bytes[LEFT_SHOULDER_PITCH]
// + "\tLSY:" + (int)command_bytes[LEFT_SHOULDER_YAW]
// + "\tLEY:" + (int)command_bytes[LEFT_ELBOW_YAW]
// + "\tLEP:" + (int)command_bytes[LEFT_ELBOW_PITCH]
+ "\t");
port.WriteLine("\\x" + BitConverter.ToString(command_bytes).Replace("-", "\\x"));
//Thread.Sleep(250);
}
catch (Exception e)
{
//Console.Write("\r" + e.Message + "\t\t\t\t");
port.Close();
port.Open();
}
}
}
private void Start()
{
urdfJoint = GetComponent <UrdfJoint>();
_robot_joint = GetComponent(typeof(RobotJoint)) as RobotJoint;
}
public JointInformation Set(int index, Vector3 jointPosition, Vector3 jointUp, Vector3 jointRight, RobotJoint jointRef)
{
jointsInfo[index].Position = jointPosition;
jointsInfo[index].UpDirection = jointUp;
jointsInfo[index].RightDirection = jointRight;
jointsInfo[index].jointRef = jointRef;
return(jointsInfo[index]);
}
