// Calculates the axis from a Reference Axis in the model public void estimateAxis(joint Joint) { Joint.Axis.xyz = estimateAxis(Joint.AxisName); }
//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"; } } } }
// 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; } } }
// 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); }
// Creates a Reference Coordinate System in the SolidWorks Model to symbolize the joint location public void createRefOrigin(joint Joint) { createRefOrigin(Joint.Origin, Joint.CoordinateSystemName); }
//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); } }
//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"; } }
//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; }
//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); } }
//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; }