Posts: 3,582
Threads: 2
Joined: Apr 2018
Reputation:
167
Hi Tero,
Using setJointsStr should place the robot at the given joint position without simulating a movement. Also, if the joints you provide are outside the robot limits it is possible that the values will be saturated to show the robot limitations (Fanuc has a virtual coupling for joints 2 and 3).
Albert
Posts: 4
Threads: 1
Joined: Feb 2021
Reputation:
0
02-19-2021, 10:18 AM
(This post was last modified: 02-19-2021, 10:37 AM by terkaa.)
Problem was that double values from string_2_doubles was used with setJoints tJoints needs to be used instead. Here is quick hack that makes it work:
static UA_StatusCode setJointsStr(void *h, const UA_NodeId objectId, size_t inputSize, const UA_Variant *input, size_t outputSize, UA_Variant *output) {
PluginOPCUA *plugin = (PluginOPCUA*)h;
if (inputSize < 2){
qDebug()<<"Input size: " << inputSize << " Output size: " << outputSize;
return UA_STATUSCODE_BADARGUMENTSMISSING;
}
QString str_item;
QString str_joints;
if (!Var_2_Str(input+0, str_item)){
return UA_STATUSCODE_BADARGUMENTSMISSING;
}
if (!Var_2_Str(input+1, str_joints)){
return UA_STATUSCODE_BADARGUMENTSMISSING;
}
// ShowMessage(plugin, QObject::tr("item is: %1").arg(str_item));
ShowMessage(plugin, QObject::tr("position is: %1").arg(str_joints));
Item item = plugin->RDK->getItem(str_item);
ShowMessage(plugin, QObject::tr("item is: %1").arg(item->Name()));
if (!plugin->RDK->Valid(item)){ //if (!ItemValid(robot)){
ShowMessage(plugin, QObject::tr("setJointsStr: RoboDK Item provided is not valid"));
return UA_STATUSCODE_BADARGUMENTSMISSING;
}
double joints[nDOFs_MAX];
item->Joints().GetValues(joints);
int numel = nDOFs_MAX;
ShowMessage(plugin, QObject::tr("Num joint is: %1").arg(numel));
string_2_doubles(str_joints, joints, &numel);
if (numel <= 0){
ShowMessage(plugin, QObject::tr("setJointsStr: Invalid joints string"));
return UA_STATUSCODE_BADARGUMENTSMISSING;
}
ShowMessage(plugin, QObject::tr("joint 1 is: 3"));
tJoints joints2 = item->Joints();
for(int o=0; o < nDOFs_MAX ;o++){ //double values from array cannot be used directly we need to use tJoints instead
joints2.Data()[o] = joints[o];
ShowMessage(plugin, QObject::tr("Joint%1 is: %2").arg(o).arg(joints2.Data()[o]));
}
bool can_move = item->MoveJ(joints2);
if (!can_move){
ShowMessage(plugin, QObject::tr("The robot can't move to this location"));
}
else {
item->setJoints(joints2);
//Sleep(200);
plugin->RDK->Render();}
return UA_STATUSCODE_GOOD;
}