I think I must be doing something wrong here, or there is a bug in this function. I can print out a list, call
IJointMotionStatement::SetJoints(), then check the values with IJointMotionStatement.JointValues that they correctly match, but in the “jog” dialog the values are very different. Some of them have the correct magnitude, but with the opposite sign, and others are totally different. Note, I am not setting a target in terms of coordinates, but only joints. So i am not initializing any coordinates/transforms on the statement. Since defining the joints fully defines a position/transform of the target, I would have thought this was sufficient, and that defining both was actually over-defining it.
Is it required to always define both, for it to work properly?
I think in current controller it is still target matrix that is the master data. SetJoints does not solve the forward kinematics so it doesn’t change the target matrix and that’s why you don’t see correct results. Joint values in position frames are only used to resolve joint turns as far as I understand.
Try using the following snippet to solve this. In the snippet I use IMotionTarget to solve forward kinematics for given joint values and then set the target matrix with it.
public void SetJoints(IRobotController ctr, IPositionFrame pf, IEnumerable<double> jointValues)
{
// Call SetJoints, joint values are used to resolve joint turns and external axes
pf.SetJoints(jointValues);
// Create motion target to solve forward kinematics
var mt = ctr.CreateTarget();
// Copy base and tool from position frame
if (pf.Statement.Base != null)
{
mt.BaseName = pf.Statement.Base.Name;
}
else
{
mt.BaseName = "";
}
if (pf.Statement.Tool != null)
{
mt.ToolName = pf.Statement.Tool.Name;
}
else
{
mt.ToolName = "";
}
//Copy configuration
mt.CurrentConfiguration = pf.Configuration;
// Write joint values and read target matrix
mt.SetAllJointValues(jointValues);
pf.TransformationInReference = mt.TargetMatrix;
}
This worked, thank you! I had some small suspicion it was something like this. It would be nice if there was a way to configure an IJointMotionStatement to be controlled by primarily joint-representation or controlled by a position-frame as it is now. This would be analogous to the options given by most robot-controllers to store things in “position registers” or “joint data”.
Followup question on this topic, the way I ended up going was with something like this. I added comments where I still have questions
// I found that its not necessary to set the motion target BaseName or ToolName. It seems to
// "inherit" the currently active base/tool from the RobotController.
var motion_target = robot_controller.CreateTarget();
// Set the motion target to the desired joint values. QUESTION:
// my mistake originally was assuming the joint values in a IJointMotionStatement was
// the "master data". I just want to confirm that is the case for the motion-target. The documentation
// is unclear on this point. This function is void. If the target does not like the joints for some reason,
// what is the best-practice for determining this? The .IsValid() method?
motion_target.SetAllJointValues(joint_config_degrees);
var frame = statement.CreatePosition(statement.Name);
// Copy the transformation matrix, then set the joints and the "configuration" to make sure it matches
// the motion target. SetJoints() is also a void function, so how to confirm success, and that the resulting // joint positions will match the desired values?
frame.TransformationInReference = motion_target.TargetMatrix;
frame.SetJoints(joint_config_degrees);
frame.Configuration = motion_target.CurrentConfiguration;
statement.SetJoints(joint_config_degrees);
// I use this to make sure the statement base/tool match the currently selected. QUESTION Is this necessary?
statement.Base = robot_controller.CurrentBase;
statement.Tool = robot_controller.CurrentTool;