/* * This file is part of OpenKC * * Copyright(c) Matthias Schöpfer (mschoepf@techfak.uni-bielefeld.de) * http://opensource.cit-ec.de/projects/openkc * * This file may be licensed under the terms of of the * GNU Lesser General Public License Version 3 (the ``LGPL''), * or (at your option) any later version. * * Software distributed under the License is distributed * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either * express or implied. See the LGPL for the specific language * governing rights and limitations. * * You should have received a copy of the LGPL along with this * program. If not, go to http://www.gnu.org/licenses/lgpl.html * or write to the Free Software Foundation, Inc., * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. * * The development of this software was supported by the * Excellence Cluster EXC 277 Cognitive Interaction Technology. * The Excellence Cluster EXC 277 is a grant of the Deutsche * Forschungsgemeinschaft (DFG) in the context of the German * Excellence Initiative. * */ /** * @file fri_okc_comm.c * * @brief Main file of libopenkcfri. * * All basic communication from/to robot is handled here. * * @author Matthias Schöpfer * @version 1.0.1 * @date 15.10.2010 */ #include "fri_okc_comm.h" #include /* #define BENCH 1 */ #define TICKS_DEBUG 1 #ifdef BENCH #include #endif /* #define CONNECTION_DEBUG*/ /** * Helper function intern to OpenKC. Taken from the Socket Programming Howto * by Gordon McMillan. Returns in_addr regardless if its IPv4 or IPv6. * * @return pointer to in_address */ static void* _get_in_addr(struct sockaddr *sa) { assert (NULL != sa); if (sa->sa_family == AF_INET) { return &(((struct sockaddr_in*)sa)->sin_addr); } return &(((struct sockaddr_in6*)sa)->sin6_addr); } /** * Helper function intern to OpenKC. This function registers a new robot * in the okc_handle for further use. * * @param remoteaddr Type #struct sockaddr_in, holds network address * to be registers/associated with the robot handle * @param okc The #okc_handle_t where the robot should be registered in. * * @return Slot of the new robot, or OKC_ERROR on error. */ static int _register_robot (struct sockaddr_in remoteaddr, okc_handle_t *okc) { int i,j; assert (NULL != okc); assert (NULL != &remoteaddr); struct sockaddr_in* remoteaddr_in = (struct sockaddr_in*) &remoteaddr; if (0 != pthread_rwlock_wrlock (okc->robot_map_names_lock)) { perror ("_register_robot: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } /* lock aquired*/ i=0; while ((okc->robot_map[i] != OKC_SLOT_UNASSIGNED) && (i < OKC_MAX_ROBOTS)) i++; if (OKC_SLOT_IN_USE == okc->robot_map[i]) { if (0 != pthread_rwlock_unlock (okc->robot_map_names_lock)) { perror ("_register_robot: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } return (OKC_ERROR); } okc->robot_map[i] = OKC_SLOT_IN_USE; okc->robot_ip_addr[i] = remoteaddr_in->sin_addr.s_addr; if (NULL == okc->robot_names[i]) { okc->robot_names[i] = malloc (OKC_ARBITRARY_STRING_LENGTH* sizeof (char)); if (NULL == okc->robot_names[i]) { perror ("_register_robot: malloc"); exit (EXIT_FAILURE); } } if (NULL == inet_ntop (remoteaddr.sin_family, _get_in_addr((struct sockaddr*) &remoteaddr), okc->robot_names[i], INET6_ADDRSTRLEN)) { perror ("_register_robot: inet_ntop"); strncpy (okc->robot_names[i],"unknown",INET6_ADDRSTRLEN); } if (0 != pthread_rwlock_rdlock (&okc->callback_lock[i])) { perror ("_register_robot: pthread_rwlock_rdlock"); exit (EXIT_FAILURE); } switch (okc->mode) { case OKC_MODE_CALLBACK_AXIS_ABS: if (NULL == okc->axis_set_abs_cb[i]) { fprintf (stderr,"_register_robot: warning: no callback function " "registered for robot %d\n",i); } break; case OKC_MODE_CALLBACK_POS_ABS: if (NULL == okc->cartpos_set_abs_cb[i]) { fprintf (stderr,"_register_robot: warning: no callback function " "registered for robot %d\n",i); } break; case OKC_MODE_CALLBACK_POS_AXIS_ABS: if (NULL == okc->cartpos_axis_set_abs_cb[i]) { fprintf (stderr,"_register_robot: warning: no callback function " "registered for robot %d\n",i); } break; case OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS: if (NULL == okc->cartpos_axis_impedance_set_abs_cb[i]) { fprintf (stderr,"_register_robot: warning: no callback function " "registered for robot %d\n",i); } break; } if (0 != pthread_rwlock_unlock (&okc->callback_lock[i])) { perror ("_register_robot: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } if (0 != pthread_rwlock_unlock (okc->robot_map_names_lock)) { perror ("_register_robot: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[i])) { perror ("_register_robot: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } okc_coords_set_all_zero (&okc->pos_correction[i]); okc_axis_set_all_zero (&okc->axis_correction[i]); memset (&okc->krl_to_robot[i], 0, sizeof (tFriKrlData)); okc->command[i] = OKC_RESET_COUNTER + OKC_COMMAND_PENDING; okc->command_ticks_count[i] = 1; okc->command_seq_count[i] = 0; for (j = 0; j < LBR_MNJ; j++) { okc->axis_stiffness[i][j] = OKC_AXIS_STIFFNESS_DEFAULT; okc->axis_damping[i][j] = OKC_AXIS_DAMPING_DEFAULT; okc->add_axis_torque[i][j] = OKC_ADD_AXIS_TORQUE_DEFAULT; } for (j = 0; j < FRI_CART_VEC; j++) { if (j < 3) { okc->cart_stiffness[i][j] = OKC_CART_XYZ_STIFFNESS_DEFAULT; okc->cart_damping[i][j] = OKC_CART_XYZ_DAMPING_DEFAULT; } else { okc->cart_stiffness[i][j] = OKC_CART_ABC_STIFFNESS_DEFAULT; okc->cart_damping[i][j] = OKC_CART_ABC_DAMPING_DEFAULT; } okc->add_tcp_ft[i][j] = OKC_ADD_TCP_FT_DEFAULT; } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[i])) { perror ("_register_robot: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } if (0 != pthread_rwlock_wrlock (&okc->rist_lbra_out_lock[i])) { perror ("_register_robot: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } okc->seq_count[i] = 0; okc->update_ticks[i] = 0; memset (&okc->message_header[i], 0, sizeof (tFriHeader)); memset (&okc->state[i], 0, sizeof (tFriRobotState)); memset (&okc->robot_data[i], 0, sizeof (tFriRobotData)); memset (&okc->connection_stats[i], 0, sizeof (tFriIntfStatistics)); memset (&okc->krl_from_robot[i], 0, sizeof (tFriKrlData)); if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[i])) { perror ("_register_robot: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } return(i); } /** * Helper function intern to OpenKC. Will receive a packet from the given socket * and put it in the buffer struct, which must have been malloced before. * * @param socket Socket to receive UDP paket from * @param buffer The message will be but here. Must have been malloced before. * @param timestamp #struct timeval pointer to hold extra informations on paket. * @param remote_address #struct sockaddr_in pointer to hold extra info on paket. * @returns Number of bytes read from socket/result of recvmsg. */ static ssize_t _okc_receive_packet (int socket, tFriMsrData* buffer, struct timeval* timestamp, struct sockaddr_in* remote_address) { struct msghdr message; struct iovec vec[1]; union { struct cmsghdr hdr; unsigned char buf[CMSG_SPACE(3*sizeof(int))]; } cmsgbuf; struct cmsghdr* cmsg; struct timeval* tv = NULL; ssize_t ret; assert (NULL != buffer); assert (NULL != remote_address); /* If vaild, socket should be larger than 2 (stdin/stdout/stderr)*/ assert (socket > 2); vec[0].iov_base = buffer; vec[0].iov_len = sizeof(tFriMsrData); memset(&message, 0, sizeof(struct msghdr)); memset(&cmsg, 0, sizeof(cmsg)); message.msg_name = (caddr_t) remote_address; if (NULL != remote_address) message.msg_namelen = sizeof (struct sockaddr); else message.msg_namelen = 0; message.msg_iov = vec; message.msg_iovlen = 1; message.msg_control = &cmsgbuf.buf; message.msg_controllen = sizeof (cmsgbuf.buf); message.msg_flags = 0; ret = recvmsg(socket, &message, 0); /* We do not need a MSG_DONTWAIT, if we select()ed on the socket before */ if (ret < 0) { perror("_okc_receive_packet: recvmsg"); return ret; } if (MSG_TRUNC == (message.msg_flags & MSG_TRUNC)) { fprintf(stderr,"_okc_receive_packet: got truncated packed. Will be " "discarded\n"); return ret; } if (NULL == timestamp) return ret; if (MSG_CTRUNC == (message.msg_flags & MSG_CTRUNC)) { fprintf(stderr,"warning: _okc_receive_packet: received truncated " "ancillary data. UDP Timestamp will be corrupt\n"); return ret; } if(message.msg_controllen < sizeof (cmsgbuf.buf)) { fprintf(stderr,"warning: _okc_receive_packet: received short ancillary" " data (%lu/%d)\n", message.msg_controllen, (int) sizeof (cmsgbuf.buf)); return ret; } for(cmsg = CMSG_FIRSTHDR(&message); cmsg != NULL; cmsg = CMSG_NXTHDR(&message, cmsg)) { if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMP) tv = (struct timeval *)CMSG_DATA(cmsg); } if(NULL != tv) { timestamp->tv_sec = tv->tv_sec; timestamp->tv_usec = tv->tv_usec; } return ret; } /** * Helper function intern to OpenKC. Will convert a provided fri connection * quality to a string. * * @param q quality that should be given as string. * * @returns string value */ static const char* _quality_to_str (int q) { switch (q) { case (FRI_QUALITY_UNACCEPTABLE): return "unacceptable"; case (FRI_QUALITY_BAD): return "bad"; case (FRI_QUALITY_OK): return "ok"; case (FRI_QUALITY_PERFECT): return "perfect"; default: return "unknown value"; } return (NULL); } /** * Helper function intern to OpenKC. Will convert a provided okc command * to a string. These commands are used when communicating to the KRL level * programm. * * @param command integer that is to be converted * * @returns string value */ static const char* _command_to_str (int command) { switch (command&OKC_COMMAND_MASK) { case OKC_FRI_START: return "FRI_START"; case OKC_FRI_STOP: return "FRI_STOP"; case OKC_RESET_STATUS: return "RESET_STATUS"; case OKC_SET_CP_STIFFNESS_DAMPING: return "SET_CP_STIFFNESS_DAMPING"; case OKC_SET_AXIS_STIFFNESS_DAMPING: return "SET_AXIS_STIFFNESS_DAMPING"; case OKC_SWITCH_CP_CONTROL: return "SWITCH_CP_CONTROL"; case OKC_SWITCH_AXIS_CONTROL: return "SWITCH_AXIS_CONTROL"; case OKC_SWITCH_GRAVCOMP: return "SWITCH_GRAVCOMP"; case OKC_SWITCH_POSITION: return "SWITCH_POSITION"; case OKC_MOVE_START_POSITION: return "MOVE_START_POSITION"; case OKC_RESET_COUNTER: return "RESET_COUNTER"; default: return "unknown command"; } return "unknown command"; } /** * Helper function intern to OpenKC. This function will take a message * and copy the values to the okc_handle, if applicable. This function * will also print out state and connection quality changes. * * @param okc handlte to #okc_handle_t, which will be modified * @param robot_id the id of the robot the message came from * @param message_data #tFriMsrData from the robot to be processed * @param laststate needed for detection of state changes * @param lastquality needed to detect changes in the connection quality * */ static void _okc_process_incoming_data (okc_handle_t* okc, int robot_id, tFriMsrData message_data, FRI_STATE laststate[], FRI_QUALITY lastquality[]) { assert (NULL != okc); assert (robot_id < OKC_MAX_ROBOTS); assert (robot_id >= 0); assert (NULL != laststate); assert (NULL != lastquality); if (0 != pthread_rwlock_wrlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("_okc_process_incoming_data: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } okc->state[robot_id] = message_data.robot; okc->robot_data[robot_id] = message_data.data; okc->krl_from_robot[robot_id] = message_data.krl; okc->connection_stats[robot_id] = message_data.intf; okc->message_header[robot_id] = message_data.head; #ifdef CONNECTION_DEBUG fprintf (stderr,"answerRate = %f, latency = %f, missRate = %f, " "missCounter = %u\n",message_data.intf.stat.answerRate, message_data.intf.stat.latency, message_data.intf.stat.missRate, message_data.intf.stat.missCounter); #endif if (message_data.intf.state != laststate[robot_id]) { fprintf (stderr,"State change from %s to %s\n", (laststate[robot_id] == FRI_STATE_CMD)?"command": (laststate[robot_id] == FRI_STATE_MON)?"monitor": "off", (message_data.intf.state == FRI_STATE_CMD)?"command": (message_data.intf.state == FRI_STATE_MON)? "monitor":"off"); laststate[robot_id] = message_data.intf.state; } if (message_data.intf.quality != lastquality[robot_id]) { fprintf (stderr,"Quality change from %s to %s\n", _quality_to_str (lastquality[robot_id]), _quality_to_str (message_data.intf.quality)); lastquality[robot_id] = message_data.intf.quality; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("_okc_process_incoming_data: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_process_incoming_data: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } switch (message_data.krl.intData[15]) { case OKC_ACK_FRI_START: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_FRI_START) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_FRI_STOP: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_FRI_STOP) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_OK: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_RESET_STATUS) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = 0; } break; case OKC_ACK_SET_CP_STIFFNESS_DAMPING: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_SET_CP_STIFFNESS_DAMPING) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_SET_AXIS_STIFFNESS_DAMPING: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_SET_AXIS_STIFFNESS_DAMPING) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_SWITCH_CP_CONTROL: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_SWITCH_CP_CONTROL) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_SWITCH_AXIS_CONTROL: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_SWITCH_AXIS_CONTROL)&& (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_SWITCH_POSITION: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_SWITCH_POSITION) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_SWITCH_GRAVCOMP: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_SWITCH_GRAVCOMP) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n",_command_to_str (okc->command[robot_id]),okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_MOVE_START_POSITION: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_MOVE_START_POSITION)&& (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = OKC_RESET_STATUS; } break; case OKC_ACK_RESET_COUNTER: if (((okc->command[robot_id]&OKC_COMMAND_MASK) == OKC_RESET_COUNTER)&& (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { #ifdef TICKS_DEBUG fprintf (stderr,"Command '%s' (%d) took %u ticks\n", _command_to_str (okc->command[robot_id]), okc->command[robot_id], okc->command_ticks_count[robot_id]); #endif okc->command[robot_id] = 0; } break; } if ((((message_data.krl.intData[15] != 0) && (message_data.krl.intData[15] != OKC_ACK_RESET_COUNTER)) && ((OKC_COMMAND_MASK&okc->command[robot_id]) != OKC_RESET_STATUS)) && (message_data.krl.intData[13] == okc->command_seq_count[robot_id])) { fprintf (stderr,"_okc_process_incoming_data: warning: internal processing" " error! Please file a bug! (intData[15] = %d, okc->command[%d]" " = %d)\n",message_data.krl.intData[15], robot_id, okc->command[robot_id]); abort(); } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_process_incoming_data: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } } /** * Helper function intern to OpenKC. This function will prepare a * #tFriCmdData (see fricomm.h provided by Kuka for details on this * struct) struct according to the settings found in the * #okc_handle_t. The function will also take care of setting or * managing new and pending commands respectively. * * @param okc pointer to #okc_handle_t * * @param robot_id Id of the robot the message shall be sended * to. Must exists (has to be registered). * * @param command_data pointer to a #tFriCmdData struct. Must have * been malloced before. * * @returns #OKC_OK on success, or #OKC_ERROR if an internal error * occurs (which should only happen if some inconsistency to OpenKC * was introduced). */ int _okc_prepare_command_data (okc_handle_t* okc, int robot_id, tFriCmdData* command_data) { int i; int acknowledge; assert (NULL != okc); assert (robot_id < OKC_MAX_ROBOTS); assert (robot_id >= 0); assert (NULL != command_data); memset (command_data,0,sizeof (tFriCmdData)); command_data->head.datagramId = FRI_DATAGRAM_ID_CMD; command_data->head.packetSize = sizeof (tFriCmdData); if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("_okc_prepare_command_data: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } command_data->head.reflSeqCount = okc->message_header[robot_id].sendSeqCount; acknowledge = okc->krl_from_robot[robot_id].intData[14]; if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("_okc_prepare_command_data: pthread_rwlock_rdlock"); exit (EXIT_FAILURE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_prepare_command_data: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } command_data->head.sendSeqCount = okc->seq_count[robot_id]++; command_data->krl = okc->krl_to_robot[robot_id]; if (OKC_COMMAND_PENDING == (okc->command[robot_id]&OKC_COMMAND_PENDING)) { /* Old Command (set data accordingly, for safty reasons */ switch (OKC_COMMAND_MASK&okc->command[robot_id]) { case OKC_FRI_START: command_data->krl.intData[15] = ((acknowledge != OKC_FRI_START) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_FRI_START:0; okc->command_ticks_count[robot_id]++; break; case OKC_FRI_STOP: command_data->krl.intData[15] = ((acknowledge != OKC_FRI_STOP ) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_FRI_STOP:0; okc->command_ticks_count[robot_id]++; break; case OKC_RESET_STATUS: command_data->krl.intData[15] = ((acknowledge != OKC_RESET_STATUS) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_RESET_STATUS:0; okc->command_ticks_count[robot_id]++; break; case OKC_SET_CP_STIFFNESS_DAMPING: command_data->krl.intData[15] = ((acknowledge != OKC_SET_CP_STIFFNESS_DAMPING) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_SET_CP_STIFFNESS_DAMPING:0; for (i = 0; i < FRI_CART_VEC; i++) { command_data->krl.intData[9+i] = round(okc->cart_stiffness[robot_id][i]); command_data->krl.realData[10+i] = okc->cart_damping[robot_id][i]; } okc->command_ticks_count[robot_id]++; break; case OKC_SET_AXIS_STIFFNESS_DAMPING: command_data->krl.intData[15] = ((acknowledge != OKC_SET_AXIS_STIFFNESS_DAMPING) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_SET_AXIS_STIFFNESS_DAMPING:0; for (i = 0; i < LBR_MNJ; i++) { command_data->krl.intData[8+i] = round(okc->axis_stiffness[robot_id][i]); command_data->krl.realData[9+i] = okc->axis_damping[robot_id][i]; } okc->command_ticks_count[robot_id]++; break; case OKC_SWITCH_CP_CONTROL: command_data->krl.intData[15] = ((acknowledge != OKC_SWITCH_CP_CONTROL) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_SWITCH_CP_CONTROL:0; okc->command_ticks_count[robot_id]++; break; case OKC_SWITCH_AXIS_CONTROL: command_data->krl.intData[15] = ((acknowledge != OKC_SWITCH_AXIS_CONTROL) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_SWITCH_AXIS_CONTROL:0; okc->command_ticks_count[robot_id]++; break; case OKC_SWITCH_GRAVCOMP: command_data->krl.intData[15] = ((acknowledge != OKC_SWITCH_GRAVCOMP) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_SWITCH_GRAVCOMP:0; okc->command_ticks_count[robot_id]++; break; case OKC_SWITCH_POSITION: command_data->krl.intData[15] = ((acknowledge != OKC_SWITCH_POSITION) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_SWITCH_POSITION:0; okc->command_ticks_count[robot_id]++; break; case OKC_MOVE_START_POSITION: command_data->krl.intData[15] = ((acknowledge != OKC_MOVE_START_POSITION) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_MOVE_START_POSITION:0; okc->command_ticks_count[robot_id]++; break; case OKC_RESET_COUNTER: command_data->krl.intData[15] = ((acknowledge != OKC_RESET_COUNTER) && (((okc->command_ticks_count[robot_id] / 5) % 2) == 0 ))? OKC_RESET_COUNTER:0; okc->command_ticks_count[robot_id]++; break; default: fprintf (stderr,"_okc_prepare_command_data: warning: unknown old " "command parameter detected: command[%d] = %d\n",robot_id, (OKC_COMMAND_MASK&okc->command[robot_id])); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_prepare_command_data: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } return (OKC_ERROR); } } if ((OKC_COMMAND_PENDING != (okc->command[robot_id]&OKC_COMMAND_PENDING)) && (0 != (okc->command[robot_id]&OKC_COMMAND_MASK))) { /* New Command */ okc->command_seq_count[robot_id]++; switch (okc->command[robot_id]) { case OKC_FRI_START: command_data->krl.intData[15] = OKC_FRI_START; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_FRI_STOP: command_data->krl.intData[15] = OKC_FRI_STOP; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_RESET_STATUS: command_data->krl.intData[15] = OKC_RESET_STATUS; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_SET_CP_STIFFNESS_DAMPING: command_data->krl.intData[15] = OKC_SET_CP_STIFFNESS_DAMPING; for (i = 0; i < FRI_CART_VEC; i++) { command_data->krl.intData[9+i] = round(okc->cart_stiffness[robot_id][i]); command_data->krl.realData[10+i] = okc->cart_damping[robot_id][i]; } okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_SET_AXIS_STIFFNESS_DAMPING: command_data->krl.intData[15] = OKC_SET_AXIS_STIFFNESS_DAMPING; for (i = 0; i < LBR_MNJ; i++) { command_data->krl.intData[8+i] = round(okc->axis_stiffness[robot_id][i]); command_data->krl.realData[9+i] = okc->axis_damping[robot_id][i]; } okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_SWITCH_CP_CONTROL: command_data->krl.intData[15] = OKC_SWITCH_CP_CONTROL; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_SWITCH_AXIS_CONTROL: command_data->krl.intData[15] = OKC_SWITCH_AXIS_CONTROL; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_SWITCH_GRAVCOMP: command_data->krl.intData[15] = OKC_SWITCH_GRAVCOMP; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_SWITCH_POSITION: command_data->krl.intData[15] = OKC_SWITCH_POSITION; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; case OKC_MOVE_START_POSITION: command_data->krl.intData[15] = OKC_MOVE_START_POSITION; okc->command[robot_id] += OKC_COMMAND_PENDING; okc->command_ticks_count[robot_id] = 1; break; default: fprintf (stderr,"_okc_prepare_command_data: warning: unknown new " "command parameter detected: command[%d] = %d\n",robot_id, okc->command[robot_id]); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_prepare_command_data: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } return (OKC_OK); } /** * Helper function intern to OpenKC. This function is for legacy * conformity and will convert old position correction format * (X,Y,Z,A,B.C) into new matrix format. This function asserts that * robot_id is valid. * * @param okc pointer to valid #okc_handle_t * @param robot_id id of robot of which position corrections shall be * converted to matrix format. * */ inline static void _pos_correction_to_matrix (okc_handle_t* okc, int robot_id) { assert (NULL != okc); assert (robot_id < OKC_MAX_ROBOTS); assert (robot_id >= 0); const fri_float_t cosa = cos (okc->pos_correction[robot_id].c); const fri_float_t cosb = cos (okc->pos_correction[robot_id].b); const fri_float_t cosg = cos (okc->pos_correction[robot_id].a); const fri_float_t sina = sin (okc->pos_correction[robot_id].c); const fri_float_t sinb = sin (okc->pos_correction[robot_id].b); const fri_float_t sing = sin (okc->pos_correction[robot_id].a); okc->matrix_pos_correction[robot_id][0] = cosa*cosb; okc->matrix_pos_correction[robot_id][1] = (cosa*sinb*sing) - (sina*cosg); okc->matrix_pos_correction[robot_id][2] = (cosa*sinb*cosg) + (sina*sing); okc->matrix_pos_correction[robot_id][3] = okc->pos_correction[robot_id].x; okc->matrix_pos_correction[robot_id][4] = sina*cosb; okc->matrix_pos_correction[robot_id][5] = (sina*sinb*sing) + (cosa*cosg); okc->matrix_pos_correction[robot_id][6] = (sina*sinb*cosg) - (cosa*sing); okc->matrix_pos_correction[robot_id][7] = okc->pos_correction[robot_id].y; okc->matrix_pos_correction[robot_id][8] = -1.0*sinb; okc->matrix_pos_correction[robot_id][9] = cosb*sing; okc->matrix_pos_correction[robot_id][10] = cosb*cosg; okc->matrix_pos_correction[robot_id][11] = okc->pos_correction[robot_id].z; } /** * Helper function intern to OpenKC. This function does a matrix * multiplication. To successfully do that, first the matrix has to be * expanded to form a homogeneous transformation. We will use a cblas * call to do the matrix multiplication and copy back the values. * * @param a 12-dim cartesian position as used in fri. Will not be * changed * @param b 12-dim cartesian position as used in fri. Will not * be changed * @param c 12-dim cartesian position as used in fri. Will * be written to, must be alloced correctly. */ static void _multiply_cart_frm_dim (const fri_float_t* a, const fri_float_t* b, fri_float_t* c) { float la[16]; float lb[16]; float lc[16]; assert (NULL != a); assert (NULL != b); assert (NULL != c); /* If this changes, one needs to replace the memcpy parts */ assert (sizeof (float) == sizeof (fri_float_t)); memcpy (la,a,12*sizeof (float)); memcpy (lb,b,12*sizeof (float)); la[12] = la[13] = la[14] = 0.0; lb[12] = lb[13] = lb[14] = 0.0; la[15] = lb[15] = 1.0; memset (lc,0,sizeof (float)*16); cblas_sgemm (CblasRowMajor,CblasNoTrans,CblasNoTrans,4,4,4,(float)1.0, (float*)la,4,(float*)lb,4, 1.0,(float*)lc,4); memcpy (c,lc,12*sizeof (fri_float_t)); } /** * Helper function intern to OpenKC. This function handles the call to * the registered and appropriate callback function. It will copy the * results coming from the callback into the okc struct, if callback * signals it was successfully. * * @param okc pointer to #okc_handle_t * * @param robot_id id of the robot whoms callback function shall be called. * * @param message_data the incoming message data handle, needed for * the latest values from the robot. */ static void _handle_callback_function_calls (okc_handle_t* okc, int robot_id, tFriMsrData message_data) { fri_float_t axis_pos[LBR_MNJ]; fri_float_t new_axis_pos[LBR_MNJ]; fri_float_t cart_pos[FRI_CART_FRM_DIM]; fri_float_t new_cart_pos[FRI_CART_FRM_DIM]; fri_float_t new_cartstiff[FRI_CART_VEC]; fri_float_t new_cartdamp[FRI_CART_VEC]; int i; assert (NULL != okc); assert (robot_id < OKC_MAX_ROBOTS); assert (robot_id >= 0); memset (axis_pos,0,sizeof (fri_float_t)*LBR_MNJ); memset (new_axis_pos,0,sizeof (fri_float_t)*LBR_MNJ); memset (cart_pos,0,sizeof (fri_float_t)*FRI_CART_FRM_DIM); memset (new_cart_pos,0,sizeof (fri_float_t)*FRI_CART_FRM_DIM); memset (new_cartstiff,0,sizeof (fri_float_t)*FRI_CART_VEC); memset (new_cartdamp,0,sizeof (fri_float_t)*FRI_CART_VEC); if (0 != pthread_rwlock_rdlock (&okc->callback_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_rdlock"); exit (EXIT_FAILURE); } /* locking callback function */ switch (okc->mode) { case OKC_MODE_CALLBACK_AXIS_ABS: for (i = 0; i < LBR_MNJ; i++) { axis_pos[i] = message_data.data.cmdJntPos[i] + message_data.data.cmdJntPosFriOffset[i]; } if (NULL != okc->axis_set_abs_cb[robot_id]) { if (OKC_OK != okc->axis_set_abs_cb[robot_id] (okc->private_to_axis_set_abs_cb[robot_id], axis_pos, new_axis_pos)) { okc_cp_lbr_mnj (axis_pos, new_axis_pos); fprintf (stderr,"_serve_connections: warning: " "callback returned non OK exit status\n"); } } else { okc_cp_lbr_mnj (axis_pos, new_axis_pos); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } okc_cp_lbr_mnj (new_axis_pos,okc->axis_abs_set[robot_id]); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } break; case OKC_MODE_CALLBACK_POS_ABS: _multiply_cart_frm_dim (message_data.data.cmdCartPos, message_data.data.cmdCartPosFriOffset, cart_pos); if (NULL != okc->cartpos_set_abs_cb[robot_id]) { if (OKC_OK != okc->cartpos_set_abs_cb[robot_id] (okc->private_to_cartpos_set_abs_cb[robot_id], cart_pos, new_cart_pos)) { okc_cp_cart_frm_dim (cart_pos, new_cart_pos); fprintf (stderr,"_serve_connections: warning: " "callback returned non OK exit status\n"); } } else { okc_cp_cart_frm_dim (cart_pos, new_cart_pos); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } okc_cp_cart_frm_dim (new_cart_pos,okc->pos_abs_set[robot_id]); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } break; case OKC_MODE_CALLBACK_POS_AXIS_ABS: _multiply_cart_frm_dim (message_data.data.cmdCartPos, message_data.data.cmdCartPosFriOffset, cart_pos); for (i = 0; i < LBR_MNJ; i++) { axis_pos[i] = message_data.data.cmdJntPos[i] + message_data.data.cmdJntPosFriOffset[i]; } if (NULL != okc->cartpos_axis_set_abs_cb[robot_id]) { if (OKC_OK != okc->cartpos_axis_set_abs_cb[robot_id] (okc->private_to_cartpos_axis_set_abs_cb[robot_id], cart_pos,axis_pos, new_cart_pos, new_axis_pos)) { okc_cp_cart_frm_dim (cart_pos, new_cart_pos); okc_cp_lbr_mnj (axis_pos, new_axis_pos); fprintf (stderr,"_serve_connections: warning: " "callback returned non OK exit status\n"); } } else { okc_cp_cart_frm_dim (cart_pos, new_cart_pos); okc_cp_lbr_mnj (axis_pos, new_axis_pos); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } okc_cp_cart_frm_dim (new_cart_pos,okc->pos_abs_set[robot_id]); okc_cp_lbr_mnj (new_axis_pos,okc->axis_abs_set[robot_id]); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } break; case OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS: _multiply_cart_frm_dim (message_data.data.cmdCartPos, message_data.data.cmdCartPosFriOffset, cart_pos); for (i = 0; i < LBR_MNJ; i++) { axis_pos[i] = message_data.data.cmdJntPos[i] + message_data.data.cmdJntPosFriOffset[i]; } if (NULL != okc->cartpos_axis_impedance_set_abs_cb[robot_id]) { if (OKC_OK != okc->cartpos_axis_impedance_set_abs_cb[robot_id] (okc->private_to_cartpos_axis_impedance_set_abs_cb[robot_id], cart_pos,axis_pos, new_cart_pos, new_axis_pos, new_cartstiff, new_cartdamp)) { okc_cp_cart_frm_dim (cart_pos, new_cart_pos); okc_cp_lbr_mnj (axis_pos, new_axis_pos); okc_set_to_cp_stiff_damp_default (new_cartstiff, new_cartdamp); fprintf (stderr,"_serve_connections: warning: " "callback returned non OK exit status\n"); } } else { okc_cp_cart_frm_dim (cart_pos, new_cart_pos); okc_cp_lbr_mnj (axis_pos, new_axis_pos); okc_set_to_cp_stiff_damp_default (new_cartstiff, new_cartdamp); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_wrlock"); exit (EXIT_FAILURE); } okc_cp_cart_frm_dim (new_cart_pos,okc->pos_abs_set[robot_id]); okc_cp_lbr_mnj (new_axis_pos,okc->axis_abs_set[robot_id]); okc_cp_cart_vec (new_cartstiff, okc->cart_stiffness[robot_id]); okc_cp_cart_vec (new_cartdamp, okc->cart_damping[robot_id]); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } break; default: fprintf (stderr, "_serve_connections: internal error: unknown callback mode\n"); } if (0 != pthread_rwlock_unlock (&okc->callback_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } } /** * Helper function intern to OpenKC. This function runs in its own thread. * This is the implementation of the actual server. After some setup-stuff * the server will wait for packages from robot(s) that need to be answered. * Depending on the mode, callback-functions are called from within this * routine or the values set by okc_set_coords_correction() or * okc_set_axis_correction() are used. * * @param okc_struct_p Pointer to the original #okc_handle_t. * * @see okc_start_server() * @see okc_stop_server() * @see okc_set_coords_correction() * @see okc_set_axis_correction() */ static void* _serve_connections (void* okc_struct_p) { fd_set master; fd_set read_fds; int fdmax; int i=0, robot_id, newrobotid; char remoteIP[INET6_ADDRSTRLEN]; struct sockaddr_in remoteaddr; tFriCmdData command_data; tFriMsrData message_data; struct sockaddr_in robot_ip; struct timeval sleep_val; struct timeval socket_timestamp; struct sockaddr_in to_address; int readsel; ssize_t nbytes; FRI_QUALITY lastquality[OKC_MAX_ROBOTS]; FRI_STATE laststate[OKC_MAX_ROBOTS]; #ifdef BENCH long long int serve=0; int last_cmd=0; int ticks=0; int last_status=0; int st_ticks=0; struct timeval tv_last, tv_now; #endif sigset_t allsigs; /**< Ignore all signals (esp. SIGRT). */ okc_handle_t* okc = (okc_handle_t*) okc_struct_p; assert (NULL != okc); /** Clear signal mask: This thread won't get any signals delivered. */ sigfillset (&allsigs); pthread_sigmask (SIG_BLOCK, &allsigs, NULL); for (i = 0; i < OKC_MAX_ROBOTS; i++) { lastquality[i] = FRI_QUALITY_UNACCEPTABLE; laststate[i] = FRI_STATE_OFF; } FD_ZERO(&master); FD_ZERO(&read_fds); fdmax = okc->listener; FD_SET (fdmax,&master); while ((okc->flags&OKC_QUIT) != OKC_QUIT) { read_fds = master; sleep_val.tv_sec = 1; sleep_val.tv_usec = 0; if ((readsel = select(fdmax+1, &read_fds, NULL, NULL, &sleep_val)) < 0) { perror("_serve_connections: select"); exit(EXIT_FAILURE); } if (0 == readsel) { /* Nothing to do, so just continue to idle */ continue; } /* else receive packet */ nbytes = _okc_receive_packet (okc->listener, &message_data, &socket_timestamp, &robot_ip); /* Try to assign packet to a already connected robot */ for (robot_id = 0; robot_id < OKC_MAX_ROBOTS; robot_id++) { if (OKC_SLOT_IN_USE == okc->robot_map[robot_id]) { if (robot_ip.sin_addr.s_addr == okc->robot_ip_addr[robot_id]) break; } } if (robot_ip.sin_addr.s_addr != okc->robot_ip_addr[robot_id]) { /** register new robot */ newrobotid = _register_robot(robot_ip, okc); if (OKC_ERROR == newrobotid) { fprintf (stderr,"_serve_connections: warning: " "could not register robot from IP %s\n" "Reason: no more open slots available.\n", inet_ntop(remoteaddr.sin_family, _get_in_addr((struct sockaddr*) &remoteaddr), remoteIP, INET6_ADDRSTRLEN)); } else { if (NULL == inet_ntop(AF_INET, _get_in_addr((struct sockaddr*) &robot_ip), remoteIP, INET6_ADDRSTRLEN)) { perror ("inet_ntop"); } robot_id = newrobotid; printf("_serve_connections: new connection from %s " "assigned to robot id %d\n", inet_ntop(AF_INET, _get_in_addr((struct sockaddr*) &robot_ip), remoteIP, INET6_ADDRSTRLEN), newrobotid); } } /** now handle data from a client */ if (nbytes == sizeof (tFriMsrData)) { /** we got some valid data from a client */ /* fprintf(stderr, "data number are %d\n", sizeof (tFriMsrData));*/ /* Copy values to struct */ #ifdef CONNECTION_DEBUG fprintf (stderr, "Received Packet from %s\n", inet_ntop(AF_INET, _get_in_addr((struct sockaddr*)&robot_ip),remoteIP, INET6_ADDRSTRLEN)); #endif _okc_process_incoming_data (okc, robot_id, message_data, laststate, lastquality); /* Generate Answer for Robot */ if (OKC_OK != _okc_prepare_command_data (okc, robot_id, &command_data)) { /* We really do not expect that anything goes wrong here*/ abort(); } command_data.cmd.cmdFlags = okc->command_flags[robot_id]; if (OKC_MODE_CALLBACK == (okc->mode&OKC_MODE_CALLBACK)) { /* we have to deal with a callback function */ _handle_callback_function_calls (okc,robot_id, message_data); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_rdlock"); exit (EXIT_FAILURE); } if (FRI_CMD_JNTPOS == (okc->command_flags[robot_id]&FRI_CMD_JNTPOS)) { okc_cp_lbr_mnj (okc->axis_abs_set[robot_id], command_data.cmd.jntPos); } if (FRI_CMD_CARTPOS == (okc->command_flags[robot_id]&FRI_CMD_CARTPOS)) { okc_cp_cart_frm_dim (okc->pos_abs_set[robot_id], command_data.cmd.cartPos); } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } } if (OKC_MODE_SETTER == (okc->mode&OKC_MODE_SETTER)) { if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_rdlock"); exit (EXIT_FAILURE); } if (FRI_CMD_JNTPOS == (command_data.cmd.cmdFlags & FRI_CMD_JNTPOS)) { for (i = 0; i < LBR_MNJ; i++) command_data.cmd.jntPos[i] = message_data.data.cmdJntPos[i] + message_data.data.cmdJntPosFriOffset[i] + ((fri_float_t*)&okc->axis_correction[robot_id])[i]; #ifdef CONNECTION_DEBUG fprintf (stderr,"Joint a1 = %f -> %f\n", message_data.data.cmdJntPos[0] + message_data.data.cmdJntPosFriOffset[0], command_data.cmd.jntPos[0]); #endif } if (FRI_CMD_CARTPOS == (command_data.cmd.cmdFlags & FRI_CMD_CARTPOS)) { _pos_correction_to_matrix (okc,robot_id); _multiply_cart_frm_dim (message_data.data.cmdCartPos, message_data.data.cmdCartPosFriOffset, command_data.cmd.cartPos); _multiply_cart_frm_dim (command_data.cmd.cartPos, okc->matrix_pos_correction[robot_id], command_data.cmd.cartPos); } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } } if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_rdlock"); exit (EXIT_FAILURE); } if ((FRI_CMD_JNTSTIFF == (okc->command_flags[robot_id] & FRI_CMD_JNTSTIFF)) && (FRI_CMD_JNTDAMP == (okc->command_flags[robot_id] & FRI_CMD_JNTDAMP))) { for (i = 0; i < LBR_MNJ; i++) { command_data.cmd.jntStiffness[i] = okc->axis_stiffness[robot_id][i]; command_data.cmd.jntDamping[i] = okc->axis_damping[robot_id][i]; } } if ((FRI_CMD_CARTSTIFF == (okc->command_flags[robot_id] & FRI_CMD_CARTSTIFF)) && (FRI_CMD_CARTDAMP == (okc->command_flags[robot_id] & FRI_CMD_CARTDAMP))) { for (i = 0; i < FRI_CART_VEC; i++) { command_data.cmd.cartStiffness[i] = okc->cart_stiffness[robot_id][i]; command_data.cmd.cartDamping[i] = okc->cart_damping[robot_id][i]; } } if (FRI_CMD_TCPFT == (okc->command_flags[robot_id] & FRI_CMD_TCPFT)) { for (i = 0; i < FRI_CART_VEC; i++) { command_data.cmd.addTcpFT[i] = okc->add_tcp_ft[robot_id][i]; } } if (FRI_CMD_JNTTRQ == (okc->command_flags[robot_id] & FRI_CMD_JNTTRQ)) { for (i = 0; i < LBR_MNJ; i++) { command_data.cmd.addJntTrq[i] = okc->add_axis_torque[robot_id][i]; } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_serve_connections: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } if ((message_data.intf.state != FRI_STATE_CMD) || (!(message_data.robot.power != 0))) { if (FRI_CMD_JNTPOS == (command_data.cmd.cmdFlags&FRI_CMD_JNTPOS)) { for (i = 0; i < LBR_MNJ; i++) { command_data.cmd.jntPos[i] = message_data.data.msrJntPos[i]; } } if (FRI_CMD_CARTPOS == (command_data.cmd.cmdFlags&FRI_CMD_CARTPOS)) { _multiply_cart_frm_dim (message_data.data.cmdCartPos, message_data.data.cmdCartPosFriOffset, command_data.cmd.cartPos); } } to_address.sin_family = AF_INET; to_address.sin_port = htons (OKC_FRI_PORT); to_address.sin_addr.s_addr = okc->robot_ip_addr[robot_id]; nbytes = sendto (okc->listener,&command_data, sizeof (tFriCmdData), 0, (struct sockaddr*) &to_address, sizeof (struct sockaddr_in)); if (nbytes < 0) { perror ("_serve_connections: error"); fprintf (stderr,"please check network setup!\n"); } if (FRI_CMD_DATA_SIZE != nbytes) fprintf (stderr,"_serve_connections: warning: " "incomplete send (%ld instead of %d)\n", nbytes, FRI_CMD_DATA_SIZE); } else{ fprintf(stderr,"not get the correct number data\n"); } } return (NULL); } /** * Checks, if a certain robot_id is associated to a robot. Returns #OKC_OK * if robot_id is connected to a real robot, or #OKC_UNAVAILABLE if no * physical robot has connected to robot_id. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to check. * @return OKC_OK, if robot is connected, OKC_UNAVAILABLE if no robot is * currently connected or OKC_ERROR on error. * */ int okc_is_robot_avail (okc_handle_t* okc, int robot_id) { int i; assert (NULL != okc); if (robot_id >= OKC_MAX_ROBOTS) { fprintf (stderr,"okc_is_robot_avail: invalid robot_id %d. Max is %d\n", robot_id, OKC_MAX_ROBOTS - 1); return OKC_ERROR; } if (0 != pthread_rwlock_rdlock (okc->robot_map_names_lock)) { perror ("okc_is_robot_avail: pthread_rwlock_rdlock"); return (OKC_ERROR); } if (OKC_SLOT_IN_USE == okc->robot_map[robot_id]) i = OKC_OK; else i = OKC_UNAVAILABLE; if (0 != pthread_rwlock_unlock (okc->robot_map_names_lock)) { perror ("okc_is_robot_avail: pthread_rwlock_unlock"); return (OKC_ERROR); } return i; } /** * Checks, if a certain robot_id is in command mode, i.e. that the * robot accepts commands (i.e. movements) from okc. Returns #OKC_OK * if robot_id is connected and in command mode, or #OKC_UNAVAILABLE * if no physical robot has connected to robot_id or robot_id is not * in command mode. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to check. * @return #OKC_OK, if robot is connected, #OKC_UNAVAILABLE if no robot is * currently connected, #OKC_NOT_IN_COMMAND_MODE if not in command mode * or #OKC_ERROR on error. * */ int okc_is_robot_in_command_mode (okc_handle_t* okc, int robot_id) { int i; assert (NULL != okc); if (robot_id >= OKC_MAX_ROBOTS) { fprintf (stderr,"okc_is_robot_in_command_mode: invalid robot_id %d. " "Max is %d\n", robot_id, OKC_MAX_ROBOTS - 1); return OKC_ERROR; } if (0 != pthread_rwlock_rdlock (okc->robot_map_names_lock)) { perror ("okc_is_robot_in_command_mode: pthread_rwlock_rdlock"); return (OKC_ERROR); } if (OKC_SLOT_IN_USE == okc->robot_map[robot_id]) i = OKC_OK; else i = OKC_UNAVAILABLE; if (0 != pthread_rwlock_unlock (okc->robot_map_names_lock)) { perror ("okc_is_robot_in_command_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } if (OKC_OK == i) { if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_is_robot_in_command_mode: pthread_rwlock_rdlock"); return (OKC_ERROR); } if (FRI_STATE_CMD == okc->connection_stats[robot_id].state) i = OKC_OK; else i = OKC_NOT_IN_COMMAND_MODE; if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_is_robot_in_command_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } } return i; } /** * Requests Robot to go into command mode. Keep in mind, that mode changes * will result in the robot dropping to monitor mode. If #OKC_OK is returned, * Command mode was successfully requested (i.e. FRI_START was executed), but * robot might not have power on yet. OpenKC will correctly mirror the joint * values until power is turned on. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to check. * @return #OKC_OK, if robot is connected, #OKC_UNAVAILABLE if no robot is * currently connected or #OKC_ERROR on error. * * @see okc_is_robot_in_command_mode(), okc_is_power_on() * */ int okc_request_command_mode (okc_handle_t* okc, int robot_id) { assert (NULL != okc); if (robot_id >= OKC_MAX_ROBOTS) { fprintf (stderr,"okc_request_command_mode: invalid robot_id %d. " "Max is %d\n", robot_id, OKC_MAX_ROBOTS - 1); return OKC_ERROR; } if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_request_command_mode: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_OK == okc_is_robot_in_command_mode (okc,robot_id)) { fprintf (stderr,"okc_request_command_mode: robot already is in command " "mode!\n"); return (OKC_INAPPROPIRATE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_command_mode: pthread_rwlock_rdlock"); return (OKC_ERROR); } if (0 != okc->command[robot_id]) fprintf (stderr,"DEBUG: command '%s' is still pending\n", _command_to_str (okc->command[robot_id])); while (0 != okc->command[robot_id]) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_command_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_command_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } } okc->command[robot_id] = OKC_FRI_START; while (0 != (okc->command[robot_id]&OKC_COMMAND_MASK)) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_command_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_command_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_command_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Requests Robot to go gracefully into monitor mode. Keep in mind, * that mode changes (cmdFlags) will result in the robot dropping to monitor * mode. If #OKC_OK is returned, fristop was successfully executed on the KRL * side... * * @param okc Pointer to #okc_handle_t. * * @param robot_id Robot ID to check. * * @return #OKC_OK, if robot is connected and mode change was * successful, #OKC_UNAVAILABLE if no robot is currently * connected, #OKC_INAPPROPRIATE, if robot is already in * momode or #OKC_ERROR on error. * * @see okc_is_robot_in_command_mode(), okc_is_power_on() * */ int okc_request_monitor_mode (okc_handle_t* okc, int robot_id) { assert (NULL != okc); if (robot_id >= OKC_MAX_ROBOTS) { fprintf (stderr,"okc_request_monitor_mode: invalid robot_id %d. " "Max is %d\n", robot_id, OKC_MAX_ROBOTS - 1); return OKC_ERROR; } if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_request_monitor_mode: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_NOT_IN_COMMAND_MODE == okc_is_robot_in_command_mode (okc,robot_id)) { fprintf (stderr,"okc_request_monitor_mode: robot already is in monitor " "mode!\n"); return (OKC_INAPPROPIRATE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_monitor_mode: pthread_rwlock_rdlock"); return (OKC_ERROR); } if (0 != okc->command[robot_id]) fprintf (stderr,"DEBUG: command '%s' is still pending\n",_command_to_str (okc->command[robot_id])); while (0 != okc->command[robot_id]) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_monitor_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_monitor_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } } okc->command[robot_id] = OKC_FRI_STOP; while (0 != (okc->command[robot_id]&OKC_COMMAND_MASK)) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_monitor_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_monitor_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_request_monitor_mode: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Checks, if a certain robot_id has drives powered on, i.e. that any * drive is powered on, since the power-bitfield is * undocumented. Returns #OKC_OK if robot_id is connected and power is * on, or #OKC_UNAVAILABLE if no physical robot has connected to * robot_id or #OKC_DRIVES_POWER_OFF if robot_id has all drives * powered off. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to check. * * @return #OKC_OK, if robot is connected, #OKC_UNAVAILABLE if no * robot is currently connected, #OKC_DRIVES_POWER_OFF, if all * drives are powered off or #OKC_ERROR on error. * */ int okc_is_power_on (okc_handle_t* okc, int robot_id) { int i; assert (NULL != okc); if (robot_id >= OKC_MAX_ROBOTS) { fprintf (stderr,"okc_is_power_on: invalid robot_id %d. " "Max is %d\n", robot_id, OKC_MAX_ROBOTS - 1); return OKC_ERROR; } if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_is_power_on: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_is_power_on: pthread_rwlock_rdlock"); return (OKC_ERROR); } i = okc->state[robot_id].power; if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_is_power_on: pthread_rwlock_unlock"); return (OKC_ERROR); } if (i > 0) return OKC_OK; return OKC_DRIVES_POWER_OFF; } /** * With this function, which is only available if #OKC_MODE_SETTER_POS * was used when creaing the okc server, the cartesian trajectorie * correction can be set. The correction is valid and will be * communicated to the robot as long as it is set. Currently, there is * no way to tell that the correction did actually take place. One has * to contol the robots position through okc_get_coords_act() and * possibly adapt to the robots IPO-Cycle by, for example, a timer, or * use the OKC_MODE_CALLBACK_* modes for tight real time control. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param k_correction Cartesian corrections. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), * #OKC_NOT_IN_COMMAND_MODE if the robot is currently in * monitor mode, #OKC_DRIVES_POWER_OFF if robots drives are * not powered or #OKC_ERROR on error. * */ int okc_set_coords_correction (okc_handle_t* okc, int robot_id, coords_t k_correction) { lbr_axis_t l; assert (NULL != okc); okc_axis_set_all_zero (&l); if (OKC_MODE_SETTER != (okc->mode&OKC_MODE_SETTER)) { fprintf (stderr,"okc_set_coords_correction: function only available in" " OKC_MODE_SETTER_*-mode!\n"); return (OKC_ERROR); } if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_set_coords_correction: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_OK != okc_is_robot_in_command_mode (okc, robot_id)) { fprintf (stderr, "okc_set_coords_correction: robot not in command mode\n"); return (OKC_NOT_IN_COMMAND_MODE); } if (OKC_OK != okc_is_power_on (okc, robot_id)) { fprintf (stderr, "okc_set_coords_correction: drives are powered off\n"); return (OKC_DRIVES_POWER_OFF); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_coords_correction: pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->pos_correction[robot_id] = k_correction; okc->axis_correction[robot_id] = l; if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_coords_correction: pthread_rwlock_wrlock"); return (OKC_ERROR); } return (OKC_OK); } /** * With this function, which is only available if OKC_MODE_SETTER_AXIS was * used when creaing the okc server, the axis trajectorie correction * can be set. The correction is valid and will be communicated to the * robot as long as it is set. Currently, there is no way to tell * that the correction did take place. One has to contol the robots * position through okc_get_axis_act() and possibly adapt to the robots * IPO-Cycle by, for example, a timer, or use the OKC_MODE_CALLBACK_* * modes to do tight real time control. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param a_correction Axis corrections. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), * #OKC_NOT_IN_COMMAND_MODE if the robot is currently in * monitor mode, #OKC_DRIVES_POWER_OFF if robots drives are * not powered or #OKC_ERROR on error. * */ int okc_set_axis_correction (okc_handle_t* okc, int robot_id, lbr_axis_t a_correction) { coords_t k; assert (NULL != okc); okc_coords_set_all_zero (&k); if (OKC_MODE_SETTER != (okc->mode&OKC_MODE_SETTER)) { fprintf (stderr,"okc_set_axis_correction: function only available in" " OKC_MODE_SETTER_*-mode!\n"); return (OKC_ERROR); } if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_set_axis_correction: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_OK != okc_is_robot_in_command_mode (okc, robot_id)) { fprintf (stderr, "okc_set_axis_correction: robot not in command mode\n"); return (OKC_NOT_IN_COMMAND_MODE); } if (OKC_OK != okc_is_power_on (okc, robot_id)) { fprintf (stderr, "okc_set_axis_correction: drives are powered off\n"); return (OKC_DRIVES_POWER_OFF); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_correction: pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->pos_correction[robot_id] = k; okc->axis_correction[robot_id] = a_correction; if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_correction: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * With this function, the commanded fri corrections can be queried, * which corresponds to the cmdJntPosFriOffset field in the FRI * documentation. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param a_correction Axis corrections. * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE * if robot is unavailable (disconnected) or #OKC_ERROR on error. * */ int okc_get_fri_axis_correction (okc_handle_t* okc, int robot_id, lbr_axis_t* a_correction) { int i; assert (NULL != okc); assert (NULL != a_correction); if (okc->mode != OKC_MODE_SETTER_AXIS) { fprintf (stderr,"okc_get_fri_axis_correction: function only available in" " OKC_MODE_SETTER-mode!\n"); return (OKC_ERROR); } if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_fri_axis_correction: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_fri_axis_correction: pthread_rwlock_wrlock"); return (OKC_ERROR); } for (i = 0; i < LBR_MNJ; i++) { ((fri_float_t*) a_correction)[i] = okc->robot_data[robot_id].cmdJntPosFriOffset[i]; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_fri_axis_correction: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Sets the axis stiffness and damping parameters. This function will * trigger a function call on the robots side, if neccessary or simply * modify the internal okc settings if the command flags are set in a * way that allows OpenKC to change stiffness and damping online, and * returns when new Stiffness and Damping are successfully changed on * the robot. Stiffness values are rounded to the nearest integer. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param a_stiff Stiffness for each axis. Consult the KUKA LBR * manual for details. Range is 1 (or #OKC_AXIS_STIFFNESS_MIN) -- * 2000.0 (or #OKC_AXIS_STIFFNESS_MAX), Default 1000.0 * (#OKC_OKC_AXIS_STIFFNESS_DEFAULT) * @param a_damp Dampings for each axis. Consult the KUKA LBR manual for * details. Range is 0.1 (#OKC_AXIS_DAMPING_MIN) -- 1.0 * (#OKC_DAMPING_MAX), Default 0.7 (#OKC_AXIS_DAMPING_DEFAULT) * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_EINVAL if input * is out of range, #OKC_INAPPROPRIATE if called in wrong * mode context or #OKC_ERROR on error. * * */ int okc_set_axis_stiffness_damping (okc_handle_t* okc, int robot_id, lbr_axis_t a_stiff, lbr_axis_t a_damp) { int i; int command_mode = 0; assert (NULL != okc); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_set_axis_stiffness_damping: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_OK != okc_check_axis_stiffness_value_range (a_stiff)) { fprintf (stderr, "okc_set_axis_stiffness_damping: invalid stiffness requested\n"); return (OKC_EINVAL); } if (OKC_OK != okc_check_axis_damping_value_range (a_damp)) { fprintf (stderr, "okc_set_axis_stiffness_damping: invalid damping requested\n"); return (OKC_EINVAL); } command_mode = okc_is_robot_in_command_mode (okc,robot_id); if ((command_mode != OKC_OK) && (command_mode != OKC_NOT_IN_COMMAND_MODE)) { fprintf (stderr, "okc_set_axis_stiffness_damping: could not resolve mode\n"); return (OKC_ERROR); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_wrlock"); return (OKC_ERROR); } if ((FRI_CMD_JNTSTIFF != (okc->command_flags[robot_id] & FRI_CMD_JNTSTIFF)) || (FRI_CMD_JNTDAMP != (okc->command_flags[robot_id] & FRI_CMD_JNTDAMP))) { if (command_mode == OKC_OK) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } fprintf (stderr,"okc_set_axis_stiffness_damping: inapproriate mode\n"); return (OKC_INAPPROPIRATE); } while (0 != okc->command[robot_id]) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } } okc->command[robot_id] = OKC_SET_AXIS_STIFFNESS_DAMPING; } else { if (OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS == (okc->mode&OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS)) { fprintf (stderr,"okc_set_axis_stiffness_damping: failed: tried to call" " okc_set_axis_stiffnes_damping when in " "OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS\n"); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_INAPPROPIRATE); } for (i = 0; i < LBR_MNJ; i++) { okc->axis_stiffness[robot_id][i] = ((fri_float_t*)&a_stiff)[i]; okc->axis_damping[robot_id][i] = ((fri_float_t*)&a_damp)[i]; } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } for (i = 0; i < LBR_MNJ; i++) { okc->axis_stiffness[robot_id][i] = ((fri_float_t*)&a_stiff)[i]; okc->axis_damping[robot_id][i] = ((fri_float_t*)&a_damp)[i]; } while (0 != (okc->command[robot_id]&OKC_COMMAND_MASK)) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_axis_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * This function is deprecated and will do nothing! */ int okc_set_axis_stiffness (okc_handle_t* okc, int robot_id, lbr_axis_t a_stiff) { fprintf (stderr,"okc_set_axis_stiffness: function deprecated\n"); return (OKC_DEPRECATED); } /** * This function is deprecated and will do nothing! */ int okc_set_axis_damping (okc_handle_t* okc, int robot_id, lbr_axis_t a_damp) { fprintf (stderr,"okc_set_axis_damping: function deprecated\n"); return (OKC_DEPRECATED); } /** * Sets the cartesian stiffness and damping parameters. This function * will trigger a function call on the robots side, if neccessary or * simply modify the internal okc settings if the command flags are * set in a way that allows OpenKC to change stiffness and damping * online, and returns when new Stiffness and Damping are successfully * changed on the robot. Stiffness values are rounded to the nearest * integer. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @param k_stiff Stiffness for each component. Consult the KUKA LBR * manual for details. Range is * 0.01(#OKC_CART_XYZ_STIFFNESS_MIN) -- * 5000.0(#OKC_CART_XYZ_STIFFNESS_MAX), Default * 2000.0(#OKC_CART_XYZ_STIFFNESS_DEFAULT) for X,Y,Z and * 0.01(#OKC_CART_ABC_STIFFNESS_MIN) -- * 300(#OKC_CART_ABC_STIFFNESS_MAX) (Default * 200(#OKC_CART_ABC_STIFFNESS_DEFAULT)) for A,B and C. * * @param k_damp Dampings for each cartesian component. Consult the * KUKA LBR manual for details. Range is 0.1 * (#OKC_CART_ABC_DAMPING_MIN / #OKC_CART_XYZ_DAMPING_MIN) -- * 1.0 (#OKC_CART_ABC_DAMPING_MAX / #OKC_CART_XYZ_DAMPING_MAX), * Default 0.7 (#OKC_CART_ABC_DAMPING_DEFAULT / * #OKC_CART_XYZ_DAMPING_DEFAULT) * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_EINVAL if input * is out of range, #OKC_INAPPROPRIATE if called in wrong * mode context or #OKC_ERROR on error. * */ int okc_set_cp_stiffness_damping (okc_handle_t* okc, int robot_id, coords_t k_stiff, coords_t k_damp) { int i; int command_mode; assert (NULL != okc); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_set_cp_stiffness_damping: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_OK != okc_check_cp_stiffness_value_range (k_stiff)) { fprintf (stderr, "okc_set_cp_stiffness_damping: invalid stiffness requested\n"); return (OKC_EINVAL); } if (OKC_OK != okc_check_cp_damping_value_range (k_damp)) { fprintf (stderr, "okc_set_cp_stiffness_damping: invalid damping requested\n"); return (OKC_EINVAL); } command_mode = okc_is_robot_in_command_mode (okc,robot_id); if ((command_mode != OKC_OK) && (command_mode != OKC_NOT_IN_COMMAND_MODE)) { fprintf (stderr, "okc_set_cp_stiffness_damping: could not resolve mode\n"); return (OKC_ERROR); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_wrlock"); return (OKC_ERROR); } if ((FRI_CMD_CARTSTIFF != (okc->command_flags[robot_id] & FRI_CMD_CARTSTIFF))|| (FRI_CMD_CARTDAMP != (okc->command_flags[robot_id] & FRI_CMD_CARTDAMP))) { if (command_mode == OKC_OK) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } fprintf (stderr,"okc_set_cp_stiffness_damping: inapproriate mode\n"); return (OKC_INAPPROPIRATE); } while (0 != okc->command[robot_id]) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } } okc->command[robot_id] = OKC_SET_CP_STIFFNESS_DAMPING; } else { if (OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS == (okc->mode&OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS)) { fprintf (stderr,"okc_set_cp_stiffness_damping: failed: tried to call" " okc_set_cp_stiffnes_damping when in " "OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS\n"); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_INAPPROPIRATE); } for (i = 0; i < FRI_CART_VEC; i++) { okc->cart_stiffness[robot_id][i] = ((fri_float_t*)&k_stiff)[i]; okc->cart_damping[robot_id][i] = ((fri_float_t*)&k_damp)[i]; } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } for (i = 0; i < FRI_CART_VEC; i++) { okc->cart_stiffness[robot_id][i] = ((fri_float_t*)&k_stiff)[i]; okc->cart_damping[robot_id][i] = ((fri_float_t*)&k_damp)[i]; } while (0 != (okc->command[robot_id]&OKC_COMMAND_MASK)) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_stiffness_damping: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } int okc_set_cp_addTcpFT (okc_handle_t* okc, int robot_id, coords_t ExtTcpFT){ int i; int command_mode; assert (NULL != okc); // printf("into set external force call"); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_set_cp_addTcpFT: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } command_mode = okc_is_robot_in_command_mode (okc,robot_id); if ((command_mode != OKC_OK) && (command_mode != OKC_NOT_IN_COMMAND_MODE)) { fprintf (stderr, "okc_set_cp_addTcpFT: could not resolve mode\n"); return (OKC_ERROR); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_wrlock"); return (OKC_ERROR); } if ((FRI_CMD_CARTSTIFF != (okc->command_flags[robot_id] & FRI_CMD_CARTSTIFF))|| (FRI_CMD_CARTDAMP != (okc->command_flags[robot_id] & FRI_CMD_CARTDAMP))) { if (command_mode == OKC_OK) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } fprintf (stderr,"okc_set_cp_addTcpFT: inapproriate mode\n"); return (OKC_INAPPROPIRATE); } while (0 != okc->command[robot_id]) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } } okc->command[robot_id] = OKC_SET_CP_STIFFNESS_DAMPING; } else { if (OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS == (okc->mode&OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS)) { fprintf (stderr,"okc_set_cp_addTcpFT: failed: tried to call" " okc_set_cp_addTcpFT when in " "OKC_MODE_CALLBACK_POS_AXIS_IMPEDANCE_ABS\n"); if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_INAPPROPIRATE); } for (i = 0; i < FRI_CART_VEC; i++) { okc->add_tcp_ft[robot_id][i] = ((fri_float_t*)&ExtTcpFT)[i]; } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } for (i = 0; i < FRI_CART_VEC; i++) { okc->add_tcp_ft[robot_id][i] = ((fri_float_t*)&ExtTcpFT)[i]; } while (0 != (okc->command[robot_id]&OKC_COMMAND_MASK)) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_set_cp_addTcpFT: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * This function is deprecated and will do nothing! */ int okc_set_cp_stiffness (okc_handle_t* okc, int robot_id, coords_t k_stiff) { fprintf (stderr,"okc_set_cp_stiffness: function deprecated\n"); return (OKC_DEPRECATED); } /** * This function is deprecated and will do nothing! */ int okc_set_cp_damping (okc_handle_t* okc, int robot_id, coords_t k_damp) { fprintf (stderr,"okc_set_cp_stiffness: function deprecated\n"); return (OKC_DEPRECATED); } /** * Will switch to different control modes (#OKC_SWITCH_AXIS_CONTROL, * #OKC_SWITCH_POSITION, #OKC_SWITCH_GRAVCOMP or * #OKC_SWITCH_CP_CONTROL). This will call a function in KRL, and * switch the controller on the Kuka side. This cannot currently be * done in command mode, so this function will exit with * #OKC_INAPPROPRIATE if you try that. You have to be in monitor mode * to be successful. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @param command must be any of #OKC_SWITCH_AXIS_CONTROL, * #OKC_SWITCH_POSITION, #OKC_SWITCH_GRAVCOMP or * #OKC_SWITCH_CP_CONTROL. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_EINVAL if input * is out of range, #OKC_INAPPROPRIATE if called in wrong * mode context or #OKC_ERROR on error. * */ int okc_switch_to_control (okc_handle_t* okc, int robot_id, int command) { assert (NULL != okc); if ((command != OKC_SWITCH_AXIS_CONTROL) && (command != OKC_SWITCH_CP_CONTROL) && (command != OKC_SWITCH_GRAVCOMP) && (command != OKC_SWITCH_POSITION)) { fprintf (stderr,"okc_switch_to_control: unknown command: %d\n", command); return (OKC_EINVAL); } if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_switch_to_control: invalid/unavailable " "robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_NOT_IN_COMMAND_MODE != okc_is_robot_in_command_mode (okc, robot_id)) { fprintf (stderr, "okc_switch_to_control: robot not in monitor mode\n"); return (OKC_INAPPROPIRATE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_switch_to_control: pthread_rwlock_wrlock"); return (OKC_ERROR); } while (0 != okc->command[robot_id]) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_switch_to_control: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_switch_to_control: pthread_rwlock_unlock"); return (OKC_ERROR); } } okc->command[robot_id] = command; while (0 != (okc->command[robot_id]&OKC_COMMAND_MASK)) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_switch_to_control: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_switch_to_control: pthread_rwlock_unlock"); return (OKC_ERROR); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_switch_to_control: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Use this function to set or alter the command flags. You should * read the FRI documentation on details on the command flags. OpenKC * supplies some recommended command flags to the available callback * functions (#OKC_CMD_FLAGS_POSITION_CONTROL_MODE, * #OKC_CMD_FLAGS_AXIS_IMPEDANCE_MODE, * #OKC_CMD_FLAGS_CP_IMPEDANCE_MODE), but basically, it is up to you * to set the command flags the way you want them to. Remember: your * milage may vary ;-) * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @param newFlags The new flags, which will NOT get checked against * fricomm.h, because, as fricomm.h states, the interface may * change, blablabla. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_INAPPROPRIATE if * called in wrong mode context or #OKC_ERROR on error. * */ int okc_alter_cmdFlags (okc_handle_t* okc, int robot_id, int newFlags) { assert (NULL != okc); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_alter_cmdFlags: invalid/unavailable " "robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_NOT_IN_COMMAND_MODE != okc_is_robot_in_command_mode (okc, robot_id)) { fprintf (stderr, "okc_alter_cmdFlags: robot not in monitor mode\n"); return (OKC_INAPPROPIRATE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_alter_cmdFlags: pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->command_flags[robot_id] = newFlags; if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_alter_cmdFlags: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } int okc_alter_cbmode (okc_handle_t* okc, int robot_id, int cbmode){ assert (NULL != okc); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_alter_cbmode: invalid/unavailable " "robot_id\n"); return (OKC_UNAVAILABLE); } if (OKC_NOT_IN_COMMAND_MODE != okc_is_robot_in_command_mode (okc, robot_id)) { fprintf (stderr, "okc_alter_cbmode: robot not in monitor mode\n"); return (OKC_INAPPROPIRATE); } if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_alter_cbmode: pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->mode = cbmode; if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("okc_alter_cbmode: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Switch to cartesian impedance control. This function is here to * maintain compatibility to former releases of OpenKC. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_EINVAL if input * is out of range, #OKC_INAPPROPRIATE if called in wrong * mode context or #OKC_ERROR on error. * * @see okc_switch_to_control() */ int okc_switch_to_cp_impedance (okc_handle_t* okc, int robot_id) { return okc_switch_to_control (okc, robot_id, OKC_SWITCH_CP_CONTROL); } /** * Switch to axis impedance control. This function is here to * maintain compatibility to former releases of OpenKC. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_EINVAL if input * is out of range, #OKC_INAPPROPRIATE if called in wrong * mode context or #OKC_ERROR on error. * * @see okc_switch_to_control() */ int okc_switch_to_axis_impedance (okc_handle_t* okc, int robot_id) { return okc_switch_to_control (okc, robot_id, OKC_SWITCH_AXIS_CONTROL); } /** * Switch to gravitation compensation mode. This function is here to * maintain compatibility to former releases of OpenKC. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_EINVAL if input * is out of range, #OKC_INAPPROPRIATE if called in wrong * mode context or #OKC_ERROR on error. * * @see okc_switch_to_control() */ int okc_switch_to_gravcomp (okc_handle_t* okc, int robot_id) { return okc_switch_to_control (okc, robot_id, OKC_SWITCH_GRAVCOMP); } /** * Switch to position control mode. This function is here to * maintain compatibility to former releases of OpenKC. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE if * robot is unavailable (disconnected), #OKC_EINVAL if input * is out of range, #OKC_INAPPROPRIATE if called in wrong * mode context or #OKC_ERROR on error. * * @see okc_switch_to_control() */ int okc_switch_to_position (okc_handle_t* okc, int robot_id) { return okc_switch_to_control (okc, robot_id, OKC_SWITCH_POSITION); } /** * Tells the server, to send a break signal to the Robot. This function is deprecated as of FRI. * * @return #OKC_DEPRICATED */ int okc_send_break_to_robot (okc_handle_t* okc, int robot_id) { fprintf (stderr,"okc_send_break_to_robot: function deprecated\n"); return (OKC_DEPRECATED); } /** * Gets the current connection quality (as of the last received packet). See * fricomm.h for details. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param quality Connection Quality (0 (worst) to 3 (best) (as time of writing) * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE * if robot is unavailable (disconnected) or #OKC_ERROR on error. */ int okc_get_connection_quality (okc_handle_t* okc, int robot_id, int* quality) { assert (NULL != okc); assert (NULL != quality); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_connection_quality: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_connection_quality: pthread_rwlock_rdlock"); return (OKC_ERROR); } (*quality) = okc->connection_stats[robot_id].quality; if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_connection_quality: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Gets the last (actual) reported axis positions (msrJntPos) from the robot. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param l pointer to #lbr_axis_t where the actual values will be copied. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE * if robot is unavailable (disconnected) or #OKC_ERROR on error. */ int okc_get_axis_act (okc_handle_t* okc, int robot_id, lbr_axis_t* l) { int i; assert (NULL != okc); assert (NULL != l); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_axis_act: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_axis_act: pthread_rwlock_rdlock"); return (OKC_ERROR); } for (i = 0; i < LBR_MNJ; i++) { ((fri_float_t*)l)[i] = okc->robot_data[robot_id].msrJntPos[i]; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_axis_act: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Gets the last (actual) reported axis positions (msrJntPos) from the robot. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param l pointer to #fri_float_t array where the actual values will be copied. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE * if robot is unavailable (disconnected) or #OKC_ERROR on error. */ int okc_get_jntpos_act (okc_handle_t* okc, int robot_id, fri_float_t* l) { assert (NULL != okc); assert (NULL != l); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_axis_act: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_axis_act: pthread_rwlock_rdlock"); return (OKC_ERROR); } okc_cp_lbr_mnj (okc->robot_data[robot_id].msrJntPos, l); if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_axis_act: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Gets the last reported *desired* (cmdJntPos + cmdJntPosFriOffset) * or set axis positions from the robot. Depending on the control * mode, these values can be significantly different from the actual * joint values. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param l pointer to #lbr_axis_t where the desired values will be copied. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE * if robot is unavailable (disconnected) or #OKC_ERROR on error. */ int okc_get_axis_des (okc_handle_t* okc, int robot_id, lbr_axis_t* l) { int i; assert (NULL != okc); assert (NULL != l); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_axis_des: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_axis_des: pthread_rwlock_rdlock"); return (OKC_ERROR); } for (i = 0; i < LBR_MNJ; i++) { ((fri_float_t*)l)[i] = okc->robot_data[robot_id].cmdJntPos[i] + okc->robot_data[robot_id].cmdJntPosFriOffset[i]; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_axis_des: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Gets the last (actual) reported axis torques (estExtJntTrq) from the robot. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param l pointer to #lbr_axis_t where the actual values will be copied. * * @return OKC_OK, if setting was successful, OKC_UNAVAILABLE * if robot is unavailable (disconnected) or OKC_ERROR on error. */ int okc_get_torques_axis_act (okc_handle_t* okc, int robot_id, lbr_axis_t* l) { int i; assert (NULL != okc); assert (NULL != l); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_torques_axis_act: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_torques_axis_act: pthread_rwlock_rdlock"); return (OKC_ERROR); } for (i = 0; i < LBR_MNJ; i++) { ((fri_float_t*)l)[i] = okc->robot_data[robot_id].estExtJntTrq[i]; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_torques_axis_act: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Gets the last (actual) reported cartesian position (msrCartPos) of the * endeffector (+ tool) from the robot. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param k pointer to #coords_t where the actual values will be copied. * * @return OKC_OK, if setting was successful, OKC_UNAVAILABLE * if robot is unavailable (disconnected) or OKC_ERROR on error. */ int okc_get_position_act (okc_handle_t* okc, int robot_id, transform_t* t) { int i; assert (NULL != okc); assert (NULL != t); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_coords_act: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_coords_act: pthread_rwlock_rdlock"); return (OKC_ERROR); } for (i = 0; i < FRI_CART_FRM_DIM; i++) { ((fri_float_t*)t)[i] = okc->robot_data[robot_id].msrCartPos[i]; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_coords_act: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Gets the last *desired* or set cartesian position (cmdCartPos + * cmdCartPosFriOffset, btw. is this really correct???) of the * endeffector (+ tool) from the robot. Depending on the control mode, * these values can be significantly different from the actual * positions. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param k pointer to #coords_t where the actual values will be copied. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE * if robot is unavailable (disconnected) or #OKC_ERROR on error. */ int okc_get_coords_des (okc_handle_t* okc, int robot_id, coords_t* k) { int i; assert (NULL != okc); assert (NULL != k); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_coords_des: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_coords_des: pthread_rwlock_rdlock"); return (OKC_ERROR); } for (i = 0; i < FRI_CART_VEC; i++) { ((fri_float_t*)k)[i] = okc->robot_data[robot_id].cmdCartPos[i]+ okc->robot_data[robot_id].cmdCartPosFriOffset[i]; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_coords_des: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Gets the last (actual) reported (cartesian) torques (estExtTcpFT) from the * endeffector. These torques are estimates only from the axis torque * sensor readings reported by the KRC. In some robot * configurations/positions, these estimates may not be anywhere * reliable. So be cautious. Usually, one should have a close look at * the variance of the estimated values, but due to limitations in * RSI-XML we cannot provide the internal variances from the KRC to * the PC. * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * @param k pointer to #coords_t where the actual values will be copied. * * @return #OKC_OK, if setting was successful, #OKC_UNAVAILABLE * if robot is unavailable (disconnected) or #OKC_ERROR on error. */ int okc_get_ft_tcp_est (okc_handle_t* okc, int robot_id, coords_t* k) { int i; assert (NULL != okc); assert (NULL != k); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_ft_tcp_est: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_ft_tcp_est: pthread_rwlock_rdlock"); return (OKC_ERROR); } for (i = 0; i < FRI_CART_VEC; i++) { ((fri_float_t*)k)[i] = okc->robot_data[robot_id].estExtTcpFT[i]; } if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_ft_tcp_est: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Function somewhat internal to OpenKC. Sets the command to "exit gracefully", * so the KRL programm will terminate in a nice way ;-) * * @param okc Pointer to #okc_handle_t. * @param robot_id Robot ID to use. * * @return #OKC_OK, or #OKC_ERROR on error. */ static int _okc_send_exit_gracefully (okc_handle_t* okc, int robot_id) { assert (NULL != okc); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_send_exit_gracefully: pthread_rwlock_wrlock"); return (OKC_ERROR); } while (0 != okc->command[robot_id]) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_send_exit_gracefully: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_wrlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_send_exit_gracefully: pthread_rwlock_unlock"); return (OKC_ERROR); } } okc->command[robot_id] = OKC_FRI_STOP; while (OKC_FRI_STOP == (okc->command[robot_id]&OKC_COMMAND_MASK)) { if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_send_exit_gracefully: pthread_rwlock_unlock"); return (OKC_ERROR); } okc_sleep_cycletime (okc, robot_id); if (0 != pthread_rwlock_rdlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_send_exit_gracefully: pthread_rwlock_unlock"); return (OKC_ERROR); } } if (0 != pthread_rwlock_unlock (&okc->pos_axis_in_lock[robot_id])) { perror ("_okc_send_exit_gracefully: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * This will stop the server thread and send an "exit gracefully" to * all attached robots. The KRL programms on the attached robot(s) * should terminate nicely. When function returns, server thread has * terminated. Everything should be nice and clean then. * * @param okc Pointer to #okc_handle_t. * * @return always OKC_OK. * @see okc_start_server */ int okc_stop_server (okc_handle_t* okc) { struct timespec ts; int i; assert (NULL != okc); for (i = 0; i < OKC_MAX_ROBOTS; i++) { if (OKC_OK == okc_is_robot_avail (okc,i)) { _okc_send_exit_gracefully (okc,i); } } ts.tv_sec = 2; ts.tv_nsec = 0; nanosleep (&ts,NULL); ts.tv_sec = 2; ts.tv_nsec = 0; nanosleep (&ts,NULL); okc->flags = OKC_QUIT; pthread_join ((*okc->comm_thread), NULL); okc_destroy_okc_handle_t (okc); return (OKC_OK); } /** * This interal function will mlockall() memory, if possible and available. * Will also reserve stack data, so hopefully page faults are at a minimum. */ static int _mlockall_reserve_stack () { #ifdef _POSIX_MEMLOCK long a[1000000]; long i; if (_POSIX_MEMLOCK > 0) { if (0 != mlockall (MCL_FUTURE)) { perror ("openkc: could not mlockall"); return OKC_ERROR; } for (i = 0; i < 1000000; i++) a[i] = i; } return OKC_OK; #else fprintf (stderr,"openkc: mlockall not supported!\n"); return OKC_ERROR; #endif } /** * This will start the server thread. The server will from then on * listen to the given port for robots that are connecting. When a * robot connects, the server thread will take care of answering in * time. * * Basically, there are two possible operation modes. Either, you * choose #OKC_MODE_SETTER_AXIS or #OKC_MODE_SETTER_POS. In these modes, * all corrections must be done using okc_set_axis_correction() or * okc_set_coords_correction(). You cannot mix calls to these functions. * When using these modes, you will have to take care of * "syncing" with the IPO-Cycle of the robot in order to do propper * control. The use of a timer can help here, to avoid "busy * waiting". * * The alternative is to register a callback-function and set either * #OKC_MODE_CALLBACK_AXIS or #OKC_MODE_CALLBACK_POS. In these modes, * you have to register a callback function (refresh_correction), * which will get the priv_for_refresh_correction as first argument * (to access custom objects from within the callback function, for * example, certain regions in the memory where a trajectorie planner * puts the desired values. The next two parameters depend on the * mode. In _AXIS mode, a pointer to a fri_float_t array of size 7, which * may be casted to a lbr_axis_t struct, is passed holding the actual * axis positions, and a pointer to a fri_float_t array of the same length * is passed, where the callback function should write the new * corrections to. In _POS mode, the array size is 6, and the fri_float_t* * may be casted to a coords_t struct. When in callback mode, one should * consider, that the server thread calls the callback after receiving * an XML message from the robot but before an answer is generated and * send. So, the callback routine should be "fast", otherwise, you * will feel the pain of soft realtime. * * @param host IP adress or hostname to bind to (in case, you have more than * one IP, which might be useful when serving the robots). * @param port Port to listen to. Per default, this should be 6075 in OpenKC. * @param mode Can be either OKC_MODE_SETTER or OKC_MODE_CALLBACK_* * * @return Pointer to #okc_handle_t. * @see okc_stop_server() * @see okc_set_coords_correction() * @see okc_set_axis_correction() * @see okc_get_coords_act() * @see okc_get_axis_act() */ okc_handle_t* okc_start_server (const char* host,const char* port, int mode) { okc_handle_t* okch; int listener; int error; int yes=1; int rv; int i; struct addrinfo hints, *ai, *p; pthread_attr_t thread_attr; struct sched_param param; int policy,result; assert (NULL != host); assert (NULL != port); FRI_PREPARE_CHECK_BYTE_ORDER; if (!FRI_CHECK_BYTE_ORDER_OK) { fprintf (stderr,"okc_start_server: failed byte order check, " "bailing out.\n"); return (NULL); } if (!FRI_CHECK_SIZES_OK) { fprintf (stderr,"okc_start_server: failed struct size check, " "bailing out.\n"); return (NULL); } fprintf (stderr,"Setting up Server at %s\n",host); okch = okc_malloc_init_okc_handle_t(); if (NULL == okch) { fprintf (stderr,"okc_start_server: error: could not create okc_handle," " giving up!\n"); return (NULL); } okch->mode = mode; switch (mode) { case OKC_MODE_CALLBACK_AXIS_ABS: for (i = 0; i < OKC_MAX_ROBOTS; i++) okch->command_flags[i] = FRI_CMD_JNTPOS; break; case OKC_MODE_CALLBACK_POS_ABS: for (i = 0; i < OKC_MAX_ROBOTS; i++) okch->command_flags[i] = FRI_CMD_CARTPOS; break; default: for (i = 0; i < OKC_MAX_ROBOTS; i++) okch->command_flags[i] = 0; } memset(&hints, 0, sizeof (hints)); hints.ai_family = AF_INET; hints.ai_socktype = SOCK_DGRAM; if ((rv = getaddrinfo(host, port, &hints, &ai)) != 0) { fprintf(stderr, "okc_start_server: %s\n", gai_strerror(rv)); okc_destroy_okc_handle_t (okch); return (NULL); } for(p = ai; p != NULL; p = p->ai_next) { listener = socket (p->ai_family, p->ai_socktype, p->ai_protocol); if (listener < 0) { perror ("okc_start_server: socket"); continue; } if (0 != setsockopt(listener, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int))) { perror ("okc_start_server: info: setsockopt: SO_REUSADDR"); } if (0 != setsockopt(listener, SOL_SOCKET, SO_TIMESTAMP, &yes, sizeof(int))) { perror ("okc_start_server: info: setsockopt: SO_TIMESTAMP"); } if (bind(listener, p->ai_addr, p->ai_addrlen) < 0) { perror ("okc_start_server: bind"); close(listener); continue; } break; } if (p == NULL) { fprintf(stderr, "okc_start_server: failed to bind\n"); okc_destroy_okc_handle_t (okch); return (NULL); } freeaddrinfo(ai); if (0 != pthread_attr_init (&thread_attr)) { perror ("okc_start_server: pthread_attr_init"); okc_destroy_okc_handle_t (okch); return (NULL); } if (0 != pthread_attr_setschedpolicy (&thread_attr, SCHED_FIFO)) { fprintf (stderr,"okc_start_server: warning: " "no SCHED_FIFO available\n"); } if (0 != pthread_attr_setinheritsched (&thread_attr, PTHREAD_EXPLICIT_SCHED)) { fprintf (stderr,"okc_start_server: warning: requesting " "PTHREAD_EXPLICIT_SCHED failed\n"); } param.sched_priority = 90; if (0 != pthread_attr_setschedparam(&thread_attr, ¶m)) { fprintf (stderr,"okc_start_server: warning: " "no SCHED_PRIORITY available\n"); } okch->listener = listener; if (0 != (error = pthread_create (okch->comm_thread, &thread_attr, _serve_connections, (void*) okch))) { if (error == EPERM) { fprintf (stderr,"okc_start_server: warning: could not create realtime " "priority " "thread: reason: permissions. Will create normal priority " "thread.\n"); if (0 != (error = pthread_create (okch->comm_thread, NULL, _serve_connections, (void*) okch))) { printf ("okc_start_server: error: could not create thread: %s", (error == EAGAIN)?"EAGAIN":"EINVAL"); okc_destroy_okc_handle_t (okch); pthread_attr_destroy (&thread_attr); return (NULL); } } else { fprintf (stderr,"okc_start_server: error: could not create thread: %s", (error == EAGAIN)?"EAGAIN":"EINVAL"); okc_destroy_okc_handle_t (okch); pthread_attr_destroy (&thread_attr); return (NULL); } } if (0 != (result = pthread_getschedparam (*okch->comm_thread,&policy, ¶m))) { perror ("okc_start_server: pthread_getschedparam"); fprintf (stderr,"result was %d\n",result); } else { fprintf (stderr,"okc_start_server: info: thread is in class " "%s at priority %d\n", (policy==SCHED_OTHER)?"SCHED_OTHER":(policy==SCHED_FIFO)?"SCHED_FIFO":"SCHED_RR", param.sched_priority); } if (policy != SCHED_FIFO) { fprintf (stderr,"okc_start_server: info: altering thread priority\n"); policy = SCHED_FIFO; param.sched_priority = 90; if (0 != pthread_setschedparam (*okch->comm_thread,policy,¶m)) { perror ("okc_start_server: pthread_setschedparam"); fprintf (stderr,"okc_start_server: warning: failed to set up " "realtime task\n"); } if (0 != (result = pthread_getschedparam (*okch->comm_thread,&policy, ¶m))) { perror ("okc_start_server: pthread_getschedparam"); fprintf (stderr,"result was %d\n",result); } else { fprintf (stderr,"okc_start_server: info: thread is in class " "%s at priority %d\n", (policy==SCHED_OTHER)?"SCHED_OTHER":(policy==SCHED_FIFO)?"SCHED_FIFO":"SCHED_RR", param.sched_priority); } } pthread_attr_destroy (&thread_attr); /*_mlockall_reserve_stack();*/ /* Causes Problems with Kernel 2.6.37 */ return okch; } /** * Lets you register a callback function for absolute value axis control. * * @param okc Pointer to #okc_handle_t. * * @param robot_id Id of the robot you want to bind the callback function to. * * @param callback pointer to #okc_callback_axis_t function. * * @param private_pointer_for_callback pass a void pointer to the * callback function to do with it whatever you need. * * @return #OKC_OK on success or #OKC_ERROR on error. * */ int okc_register_axis_set_absolute_callback (okc_handle_t* okc, int robot_id, okc_callback_axis_t callback, void* private_pointer_for_callback) { if (0 != pthread_rwlock_wrlock (&okc->callback_lock[robot_id])) { perror ("okc_register_axis_set_absolute_callback: pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->axis_set_abs_cb[robot_id] = callback; okc->private_to_axis_set_abs_cb[robot_id] = private_pointer_for_callback; if (0 != pthread_rwlock_unlock (&okc->callback_lock[robot_id])) { perror ("okc_register_axis_set_absolute_callback: pthread_rwlock_unlock"); return (OKC_ERROR); } return OKC_OK; } /** * Lets you register a callback function for absolute value cartesian control. * * @param okc Pointer to #okc_handle_t. * * @param robot_id Id of the robot you want to bind the callback function to. * * @param callback pointer to #okc_callback_cartpos_t function. * * @param private_pointer_for_callback pass a void pointer to the * callback function to do with it whatever you need. * * @return #OKC_OK on success or #OKC_ERROR on error. * */ int okc_register_cartpos_set_absolute_callback (okc_handle_t* okc, int robot_id, okc_callback_cartpos_t callback, void* private_pointer_for_callback) { if (0 != pthread_rwlock_wrlock (&okc->callback_lock[robot_id])) { perror ("okc_register_cartpos_set_absolute_callback: " "pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->cartpos_set_abs_cb[robot_id] = callback; okc->private_to_cartpos_set_abs_cb[robot_id] = private_pointer_for_callback; if (0 != pthread_rwlock_unlock (&okc->callback_lock[robot_id])) { perror ("okc_register_cartpos_set_absolute_callback: " "pthread_rwlock_unlock"); return (OKC_ERROR); } return OKC_OK; } /** * Lets you register a callback function for absolute value cartesian * position and nullspace axis control. * * @param okc Pointer to #okc_handle_t. * * @param robot_id Id of the robot you want to bind the callback function to. * * @param callback pointer to #okc_callback_cartpos_axis_t function. * * @param private_pointer_for_callback pass a void pointer to the * callback function to do with it whatever you need. * * @return #OKC_OK on success or #OKC_ERROR on error. * */ int okc_register_cartpos_axis_set_absolute_callback (okc_handle_t* okc, int robot_id, okc_callback_cartpos_axis_t callback, void* private_pointer_for_callback) { if (0 != pthread_rwlock_wrlock (&okc->callback_lock[robot_id])) { perror ("okc_register_cartpos_axis_set_absolute_callback: " "pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->cartpos_axis_set_abs_cb[robot_id] = callback; okc->private_to_cartpos_axis_set_abs_cb[robot_id] = private_pointer_for_callback; if (0 != pthread_rwlock_unlock (&okc->callback_lock[robot_id])) { perror ("okc_register_cartpos_axis_set_absolute_callback: " "pthread_rwlock_unlock"); return (OKC_ERROR); } return OKC_OK; } /** * Lets you register a callback function for absolute value cartesian * position control with axis nullspace control and online setting of * impedance parameters. * * @param okc Pointer to #okc_handle_t. * * @param robot_id Id of the robot you want to bind the callback function to. * * @param callback pointer to #okc_callback_cartpos_axis_impedance_t function. * * @param private_pointer_for_callback pass a void pointer to the * callback function to do with it whatever you need. * * @return #OKC_OK on success or #OKC_ERROR on error. * */ int okc_register_cartpos_axis_impedance_set_absolute_callback (okc_handle_t* okc, int robot_id, okc_callback_cartpos_axis_impedance_t callback, void* private_pointer_for_callback) { if (0 != pthread_rwlock_wrlock (&okc->callback_lock[robot_id])) { perror ("okc_register_cartpos_axis_impedance_set_absolute_callback: " "pthread_rwlock_wrlock"); return (OKC_ERROR); } okc->cartpos_axis_impedance_set_abs_cb[robot_id] = callback; okc->private_to_cartpos_axis_impedance_set_abs_cb[robot_id] = private_pointer_for_callback; if (0 != pthread_rwlock_unlock (&okc->callback_lock[robot_id])) { perror ("okc_register_cartpos_axis_impedance_set_absolute_callback: " "pthread_rwlock_unlock"); return (OKC_ERROR); } return OKC_OK; } /** * Queries the cycle time, as the cycle time (desiredCmdSampleTime) is * set on the KRL side (FRIOPEN). If you want your program to work * with different cycle times, you will love this function ;-) * * @param okc Pointer to #okc_handle_t. * * @param robot_id Id of the robot you want to bind the callback function to. * * @param cycle_time Pointer to #fri_float_t, gives cycle time in seconds. * * @param private_pointer_for_callback pass a void pointer to the * callback function to do with it whatever you need. * * @return #OKC_OK on success, #OKC_UNAVAILABLE, if robot is not * connected or #OKC_ERROR on error. * */ int okc_get_cycle_time (okc_handle_t* okc, int robot_id, fri_float_t* cycle_time) { assert (NULL != okc); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_cycle_time: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_cycle_time: pthread_rwlock_rdlock"); return (OKC_ERROR); } (*cycle_time) = okc->connection_stats[robot_id].desiredCmdSampleTime; if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_get_cycle_time: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); } /** * Will fill the name parameter with the internal "name" of the robot, * which will be its IP adress. In a multi robot setup, you can use * this name to distinguish the robots from one another, since the * robot_ids are right now given on a first come - first serve basis. * * @param okc Pointer to #okc_handle_t. * * @param robot_id Id of the robot you want to bind the callback function to. * * @param name Pointer to a char array, which should at least be * #size_of_name long. * * @param size_of_name size of #name in bytes. * * @return #OKC_OK on success, #OKC_UNAVAILABLE, if robot is not * connected or #OKC_ERROR on error. * */ int okc_get_robot_name (okc_handle_t* okc, int robot_id, char* name, size_t size_of_name) { assert (NULL != okc); assert (NULL != name); if (OKC_OK != okc_is_robot_avail (okc,robot_id)) { fprintf (stderr, "okc_get_robot_name: invalid/unavailable robot_id\n"); return (OKC_UNAVAILABLE); } if (0 != pthread_rwlock_rdlock (okc->robot_map_names_lock)) { perror ("okc_get_robot_name: pthread_rwlock_rdlock"); return (OKC_ERROR); } strncpy (name,okc->robot_names[robot_id],size_of_name); if (0 != pthread_rwlock_unlock (okc->robot_map_names_lock)) { perror ("okc_get_robot_name: pthread_rwlock_unlock"); return (OKC_ERROR); } return (OKC_OK); }