Ejemplo n.º 1
0
        /// <summary>
        /// Initialize a motionPlanner instance for a specific robot.
        /// This instance is stored in the dictonary of the class which manages and maintains the instances of all motionPlanners of all robots.
        /// </summary>
        /// <param name="robot"></param>The VC robot instance for which a motionPlanner instance should be created
        /// <param name="pathToRobotUrdfDescriptionFile"></param>The URDF description of the robot
        /// <param name="kinChainStart"></param>The name of the first part in the kinematic chain.
        /// <param name="kinChainEnd"></param>The name of the last part in the kinematic chain.
        /// <param name="pathToObstacleFile"></param>The path to an obstacle model file (e.g. stl format).
        public MotionPlan InitializeMotionPlanner(IRobot robot, String pathToRobotUrdfDescriptionFile, String kinChainStart, String kinChainEnd, String pathToObstacleFile)
        {
            MotionPlan motionPlan = new MotionPlan(pathToRobotUrdfDescriptionFile, kinChainStart, kinChainEnd);

            if (!motionPlan.isValid())
            {
                Console.WriteLine("Failed to initialize motionPlan, unable to load robot urdf file \"" + pathToRobotUrdfDescriptionFile + "\" from " + kinChainStart + " to " + kinChainEnd + "!");
                return(null);
            }
            MotionPlanRobotDescription description = motionPlan.getMotionPlanRobotDescription();

            Vector3 wpr = robot.Component.TransformationInWorld.GetWPR();

            description.setRobotPosition(robot.Component.TransformationInWorld.GetP().X / 1000,
                                         robot.Component.TransformationInWorld.GetP().Y / 1000,
                                         robot.Component.TransformationInWorld.GetP().Z / 1000);
            description.setRobotRotation(wpr.X, wpr.Y, wpr.Z);

            motionPlan.addObstacle(pathToObstacleFile);

            return(motionPlan);
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Trigger and receive a concrete motionPlan from startFrame to goalFrame.
        /// Obstacles must be added to the motionPlanner instance of the robot before.
        /// </summary>
        /// <param name="robot"></param>The robot for which a motion should be planned.
        /// <param name="motionPlan"></param>The preconfigured motionPlan which should be used.
        /// <param name="startFrame"></param>The frame where the motion should start.
        /// <param name="goalFrame"></param>The frame where the motion should end.
        /// <returns></returns>
        public VectorOfDoubleVector planMotion(IRobot robot, MotionPlan motionPlan, String startFrame, String goalFrame)
        {
            MotionPlanRobotDescription description = motionPlan.getMotionPlanRobotDescription();

            // we need the current position of the robot to enhance the result of the inverse kinematics
            VectorOfDouble currentPositionJointAngles = new VectorOfDouble(robot.RobotController.Joints.Count);

            if (robot.RobotController.Joints.Count == 7)
            {
                currentPositionJointAngles.Add(robot.RobotController.Joints[0].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[1].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[6].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[2].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[3].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[4].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[5].Value);
            }
            else
            {
                currentPositionJointAngles.Add(robot.RobotController.Joints[0].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[1].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[2].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[3].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[4].Value);
                currentPositionJointAngles.Add(robot.RobotController.Joints[5].Value);
            }



            //TODO: Make those frames global?
            IFeature startNode = robot.Component.FindFeature(startFrame);

            if (startNode == null)
            {
                IoC.Get <IMessageService>().AppendMessage("Start Frame \"" + startFrame + "\" was not found.", MessageLevel.Error);
                return(null);
            }
            IFeature goalNode = robot.Component.FindFeature(goalFrame);

            if (goalNode == null)
            {
                IoC.Get <IMessageService>().AppendMessage("Goal Frame \"" + goalFrame + "\" was not found.", MessageLevel.Error);
                return(null);
            }


            Matrix         startPosition    = robot.Component.RootNode.GetFeatureTransformationInWorld(startNode);
            Matrix         goalPosition     = robot.Component.RootNode.GetFeatureTransformationInWorld(goalNode);
            Vector3        startRotation    = startPosition.GetWPR();
            Vector3        goalRotation     = goalPosition.GetWPR();
            Vector3        robotPosition    = robot.Component.TransformationInWorld.GetP();
            VectorOfDouble startJointAngles = currentPositionJointAngles;
            //IoC.Get<IMessageService>().AppendMessage("Goal Frame \"" + goalFrame + "\": [" + String.Format("{0:0.000}", (goalPosition.GetP().X / 1000)) + ", " + String.Format("{0:0.000}", (goalPosition.GetP().Y / 1000)) + ", " + String.Format("{0:0.000}", (goalPosition.GetP().Z / 1000)) + "] ["+ String.Format("{0:0.000}", goalRotation.X)+" "+ String.Format("{0:0.000}", goalRotation.Y)+" "+ String.Format("{0:0.000}", goalRotation.Z) + "]", MessageLevel.Warning);

            VectorOfDouble goalJointAngles = description.getIK(goalPosition.GetP().X / 1000,
                                                               goalPosition.GetP().Y / 1000,
                                                               goalPosition.GetP().Z / 1000,
                                                               goalRotation.X, goalRotation.Y, goalRotation.Z, startJointAngles, 0.5, "Distance");

            motionPlan.setStartPosition(startJointAngles);
            motionPlan.setGoalPosition(goalJointAngles);

            String startOut = "[", goalOut = "[";

            for (int i = 0; i < startJointAngles.Count; i++)
            {
                startOut += String.Format("{0:0.000}", startJointAngles[i]) + " ";
            }
            for (int i = 0; i < goalJointAngles.Count; i++)
            {
                goalOut += String.Format("{0:0.000}", goalJointAngles[i]) + " ";
            }
            startOut += "]";
            goalOut  += "]";

            motionPlan.setSolveTime(10.0);
            motionPlan.setStateValidityCheckingResolution(0.01);
            //motionPlan.setReportFirstExactSolution(true);
            motionPlan.setPlannerByString("RRTConnect");

            if (motionPlan.plan() > 0)
            {
                //IoC.Get<IMessageService>().AppendMessage("Found motion from " + startFrame + "@" + startOut + " to " + goalFrame + "@" + goalOut + ": ", MessageLevel.Warning);
                VectorOfDoubleVector plan = motionPlan.getLastResult();
                //motionPlan.showSetupInInspector();
                foreach (VectorOfDouble jointConfiguration in plan)
                {
                    String motionBuf = "[", sep = "";
                    foreach (double jointAngle in jointConfiguration)
                    {
                        motionBuf += sep + String.Format("{0:0.000}", jointAngle);
                        sep        = ",";
                    }

                    //IoC.Get<IMessageService>().AppendMessage(motionBuf + "]", MessageLevel.Warning);
                }

                //IoC.Get<IMessageService>().AppendMessage("Found motion END", MessageLevel.Warning);
                return(plan);
            }
            else
            {
                //motionPlan.showSetupInInspector();
                IoC.Get <IMessageService>().AppendMessage("Failed to find motion from " + startOut + " to " + goalOut + ": " + motionPlan.getLastPlanningError(), MessageLevel.Warning);
            }


            return(null);
        }