[sig.johnnson]

In the C API, Item_SolveIK() is:

Code:

struct Joints_t Item_SolveIK(const struct Item_t* inst, const struct Mat_t* pose, const struct Mat_t* tool, const struct Mat_t* ref) {
struct Joints_t jnts;
struct Mat_t base2flange = *pose; //pose of the robot flange with respect to the robot base frame
if (tool != NULL) {
//struct Mat_t poseInv;
//Mat_Inv_out();
//base2flange = pose * tool->inv();
}
if (ref != NULL) {
//base2flange = (*ref) * base2flange;
}
_RoboDK_check_connection(inst->_RDK);
_RoboDK_send_Line(inst->_RDK, "G_IK");
_RoboDK_send_Pose(inst->_RDK, base2flange);
_RoboDK_send_Item(inst->_RDK, inst);
jnts = _RoboDK_recv_Array_Joints(inst->_RDK); //&jnts  VS
_RoboDK_check_status(inst->_RDK);
return jnts;
}

I notice that the behavior for when a tool or reference frame is passed has been commented-out.


This is not the case for the C++ and Python API. (C++ below).

Code:

tJoints Item::SolveIK(const Mat &pose, const Mat *tool, const Mat *ref){
   tJoints jnts;
   Mat base2flange(pose);
   if (tool != nullptr){
       base2flange = pose*tool->inv();
   }
   if (ref != nullptr){
       base2flange = (*ref) * base2flange;
   }
   _RDK->_check_connection();
   _RDK->_send_Line("G_IK");
   _RDK->_send_Pose(base2flange);
   _RDK->_send_Item(this);
   _RDK->_recv_Array(&jnts);
   _RDK->_check_status();
   return jnts;
}

Does the C call behave the same as the Python and C++ API calls?
If not, would it work if I just uncommented the relevant lines or were they commented-out due to problems?

---

Update: I think I partially answered my own question here. It looks like struct Mat_t has no ->inv() value, so I think uncommenting would not work at all.



Here's some code that might fix only the tool feature. Would this work?

Code:

struct Joints_t Item_SolveIK(const struct Item_t* inst, const struct Mat_t* pose, const struct Mat_t* tool, const struct Mat_t* ref) {
struct Joints_t jnts;
struct Mat_t base2flange = *pose; //pose of the robot flange with respect to the robot base frame
if (tool != NULL) {
//struct Mat_t poseInv;
//Mat_Inv_out();
//base2flange = pose * tool->inv();
struct Mat_t temp;
Mat_Inv_out(&temp, tool);
Mat_Multiply_out(&base2flange, &pose, tool);
}
if (ref != NULL) {
//base2flange = (*ref) * base2flange;
}
_RoboDK_check_connection(inst->_RDK);
_RoboDK_send_Line(inst->_RDK, "G_IK");
_RoboDK_send_Pose(inst->_RDK, base2flange);
_RoboDK_send_Item(inst->_RDK, inst);
jnts = _RoboDK_recv_Array_Joints(inst->_RDK); //&jnts  VS
_RoboDK_check_status(inst->_RDK);
return jnts;
}

[sig.johnnson]

Update: I answered my own question again... I modified the library and here is the working code. I'm marking this as resolved.

Code:

struct Joints_t Item_SolveIK(const struct Item_t* inst, const struct Mat_t* pose, const struct Mat_t* tool, const struct Mat_t* ref) {
struct Joints_t jnts;
struct Mat_t base2flange = *pose; // pose of the robot flange with respect to the robot base frame
struct Mat_t incoming_pose = *pose; // an extra copy is needed to do matrix multiplication by reference
struct Mat_t incoming_tool = *tool; // the pose of the tool with respect to the robot flange
struct Mat_t dummy_matrix; // needed for matrix multiplication by reference

if (tool != NULL) {
Mat_Inv(&incoming_tool);
Mat_Multiply_out(&base2flange, &incoming_pose, &incoming_tool);
}
if (ref != NULL) {
struct Mat_t incoming_ref = *ref;
Mat_Multiply_out(&dummy_matrix, &incoming_ref, &base2flange);
base2flange = dummy_matrix;
}

_RoboDK_check_connection(inst->_RDK);
_RoboDK_send_Line(inst->_RDK, "G_IK");
_RoboDK_send_Pose(inst->_RDK, base2flange);
_RoboDK_send_Item(inst->_RDK, inst);
jnts = _RoboDK_recv_Array_Joints(inst->_RDK); // &jnts  VS
_RoboDK_check_status(inst->_RDK);
return jnts;
}

[Albert]

Excellent, thank you for your contribution the the C API! We just pushed this update to the official version of the C API.

We made a minor edit to support using a NULL tool, which assumes the pose you provide is already the robot flange.