+- RoboDK Forum (//m.sinclairbody.com/forum)
+-- Forum: RoboDK (EN) (//m.sinclairbody.com/forum/Forum-RoboDK-EN)
+--- Forum: RoboDK API (//m.sinclairbody.com/forum/Forum-RoboDK-API)
+--- Thread: Behavior of SolveIK in C API vs C++ and Python (/Thread-Behavior-of-SolveIK-in-C-API-vs-C-and-Python)
Behavior of SolveIK in C API vs C++ and Python-sig.johnnson-01-17-2023
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();
}
如果(ref ! = NULL) {
//base2flange = (*ref) * base2flange;
}
_RoboDK_check_connection(本月- >_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;
}
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);
}
如果(ref ! = NULL) {
//base2flange = (*ref) * base2flange;
}
_RoboDK_check_connection(本月- >_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;
}
RE: Behavior of SolveIK in C API vs C++ and Python-sig.johnnson-01-18-2023
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);
}
如果(ref ! = NULL) {
struct Mat_t incoming_ref = *ref;
Mat_Multiply_out(&dummy_matrix, &incoming_ref, &base2flange);
base2flange = dummy_matrix;
}
_RoboDK_check_connection(本月- >_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;
}
RE: Behavior of SolveIK in C API vs C++ and Python-Albert-01-18-2023
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.