コード例 #1
0
 // Calculates the axis from a Reference Axis in the model
 public void estimateAxis(joint Joint)
 {
     Joint.Axis.xyz = estimateAxis(Joint.AxisName);
 }
コード例 #2
0
 //This method adds in the limits from a limit mate, to make a joint a revolute joint. It really needs to checked for correctness.
 public void addLimits(joint Joint, List<Mate2> limitMates)
 {
     // The number of limit Mates should only be one. But for completeness, I cycle through every found limit mate.
     foreach (Mate2 swMate in limitMates)
     {
         // [TODO] This assumes the limit mate limits the right degree of freedom, it really should check that assumption
         if ((Joint.type == "continuous" && swMate.Type == (int)swMateType_e.swMateANGLE) ||
             (Joint.type == "prismatic" && swMate.Type == (int)swMateType_e.swMateDISTANCE))
         {
             Joint.Limit = new limit();
             Joint.Limit.upper = swMate.MaximumVariation; // Lucky me that no conversion is necessary
             Joint.Limit.lower = swMate.MinimumVariation;
             if (Joint.type == "continuous")
             {
                 Joint.type = "revolute";
             }
         }
     }
 }
コード例 #3
0
 // Creates a Reference Axis to be used to calculate the joint axis
 public void createRefAxis(joint Joint)
 {
     //Adds sketch segment
     SketchSegment rotaxis = addSketchGeometry(Joint.Axis, Joint.Origin);
     if (rotaxis != null)
     {
         //Use special method to create the axis
         Feature featAxis = insertAxis(rotaxis);
         if (featAxis != null)
         {
             featAxis.Name = Joint.AxisName;
         }
     }
 }
コード例 #4
0
        // Takes a links joint and calculates the local transform from the global transforms of the parent and child. It also converts the
        // axis to local values
        public void localizeJoint(joint Joint, string parentCoordsysName)
        {
            MathTransform parentTransform = getCoordinateSystemTransform(parentCoordsysName);
            double[] parentRPY = ops.getRPY(parentTransform);


            Matrix<double> ParentJointGlobalTransform = ops.getTransformation(parentTransform);
            MathTransform coordsysTransform = getCoordinateSystemTransform(Joint.CoordinateSystemName);
            double[] coordsysRPY = ops.getRPY(coordsysTransform);

            //Transform from global origin to child joint
            Matrix<double> ChildJointGlobalTransform = ops.getTransformation(coordsysTransform);
            Matrix<double> ChildJointOrigin = ParentJointGlobalTransform.Inverse() * ChildJointGlobalTransform;
            double[] globalRPY = ops.getRPY(ChildJointOrigin);


            //Localize the axis to the Link's coordinate system.
            localizeAxis(Joint.Axis.xyz, Joint.CoordinateSystemName);

            // Get the array values and threshold them so small values are set to 0.
            Joint.Origin.xyz = ops.getXYZ(ChildJointOrigin);
            ops.threshold(Joint.Origin.xyz, 0.00001);
            Joint.Origin.rpy = ops.getRPY(ChildJointOrigin);
            ops.threshold(Joint.Origin.xyz, 0.00001);
        }
コード例 #5
0
 // Creates a Reference Coordinate System in the SolidWorks Model to symbolize the joint location
 public void createRefOrigin(joint Joint)
 {
     createRefOrigin(Joint.Origin, Joint.CoordinateSystemName);
 }
コード例 #6
0
 //Creates the Origin_global coordinate system
 public void createBaseRefOrigin(bool zIsUp)
 {
     if (!ActiveSWModel.Extension.SelectByID2("Origin_global", "COORDSYS", 0, 0, 0, false, 0, null, 0))
     {
         joint Joint = new joint();
         Joint.Origin = new origin();
         if (zIsUp)
         {
             Joint.Origin.rpy = new double[] { -Math.PI / 2, 0, 0 };
         }
         else
         {
             Joint.Origin.rpy = new double[] { 0, 0, 0 };
         }
         Joint.Origin.xyz = new double[] { 0, 0, 0 };
         Joint.CoordinateSystemName = "Origin_global";
         if (referenceSketchName == null)
         {
             referenceSketchName = setup3DSketch();
         }
         createRefOrigin(Joint);
     }
 }
コード例 #7
0
 //Verifies that the reference geometry still exists. This can happen if the reference geometry was deleted but the configuration was kept
 public void checkRefGeometryExists(joint Joint)
 {
     if (!checkRefCoordsysExists(Joint.CoordinateSystemName))
     {
         Joint.CoordinateSystemName = "Automatically Generate";
     }
     if (!checkRefAxisExists(Joint.AxisName))
     {
         Joint.AxisName = "Automatically Generate";
     }
 }
