Ejemplo n.º 1
0
 protected override void OnAwake()
 {
     type      = ICLOiSimPlugin.Type.REALSENSE;
     cameras   = GetComponentsInChildren <SensorDevices.Camera>();
     imu       = GetComponentInChildren <SensorDevices.IMU>();
     partsName = name;
 }
    protected override void OnAwake()
    {
        micomInput  = gameObject.AddComponent <MicomInput>();
        micomSensor = gameObject.AddComponent <MicomSensor>();

        imuSensor = gameObject.GetComponentInChildren <SensorDevices.IMU>();
    }
Ejemplo n.º 3
0
 protected override void OnAwake()
 {
     type = ICLOiSimPlugin.Type.IMU;
     imu  = gameObject.GetComponent <SensorDevices.IMU>();
     attachedDevices.Add("IMU", imu);
     partsName = DeviceHelper.GetPartName(gameObject);
 }
Ejemplo n.º 4
0
    protected override void OnStart()
    {
        var updateRate = GetPluginParameters().GetValue <float>("update_rate", 20);

        SetUpdateRate(updateRate);

        var kp = GetPluginParameters().GetValue <float>("PID/kp");
        var ki = GetPluginParameters().GetValue <float>("PID/ki");
        var kd = GetPluginParameters().GetValue <float>("PID/kd");

        var pidControl = new PID(kp, ki, kd);

        wheelBase         = GetPluginParameters().GetValue <float>("wheel/base");
        wheelRadius       = GetPluginParameters().GetValue <float>("wheel/radius");
        divideWheelRadius = 1.0f / wheelRadius;

        var wheelNameLeft  = GetPluginParameters().GetValue <string>("wheel/location[@type='left']");
        var wheelNameRight = GetPluginParameters().GetValue <string>("wheel/location[@type='right']");

        var motorFriction = GetPluginParameters().GetValue <float>("wheel/friction/motor", 0.1f);        // Currently not used
        var brakeFriction = GetPluginParameters().GetValue <float>("wheel/friction/brake", 0.1f);        // Currently not used

        var modelList = GetComponentsInChildren <ModelPlugin>();

        foreach (var model in modelList)
        {
            // Debug.Log(model.name);
            if (model.name.Equals(wheelNameLeft))
            {
                var jointWheelLeft = model.GetComponentInChildren <HingeJoint>();
                motorLeft = new Motor("Left", jointWheelLeft, pidControl);

                // var wheelLeftBody = jointWheelLeft.gameObject.GetComponent<Rigidbody>();
                var wheelLeftTransform = jointWheelLeft.gameObject.transform.parent;
                var wheelLeftPose      = new Pose(wheelLeftTransform.localPosition, wheelLeftTransform.localRotation);
                partsPoseMapTable.Add(wheelNameLeft, wheelLeftPose);

                // Debug.LogFormat("joint Wheel({0}) Left max angular velocity({1})", jointWheelLeft.name, jointWheelLeft.gameObject.GetComponent<Rigidbody>().maxAngularVelocity);
            }
            else if (model.name.Equals(wheelNameRight))
            {
                var jointWheelRight = model.GetComponentInChildren <HingeJoint>();
                motorRight = new Motor("Right", jointWheelRight, pidControl);

                var wheelRightTransform = jointWheelRight.gameObject.transform.parent;
                var wheelRightPose      = new Pose(wheelRightTransform.localPosition, wheelRightTransform.localRotation);
                partsPoseMapTable.Add(wheelNameRight, wheelRightPose);

                // Debug.LogFormat("joint Wheel({0}) Left max angular velocity({1})", jointWheelRight.name, jointWheelRight.gameObject.GetComponent<Rigidbody>().maxAngularVelocity);
            }

            if (motorLeft != null && motorRight != null)
            {
                break;
            }
        }

        if (GetPluginParameters().GetValues <string>("uss/sensor", out var ussList))
        {
            foreach (var uss in ussList)
            {
                foreach (var model in modelList)
                {
                    if (model.name.Equals(uss))
                    {
                        var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
                        ussSensors.Add(sonarSensor);
                        // Debug.Log("ussSensor found : " + sonarSensor.name);
                    }
                }
            }
            micomSensorData.uss.Distances = new double[ussList.Count];
        }

        if (GetPluginParameters().GetValues <string>("ir/sensor", out var irList))
        {
            foreach (var ir in irList)
            {
                foreach (var model in modelList)
                {
                    if (model.name.Equals(ir))
                    {
                        var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
                        irSensors.Add(sonarSensor);
                        // Debug.Log("irSensor found : " + sonarSensor.name);
                    }
                }
            }
            micomSensorData.ir.Distances = new double[irList.Count];
        }

        if (GetPluginParameters().GetValues <string>("magnet/sensor", out var magnetList))
        {
            foreach (var model in modelList)
            {
                // TODO: to be implemented
            }
        }

        var targetContactName = GetPluginParameters().GetAttribute <string>("bumper", "contact");
        // Debug.Log(targetContactName);

        var contactsInChild = GetComponentsInChildren <SensorDevices.Contact>();

        foreach (var contact in contactsInChild)
        {
            if (contact.name.Equals(targetContactName))
            {
                bumperContact = contact;
                // Debug.Log("Found");
            }
        }

        if (bumperContact != null)
        {
            if (GetPluginParameters().GetValues <string>("bumper/joint_name", out var bumperJointNameList))
            {
                var linkList = GetComponentsInChildren <LinkPlugin>();
                foreach (var link in linkList)
                {
                    foreach (var bumperJointName in bumperJointNameList)
                    {
                        if (link.jointList.TryGetValue(bumperJointName, out var jointValue))
                        {
                            bumperSensors.Add(jointValue as ConfigurableJoint);
                            Debug.Log(bumperJointName);
                        }
                    }
                }
            }

            var bumperCount = (bumperSensors == null || bumperSensors.Count == 0) ? 1 : bumperSensors.Count;

            micomSensorData.bumper.Bumpeds = new bool[bumperCount];
        }

        imuSensor = gameObject.GetComponentInChildren <SensorDevices.IMU>();

        if (imuSensor != null)
        {
            var imuSensorModelTransform = imuSensor.transform.parent;
            var newPose = new Pose(imuSensorModelTransform.localPosition, imuSensorModelTransform.localRotation);
            partsPoseMapTable.Add(imuSensor.name, newPose);
        }
    }
