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]); }