コード例 #8
0
        //Fills the property boxes on the joint properties page
        public void fillJointPropertyBoxes(joint Joint)
        {
            fillBlank(jointBoxes);
            AutoUpdatingForm = true;
            if (Joint != null) //For the base_link or if none is selected
            {
                Joint.fillBoxes(textBox_joint_name, comboBox_joint_type);
                Joint.Parent.fillBoxes(label_parent);
                Joint.Child.fillBoxes(label_child);

                //G5: Maximum decimal places to use (not counting exponential notation) is 5

                Joint.Origin.fillBoxes(textBox_joint_x,
                                       textBox_joint_y,
                                       textBox_joint_z,
                                       textBox_joint_roll,
                                       textBox_joint_pitch,
                                       textBox_joint_yaw,
                                       "G5");

                Joint.Axis.fillBoxes(textBox_axis_x, textBox_axis_y, textBox_axis_z, "G5");

                if (Joint.Limit != null)
                {
                    Joint.Limit.fillBoxes(textBox_limit_lower,
                                          textBox_limit_upper,
                                          textBox_limit_effort,
                                          textBox_limit_velocity,
                                          "G5");
                }

                if (Joint.Calibration != null)
                {
                    Joint.Calibration.fillBoxes(textBox_calibration_rising,
                                                textBox_calibration_falling,
                                                "G5");
                }

                if (Joint.Dynamics != null)
                {
                    Joint.Dynamics.fillBoxes(textBox_damping,
                                             textBox_friction,
                                             "G5");
                }

                if (Joint.Safety != null)
                {
                    Joint.Safety.fillBoxes(textBox_soft_lower,
                                           textBox_soft_upper,
                                           textBox_k_position,
                                           textBox_k_velocity,
                                           "G5");
                }
            }

            if (Joint != null && (Joint.type == "revolute" || Joint.type == "continuous"))
            {
                label_lower_limit.Text = "lower (rad)";
                label_limit_upper.Text = "upper (rad)";
                label_effort.Text      = "effort (N-m)";
                label_velocity.Text    = "velocity (rad/s)";
                label_friction.Text    = "friction (N-m)";
                label_damping.Text     = "damping (N-m-s/rad)";
                label_soft_lower.Text  = "soft lower limit (rad)";
                label_soft_upper.Text  = "soft upper limit (rad)";
                label_kposition.Text   = "k position";
                label_kvelocity.Text   = "k velocity";
            }
            else if (Joint != null && Joint.type == "prismatic")
            {
                label_lower_limit.Text = "lower (m)";
                label_limit_upper.Text = "upper (m)";
                label_effort.Text      = "effort (N)";
                label_velocity.Text    = "velocity (m/s)";
                label_friction.Text    = "friction (N)";
                label_damping.Text     = "damping (N-s/m)";
                label_soft_lower.Text  = "soft lower limit (m)";
                label_soft_upper.Text  = "soft upper limit (m)";
                label_kposition.Text   = "k position";
                label_kvelocity.Text   = "k velocity";
            }
            else
            {
                label_lower_limit.Text = "lower";
                label_limit_upper.Text = "upper";
                label_effort.Text      = "effort";
                label_velocity.Text    = "velocity";
                label_friction.Text    = "friction";
                label_damping.Text     = "damping";
                label_soft_lower.Text  = "soft lower limit";
                label_soft_upper.Text  = "soft upper limit";
                label_kposition.Text   = "k position";
                label_kvelocity.Text   = "k velocity";
            }
            comboBox_origin.Items.Clear();
            List <string> originNames = Exporter.FindRefGeoNames("CoordSys");

            comboBox_origin.Items.AddRange(originNames.ToArray());
            comboBox_axis.Items.Clear();
            List <string> axesNames = Exporter.FindRefGeoNames("RefAxis");

            comboBox_axis.Items.AddRange(axesNames.ToArray());
            comboBox_origin.SelectedIndex = comboBox_origin.FindStringExact(Joint.CoordinateSystemName);
            if (Joint.AxisName != "")
            {
                comboBox_axis.SelectedIndex = comboBox_axis.FindStringExact(Joint.AxisName);
            }
            AutoUpdatingForm = false;
        }
