/* * 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. * */ #include "fri_okc_types.h" #include #include #include #include #include void okc_coords_set_all_zero (coords_t* k) { k->x = k->y = k->z = k->a = k->b = k->c = 0.0; } void okc_axis_set_all_zero (lbr_axis_t* l) { l->a1 = l->a2 = l->a3 = l->a4 = l->a5 = l->a6 = l->e1 = 0.0; } void okc_copy_axis (const lbr_axis_t src, lbr_axis_t* dest) { memcpy (dest,&src,sizeof (lbr_axis_t)); } void okc_copy_coords (const coords_t src, coords_t* dest) { memcpy (dest,&src,sizeof (coords_t)); } void okc_print_coords (coords_t k) { printf ("x = %3.3f\ty = %3.3f\t z = %3.3f\n",k.x,k.y,k.z); printf ("a = %3.3f\tb = %3.3f\t c = %3.3f\n",k.a,k.b,k.c); } void okc_print_axis (lbr_axis_t l) { printf ("A1 = %3.3f\t A2 = %3.3f\t A3 = %3.3f\t A4 = %3.3f\n",l.a1, l.a2, l.a3, l.a4); printf ("A5 = %3.3f\t A6 = %3.3f\t E1 = %3.3f\n",l.a5, l.a6, l.e1); } void okc_print_lbr_mnj (fri_float_t* l) { printf ("A1 = %f, A2 = %f, E1 = %f, A3 = %f\nA4 = %f, A5 = %f, A6 = %f\n", l[0],l[1],l[2],l[3],l[4],l[5],l[6]); } okc_handle_t* okc_malloc_init_okc_handle_t (void) { okc_handle_t* okch; int i; okch = malloc (sizeof (okc_handle_t)); if (NULL == okch) { perror ("okc_malloc_init_okc_handle_t: malloc"); return (NULL); } okch->flags = 0; okch->listener = 0; okch->mode = -1; for (i=0; i < OKC_MAX_ROBOTS; i++) { okch->robot_map[i] = OKC_SLOT_UNASSIGNED; okch->robot_names[i] = NULL; okch->axis_set_abs_cb[i] = NULL; okch->cartpos_set_abs_cb[i] = NULL; okch->cartpos_axis_set_abs_cb[i] = NULL; okch->cartpos_axis_impedance_set_abs_cb[i] = NULL; okch->private_to_axis_set_abs_cb[i] = NULL; okch->private_to_cartpos_set_abs_cb[i] = NULL; okch->private_to_cartpos_axis_set_abs_cb[i] = NULL; okch->private_to_cartpos_axis_impedance_set_abs_cb[i] = NULL; } for (i=0; i < OKC_MAX_ROBOTS; i++) { if (0 != pthread_rwlock_init (&okch->pos_axis_in_lock[i],NULL)) { perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init"); free (okch); return (NULL); } if (0 != pthread_rwlock_init (&okch->rist_lbra_out_lock[i],NULL)) { perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init"); free (okch); return (NULL); } if (0 != pthread_rwlock_init (&okch->callback_lock[i],NULL)) { perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init"); free (okch); return (NULL); } } if (0 != pthread_rwlock_init (okch->robot_map_names_lock, NULL)) { perror ("okc_malloc_init_okc_handle_t: pthread_rwlock_init"); free (okch); return (NULL); } okch->comm_thread = malloc (sizeof (pthread_t)); if (NULL == okch->comm_thread) { perror ("okc_malloc_init_okc_handle_t: malloc"); free (okch); return (NULL); } return okch; } okc_handle_t* okc_destroy_okc_handle_t (okc_handle_t* okch) { int i; for (i=0; i < OKC_MAX_ROBOTS; i++) { if (0 != pthread_rwlock_destroy (&okch->pos_axis_in_lock[i])) { perror ("okc_destroy_okc_handle_t: pthread_rwlock_destroy"); } if (0 != pthread_rwlock_destroy (&okch->rist_lbra_out_lock[i])) { perror ("okc_destroy_okc_handle_t: pthread_rwlock_destroy"); } } if (0 != pthread_rwlock_destroy (okch->robot_map_names_lock)) { perror ("okc_destroy_okc_handle_t: pthread_rwlock_destroy"); } free (okch->comm_thread); free (okch); return (NULL); } int okc_is_coords_zero (coords_t k) { if (fabs (k.x) > OKC_DBL_EPSILON) return 0; if (fabs (k.y) > OKC_DBL_EPSILON) return 0; if (fabs (k.z) > OKC_DBL_EPSILON) return 0; if (fabs (k.a) > OKC_DBL_EPSILON) return 0; if (fabs (k.b) > OKC_DBL_EPSILON) return 0; if (fabs (k.c) > OKC_DBL_EPSILON) return 0; return 1; } int okc_is_axis_zero (lbr_axis_t l) { if (fabsf (l.a1) > OKC_DBL_EPSILON) return 0; if (fabsf (l.e1) > OKC_DBL_EPSILON) return 0; if (fabsf (l.a2) > OKC_DBL_EPSILON) return 0; if (fabsf (l.a3) > OKC_DBL_EPSILON) return 0; if (fabsf (l.a4) > OKC_DBL_EPSILON) return 0; if (fabsf (l.a5) > OKC_DBL_EPSILON) return 0; if (fabsf (l.a6) > OKC_DBL_EPSILON) return 0; return 1; } void okc_subtract_coords (coords_t* difference, coords_t minuend, coords_t subtrahend) { difference->x = minuend.x - subtrahend.x; difference->y = minuend.y - subtrahend.y; difference->z = minuend.z - subtrahend.z; difference->a = minuend.a - subtrahend.a; difference->b = minuend.b - subtrahend.b; difference->c = minuend.c - subtrahend.c; } void okc_subtract_axis (lbr_axis_t* difference, lbr_axis_t minuend, lbr_axis_t subtrahend) { difference->a1 = minuend.a1 - subtrahend.a1; difference->a2 = minuend.a2 - subtrahend.a2; difference->e1 = minuend.e1 - subtrahend.e1; difference->a3 = minuend.a3 - subtrahend.a3; difference->a4 = minuend.a4 - subtrahend.a4; difference->a5 = minuend.a5 - subtrahend.a5; difference->a6 = minuend.a6 - subtrahend.a6; } static void _okc_print_head (tFriHeader h) { printf ("\tPackets Send: %u Packets Received: %u\n",h.sendSeqCount, h.reflSeqCount); } static const char* _okc_quality_to_str (fri_uint16_t 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); } static void _okc_print_state (tFriIntfState s) { printf ("\tAvg. Answer Rate: %.2f, Latency %.5f (± %.5f)\n", s.stat.answerRate, s.stat.latency, s.stat.jitter); printf ("\tMissRate: %.4f, Missed Packet Counter: %u\n", s.stat.missRate, s.stat.missCounter); printf ("\tRobot Timestamp: %f\n", s.timestamp); printf ("\tState = %s, Quality = %s\n",(s.state == FRI_STATE_MON)?"monitor": (s.state == FRI_STATE_CMD)?"command":"unknown mode", _okc_quality_to_str (s.quality)); printf ("\tDesired Sample Time: %f\n", s.desiredMsrSampleTime); printf ("\tDesired Command Sample Time: %f\n",s.desiredCmdSampleTime); printf ("\tSafty Limits: %f\n",s.safetyLimits); } static const char* _okc_control_to_str (fri_uint16_t c) { switch (c) { case FRI_CTRL_POSITION: return "position control"; case FRI_CTRL_CART_IMP: return "cartesian impedance control"; case FRI_CTRL_JNT_IMP: return "joint impedance control"; case FRI_CTRL_OTHER: return "'other'"; } return "unknown"; } static void _okc_print_robotstate (tFriRobotState r) { printf ("\tpower (undocumented bitfield) = 0x%X\n",r.power); printf ("\tcontrol strategy = %s\n",_okc_control_to_str (r.control)); printf ("\terror (undocumented) = %u\n",r.error); printf ("\twarning (undocumented) = %u\n",r.warning); printf ("\tjoint temperatures = (%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f)\n", r.temperature[0],r.temperature[1],r.temperature[2],r.temperature[3], r.temperature[4],r.temperature[5],r.temperature[6]); } static void _okc_print_float_array (fri_float_t* f, int dim) { assert (dim > 0); int j; printf ("(%2.2f", f[0]); for (j = 1;j < dim;j++) { printf (", %2.2f", f[j]); } printf(")\n"); } static void _okc_print_robot_data (tFriRobotData r) { int i,j; printf ("Measured Joint Position (rad) = "); _okc_print_float_array (r.msrJntPos,LBR_MNJ); printf ("Measured Cart Position (m) = "); _okc_print_float_array (r.msrCartPos, FRI_CART_FRM_DIM); printf ("Commanded Joint Position (rad) = "); _okc_print_float_array (r.cmdJntPos,LBR_MNJ); printf ("Commanded Cart Position (m) = "); _okc_print_float_array (r.cmdCartPos, FRI_CART_FRM_DIM); printf ("Commanded Joint Offset (rad) = "); _okc_print_float_array (r.cmdJntPosFriOffset,LBR_MNJ); printf ("Measured Joint Torques (Nm) = "); _okc_print_float_array (r.msrJntTrq, LBR_MNJ); printf ("Estimated External Joint Torques (Nm) = "); _okc_print_float_array (r.estExtJntTrq, LBR_MNJ); printf ("Estimated Force/Torque TCP (N/NM) = "); _okc_print_float_array (r.estExtTcpFT,FRI_CART_FRM_DIM); printf ("\tJacobian:\n"); for (i = 0; i < FRI_CART_VEC; i++) { printf ("\t\t"); for (j = 0; j < LBR_MNJ; j++) { printf ("%2.2f ",r.jacobian[i*LBR_MNJ+j]); } printf ("\n"); } printf ("\tMass Matrix:\n"); for (i = 0; i < LBR_MNJ; i++) { printf ("\t\t"); for (j = 0; j < LBR_MNJ; j++) { printf ("%2.2f ",r.massMatrix[i*LBR_MNJ+j]); } printf ("\n"); } printf ("\tGravity = "); _okc_print_float_array (r.gravity, LBR_MNJ); } static void _okc_print_krl_data (tFriKrlData k) { int i; printf ("\tKRL DATA:\tfloat\tint\tbool\n"); for (i = 0;i < FRI_USER_SIZE; i++) { printf ("\t\t%2.3f\t%d\t%d\n", k.realData[i],k.intData[i], (k.boolData&(1<rist_lbra_out_lock[robot_id])) { perror ("okc_print_robot_informations: pthread_rwlock_rdlock"); exit (EXIT_FAILURE); } rstate = okc->state[robot_id]; rdata = okc->robot_data[robot_id]; krl = okc->krl_from_robot[robot_id]; con_state = okc->connection_stats[robot_id]; head = okc->message_header[robot_id]; if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_print_robot_informations: pthread_rwlock_unlock"); exit (EXIT_FAILURE); } printf ("Robot Informations for Robot %d:\n",robot_id); _okc_print_head (head); _okc_print_state (con_state); _okc_print_robotstate (rstate); _okc_print_robot_data (rdata); _okc_print_krl_data (krl); } static int _okc_check_range (fri_float_t d, fri_float_t min, fri_float_t max) { if ((d < min) || (d > max)) return 1; return 0; } int okc_check_axis_stiffness_value_range (lbr_axis_t stiff) { if (1 == _okc_check_range (stiff.a1,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.a2,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.e1,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.a3,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.a4,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.a5,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.a6,OKC_AXIS_STIFFNESS_MIN,OKC_AXIS_STIFFNESS_MAX)) return (OKC_EINVAL); return (OKC_OK); } int okc_check_axis_damping_value_range (lbr_axis_t damp) { if (1 == _okc_check_range (damp.a1,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.a2,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.e1,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.a3,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.a4,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.a5,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.a6,OKC_AXIS_DAMPING_MIN,OKC_AXIS_DAMPING_MAX)) return (OKC_EINVAL); return (OKC_OK); } int okc_check_cp_stiffness_value_range (coords_t stiff) { if (1 == _okc_check_range (stiff.x,OKC_CART_XYZ_STIFFNESS_MIN, OKC_CART_XYZ_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.y,OKC_CART_XYZ_STIFFNESS_MIN, OKC_CART_XYZ_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.z,OKC_CART_XYZ_STIFFNESS_MIN, OKC_CART_XYZ_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.a,OKC_CART_ABC_STIFFNESS_MIN, OKC_CART_ABC_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.b,OKC_CART_ABC_STIFFNESS_MIN, OKC_CART_ABC_STIFFNESS_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (stiff.c,OKC_CART_ABC_STIFFNESS_MIN, OKC_CART_ABC_STIFFNESS_MAX)) return (OKC_EINVAL); return (OKC_OK); } int okc_check_cp_damping_value_range (coords_t damp) { if (1 == _okc_check_range (damp.x,OKC_CART_XYZ_DAMPING_MIN, OKC_CART_XYZ_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.y,OKC_CART_XYZ_DAMPING_MIN, OKC_CART_XYZ_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.z,OKC_CART_XYZ_DAMPING_MIN, OKC_CART_XYZ_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.a,OKC_CART_ABC_DAMPING_MIN, OKC_CART_ABC_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.b,OKC_CART_ABC_DAMPING_MIN, OKC_CART_ABC_DAMPING_MAX)) return (OKC_EINVAL); if (1 == _okc_check_range (damp.c,OKC_CART_ABC_DAMPING_MIN, OKC_CART_ABC_DAMPING_MAX)) return (OKC_EINVAL); return (OKC_OK); } int okc_sleep_cycletime (okc_handle_t* okc, int robot_id) { struct timespec time_val; struct timespec time_left; time_val.tv_sec = 0; if (0 != pthread_rwlock_rdlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_sleep_cycletime: pthread_rwlock_rdlock"); return (OKC_ERROR); } time_val.tv_nsec = round (okc->connection_stats[0].desiredMsrSampleTime* 1000000000.0); if (0 != pthread_rwlock_unlock (&okc->rist_lbra_out_lock[robot_id])) { perror ("okc_sleep_cycletime: pthread_rwlock_unlock"); return (OKC_ERROR); } while (0 != nanosleep (&time_val,&time_left)) { time_val = time_left; } return (OKC_OK); } void okc_print_cart_frm_dim (fri_float_t* vec) { printf ("%f\t%f\t%f\t\t%f\n",vec[0],vec[1],vec[2],vec[3]); printf ("%f\t%f\t%f\t\t%f\n",vec[4],vec[5],vec[6],vec[7]); printf ("%f\t%f\t%f\t\t%f\n",vec[8],vec[9],vec[10],vec[11]); } void okc_print_transform (transform_t t) { printf ("%f\t%f\t%f\t\t%f\n",t.rxx, t.rxy, t.rxz, t.tx); printf ("%f\t%f\t%f\t\t%f\n",t.ryx, t.ryy, t.ryz, t.ty); printf ("%f\t%f\t%f\t\t%f\n",t.rzx, t.rzy, t.rzz, t.tz); } void okc_cp_cart_frm_dim (const fri_float_t* source, fri_float_t* target) { memcpy (target,source,sizeof (fri_float_t)*FRI_CART_FRM_DIM); } void okc_cp_lbr_mnj (const fri_float_t* source, fri_float_t* target) { memcpy (target,source,sizeof (fri_float_t)*LBR_MNJ); } void okc_cp_cart_vec (const fri_float_t* source, fri_float_t* target) { memcpy (target,source,sizeof (fri_float_t)*FRI_CART_VEC); } void okc_set_to_cp_stiff_damp_default (fri_float_t* cp_stiff, fri_float_t* cp_damp) { int i; for (i = 0; i < FRI_CART_VEC; i++) { if (i < 3) { cp_stiff[i] = OKC_CART_XYZ_STIFFNESS_DEFAULT; cp_damp[i] = OKC_CART_XYZ_DAMPING_DEFAULT; } else { cp_stiff[i] = OKC_CART_ABC_STIFFNESS_DEFAULT; cp_damp[i] = OKC_CART_ABC_DAMPING_DEFAULT; } } }