Ejemplo n.º 5
0
    protected override void OnStart()
    {
        var updateRate = GetPluginParameters().GetValue <float>("update_rate", 20);

        SetUpdateRate(updateRate);

        _PGain = GetPluginParameters().GetValue <float>("PID/kp");
        _IGain = GetPluginParameters().GetValue <float>("PID/ki");
        _DGain = GetPluginParameters().GetValue <float>("PID/kd");

        wheelBase         = GetPluginParameters().GetValue <float>("wheel/base");
        wheelRadius       = GetPluginParameters().GetValue <float>("wheel/radius");
        divideWheelRadius = 1.0f / wheelRadius;

        _wheelNameLeft  = GetPluginParameters().GetValue <string>("wheel/location[@type='left']");
        _wheelNameRight = GetPluginParameters().GetValue <string>("wheel/location[@type='right']");

        var motorFriction = GetPluginParameters().GetValue <float>("wheel/friction/motor", 0.1f);        // Currently not used
        var brakeFriction = GetPluginParameters().GetValue <float>("wheel/friction/brake", 0.1f);        // Currently not used

        var modelList = GetComponentsInChildren <SDF.Helper.Model>();

        foreach (var model in modelList)
        {
            // Debug.Log(model.name);
            if (model.name.Equals(_wheelNameLeft) || model.name.Equals(_wheelNameRight))
            {
                var motorObject = model.gameObject;
                var motor       = gameObject.AddComponent <Motor>();
                motor.SetTargetJoint(motorObject);
                motor.SetPID(_PGain, _IGain, _DGain);
                _motors.Add(model.name, motor);

                SetPartsInitialPose(model.name, motorObject);
            }
        }

        if (GetPluginParameters().GetValues <string>("uss/sensor", out var ussList))
        {
            foreach (var uss in ussList)
            {
                foreach (var model in modelList)
                {
                    if (model.name.Equals(uss))
                    {
                        var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
                        ussSensors.Add(sonarSensor);
                        // Debug.Log("ussSensor found : " + sonarSensor.name);
                    }
                }
            }
            micomSensorData.uss.Distances = new double[ussList.Count];
        }

        if (GetPluginParameters().GetValues <string>("ir/sensor", out var irList))
        {
            foreach (var ir in irList)
            {
                foreach (var model in modelList)
                {
                    if (model.name.Equals(ir))
                    {
                        var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
                        irSensors.Add(sonarSensor);
                        // Debug.Log("irSensor found : " + sonarSensor.name);
                    }
                }
            }
            micomSensorData.ir.Distances = new double[irList.Count];
        }

        if (GetPluginParameters().GetValues <string>("magnet/sensor", out var magnetList))
        {
            foreach (var model in modelList)
            {
                // TODO: to be implemented
            }
        }

        var targetContactName = GetPluginParameters().GetAttribute <string>("bumper", "contact");
        // Debug.Log(targetContactName);

        var contactsInChild = GetComponentsInChildren <SensorDevices.Contact>();

        foreach (var contact in contactsInChild)
        {
            if (contact.name.Equals(targetContactName))
            {
                bumperContact = contact;
                // Debug.Log("Found");
            }
        }

        if (bumperContact != null)
        {
            if (GetPluginParameters().GetValues <string>("bumper/joint_name", out var bumperJointNameList))
            {
                var linkList = GetComponentsInChildren <SDF.Helper.Link>();
                foreach (var link in linkList)
                {
                    foreach (var bumperJointName in bumperJointNameList)
                    {
                        if (link.jointList.TryGetValue(bumperJointName, out var articulationBody))
                        {
                            if (articulationBody.jointType == ArticulationJointType.PrismaticJoint)
                            {
                                bumperSensors.Add(articulationBody);
                                Debug.Log(bumperJointName);
                            }
                            else
                            {
                                Debug.Log(bumperJointName + " is not a prismatic joint type!!!");
                            }
                        }
                    }
                }
            }

            var bumperCount = (bumperSensors == null || bumperSensors.Count == 0) ? 1 : bumperSensors.Count;

            micomSensorData.bumper.Bumpeds = new bool[bumperCount];
        }

        imuSensor = gameObject.GetComponentInChildren <SensorDevices.IMU>();

        if (imuSensor != null)
        {
            SetPartsInitialPose(imuSensor.name, imuSensor.gameObject);
        }
    }