コード例 #9
0
        //Saves data from text boxes back into a joint
        public void saveJointDataFromPropertyBoxes(joint Joint)
        {
            Joint.update(textBox_joint_name, comboBox_joint_type);

            Joint.Parent.update(label_parent);
            Joint.Child.update(label_child);

            Joint.CoordinateSystemName = comboBox_origin.Text;
            Joint.AxisName             = comboBox_axis.Text;

            Joint.Origin.update(textBox_joint_x,
                                textBox_joint_y,
                                textBox_joint_z,
                                textBox_joint_roll,
                                textBox_joint_pitch,
                                textBox_joint_yaw);

            Joint.Axis.update(textBox_axis_x,
                              textBox_axis_y,
                              textBox_axis_z);

            if (textBox_limit_lower.Text == "" && textBox_limit_upper.Text == "" && textBox_limit_effort.Text == "" && textBox_limit_velocity.Text == "")
            {
                if (Joint.type == "prismatic" || Joint.type == "revolute")
                {
                    if (Joint.Limit == null)
                    {
                        Joint.Limit = new limit();
                    }
                    else
                    {
                        Joint.Limit.effort   = 0;
                        Joint.Limit.velocity = 0;
                    }
                }
                else
                {
                    Joint.Limit = null;
                }
            }
            else
            {
                if (Joint.Limit == null)
                {
                    Joint.Limit = new limit();
                }
                Joint.Limit.setValues(textBox_limit_lower,
                                      textBox_limit_upper,
                                      textBox_limit_effort,
                                      textBox_limit_velocity);
            }

            if (textBox_calibration_rising.Text == "" && textBox_calibration_falling.Text == "")
            {
                Joint.Calibration = null;
            }
            else
            {
                if (Joint.Calibration == null)
                {
                    Joint.Calibration = new calibration();
                }
                Joint.Calibration.setValues(textBox_calibration_rising,
                                            textBox_calibration_falling);
            }

            if (textBox_friction.Text == "" && textBox_damping.Text == "")
            {
                Joint.Dynamics = null;
            }
            else
            {
                if (Joint.Dynamics == null)
                {
                    Joint.Dynamics = new dynamics();
                }
                Joint.Dynamics.setValues(textBox_damping,
                                         textBox_friction);
            }

            if (textBox_soft_lower.Text == "" && textBox_soft_upper.Text == "" && textBox_k_position.Text == "" && textBox_k_velocity.Text == "")
            {
                Joint.Safety = null;
            }
            else
            {
                if (Joint.Safety == null)
                {
                    Joint.Safety = new safety_controller();
                }
                Joint.Safety.setValues(textBox_soft_lower,
                                       textBox_soft_upper,
                                       textBox_k_position,
                                       textBox_k_velocity);
            }
        }
コード例 #10
0
        //Fills the property boxes on the joint properties page
        public void fillJointPropertyBoxes(joint Joint)
        {
            fillBlank(jointBoxes);
            AutoUpdatingForm = true;
            if (Joint != null) //For the base_link or if none is selected
            {
                Joint.fillBoxes(textBox_joint_name, comboBox_joint_type);
                Joint.Parent.fillBoxes(label_parent);
                Joint.Child.fillBoxes(label_child);

                //G5: Maximum decimal places to use (not counting exponential notation) is 5

                Joint.Origin.fillBoxes(textBox_joint_x, 
                                       textBox_joint_y, 
                                       textBox_joint_z, 
                                       textBox_joint_roll, 
                                       textBox_joint_pitch, 
                                       textBox_joint_yaw, 
                                       "G5");

                Joint.Axis.fillBoxes(textBox_axis_x, textBox_axis_y, textBox_axis_z, "G5");

                if (Joint.Limit != null)
                {
                    Joint.Limit.fillBoxes(textBox_limit_lower,
                                          textBox_limit_upper,
                                          textBox_limit_effort,
                                          textBox_limit_velocity,
                                          "G5");
                }

                if (Joint.Calibration != null)
                {
                    Joint.Calibration.fillBoxes(textBox_calibration_rising,
                                                textBox_calibration_falling,
                                                "G5");
                }

                if (Joint.Dynamics != null)
                {
                    Joint.Dynamics.fillBoxes(textBox_damping,
                                             textBox_friction,
                                             "G5");
                }

                if (Joint.Safety != null)
                {
                    Joint.Safety.fillBoxes(textBox_soft_lower,
                                           textBox_soft_upper,
                                           textBox_k_position,
                                           textBox_k_velocity,
                                           "G5");
                }
            }

            if (Joint != null && (Joint.type == "revolute" || Joint.type == "continuous"))
            {
                label_lower_limit.Text = "lower (rad)";
                label_limit_upper.Text = "upper (rad)";
                label_effort.Text = "effort (N-m)";
                label_velocity.Text = "velocity (rad/s)";
                label_friction.Text = "friction (N-m)";
                label_damping.Text = "damping (N-m-s/rad)";
                label_soft_lower.Text = "soft lower limit (rad)";
                label_soft_upper.Text = "soft upper limit (rad)";
                label_kposition.Text = "k position";
                label_kvelocity.Text = "k velocity";
            }
            else if (Joint != null && Joint.type == "prismatic")
            {
                label_lower_limit.Text = "lower (m)";
                label_limit_upper.Text = "upper (m)";
                label_effort.Text = "effort (N)";
                label_velocity.Text = "velocity (m/s)";
                label_friction.Text = "friction (N)";
                label_damping.Text = "damping (N-s/m)";
                label_soft_lower.Text = "soft lower limit (m)";
                label_soft_upper.Text = "soft upper limit (m)";
                label_kposition.Text = "k position";
                label_kvelocity.Text = "k velocity";
            }
            else
            {
                label_lower_limit.Text = "lower";
                label_limit_upper.Text = "upper";
                label_effort.Text = "effort";
                label_velocity.Text = "velocity";
                label_friction.Text = "friction";
                label_damping.Text = "damping";
                label_soft_lower.Text = "soft lower limit";
                label_soft_upper.Text = "soft upper limit";
                label_kposition.Text = "k position";
                label_kvelocity.Text = "k velocity";
            }
            comboBox_origin.Items.Clear();
            List<string> originNames = Exporter.FindRefGeoNames("CoordSys");
            comboBox_origin.Items.AddRange(originNames.ToArray());
            comboBox_axis.Items.Clear();
            List<string> axesNames = Exporter.FindRefGeoNames("RefAxis");
            comboBox_axis.Items.AddRange(axesNames.ToArray());
            comboBox_origin.SelectedIndex = comboBox_origin.FindStringExact(Joint.CoordinateSystemName);
            if (Joint.AxisName != "")
            {
                comboBox_axis.SelectedIndex = comboBox_axis.FindStringExact(Joint.AxisName);
            }
            AutoUpdatingForm = false;
            
        }
