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