コード例 #11
0
        //Saves data from text boxes back into a joint
        public void saveJointDataFromPropertyBoxes(joint Joint)
        {
            Joint.update(textBox_joint_name, comboBox_joint_type);

            Joint.Parent.update(label_parent);
            Joint.Child.update(label_child);

            Joint.CoordinateSystemName = comboBox_origin.Text;
            Joint.AxisName = comboBox_axis.Text;

            Joint.Origin.update(textBox_joint_x, 
                                textBox_joint_y, 
                                textBox_joint_z, 
                                textBox_joint_roll, 
                                textBox_joint_pitch, 
                                textBox_joint_yaw);

            Joint.Axis.update(textBox_axis_x, 
                              textBox_axis_y, 
                              textBox_axis_z);

            if (textBox_limit_lower.Text == "" && textBox_limit_upper.Text == "" && textBox_limit_effort.Text == "" && textBox_limit_velocity.Text == "")
            {
                if (Joint.type == "prismatic" || Joint.type == "revolute")
                {
                    if (Joint.Limit == null)
                    {
                        Joint.Limit = new limit();
                    }
                    else
                    {
                        Joint.Limit.effort = 0;
                        Joint.Limit.velocity = 0;
                    }
                }
                else
                {
                    Joint.Limit = null;
                }
            }
            else
            {
                if (Joint.Limit == null)
                {
                    Joint.Limit = new limit();
                }
                Joint.Limit.setValues(textBox_limit_lower, 
                                   textBox_limit_upper, 
                                   textBox_limit_effort, 
                                   textBox_limit_velocity);
            }

            if (textBox_calibration_rising.Text == "" && textBox_calibration_falling.Text == "")
            {
                Joint.Calibration = null;
            }
            else
            {
                if (Joint.Calibration == null)
                {
                    Joint.Calibration = new calibration();
                }
                Joint.Calibration.setValues(textBox_calibration_rising, 
                                         textBox_calibration_falling);
            }

            if (textBox_friction.Text == "" && textBox_damping.Text == "")
            {
                Joint.Dynamics = null;
            }
            else
            {
                if (Joint.Dynamics == null)
                {
                    Joint.Dynamics = new dynamics();
                }
                Joint.Dynamics.setValues(textBox_damping, 
                                      textBox_friction);
            }

            if (textBox_soft_lower.Text == "" && textBox_soft_upper.Text == "" && textBox_k_position.Text == "" && textBox_k_velocity.Text == "")
            {
                Joint.Safety = null;
            }
            else
            {
                if (Joint.Safety == null)
                {
                    Joint.Safety = new safety_controller();
                }
                Joint.Safety.setValues(textBox_soft_lower, 
                                    textBox_soft_upper, 
                                    textBox_k_position, 
                                    textBox_k_velocity);

            }
        }