Upozornění: Text přílohy byl získán strojově a nemusí přesně odpovídat originálu. Zejména u strojově nečitelných smluv, kde jsme použili OCR. originál smlouvy stáhnete odsud
//Příloha č. 2 Smlouvy - Technická specifikace softwarového rozhraní v ANSI C++
//
// Source code written by Czech Technical University to specify the conditions
// on the software interroperability for tender procedure. The goods is
// “6D collaborative robot”. Date: October 2022.
// In Czech (“Dodavka kolaborativniho 6D robot”)
//
// The programming language is ANSI C/C++. The operating system can be
// either OS Linux or both OS Linux and OS MS Windows. The example API should
// be fully available and functional upon the product delivery including the source
// code for API in ANSI C/C++. The API implementation cannot use any commercial
// programming software requiring additional license fees of any kind
// to the debit of Czech Technical University or other chargers and obligations.
// The source code cannot be commented out to fulfill the functionality.
#include
#include
#include
#include
#include
#include
// Include other libraries needed by the selling party to fulfill the
// condition of the tender procedure
// Use standard namespace
using namespace std;
// -----------------------------------------------------------------
// COBOT API (Application Programming Interface in ANSI C++)
// -----------------------------------------------------------------
// BEGIN OF API SPECIFICATION
// Connects to the cobot controller on a given IP address
// from a computer with given IP address
// Returns 0 on success
// Returns -1 on failure
int
ConnectCobotCommunication(char ipaddressCobot[], char ipaddressPC[])
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
return 0; // success
}
// Disconnects the cobot controller
void
DisconnectCobotCommunication()
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
}
// Set the parameters to optimize the motion: payload in kg
// and moment of intertia of payload in kg.m2 and the distance
// between the head of robot to the center of gravity of load.
int
SetPayloadSpecs(float payloadKg,
float distanceCOG[3],
float kgm2[6])
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
if ((payloadKg >= 0.0) &&
(payloadKg < 15.5)) {
bool valid=true;
for (int i=0; i < 6; i++)
// conditions if parameters are set correctly
if ( (kgm2[i] < 0.0) || (kgm2[i] > 0.8) )
valid = false;
if (valid) {
// Pass to the controller the three values:
// a) payload weight in kg,
// b) payload center of gravity distance from robot head in meters as
// 3D vector,
// c) payload moment of inertia in kg.m2 as a vector with six elements:
// [Ixx, Iyy, Izz, Ixy, Ixz, Iyz]
// .....
return 0; // OK
}
}
return -1; // error - out of range
}
// Start movement of the cobotic arm the a new pose that was set by
// the command 'SetPositionForTheNextMotion'
// eps ... specify in meters the convergence condition
// It is non-blocking operation, so it returns immediately
// even if the motion is in process
// Returns 0 if the operation was successfully started
// Returns -1 if the operation cannot be executed for some reason;
// e.g. the setup was not made yet.
int
ExecuteCobotMotion(float eps)
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// non-blocking operation execution
// Execute the motion of the cobot to a new position of joints J06
// with the maximum error at joints eps. The values are given in
// degrees
// returns 0 ... success,
// returns -1 ... failure for some reason
// position required
return 0;
}
// Stops running motion immediately, not resulting in an error state
// It is non-blocking operation execution
// Returns 0 on success
// Returns a negative value on error, the motion cannot be executed
int
StopCobotMotion()
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
return 0; // returns 0 ... success
}
// Returns current cobot position at six joints at this moment of time
// Returns 0 ... cobot is not moving now
// Returns 1 ... cobot is moving now
// Returns -1 or other negative value if the cobot motion was not
// successfully completed,
// for example, there was a blocking obstacle on the motion path
int
GetCobotPosition(float J06[])
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// Get J06[] just now, with the uncertainity of latency by UDP
// communication if the cobot is moving or not
// ....
if (1) // to be changed by correct condition
return 0; // return 0 if cobot is not moving at this moment
return 1; // return 1 if the cobot is moving now
}
// If a cobot arm/controller went to a failure state, it removes
// the error state and reinitate the status
int
ReinitiateCobotController()
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// Returns -1 or negative code if the operation failed
return 0; // return 0 if the operation was ended with success
}
// Returns 0, if a cobot arm/controller has no error and is communicating
// Returns a negative value, when the cobotic arm or the controller
// went to a failure
int
IsCobotInErrorState()
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// returns a value corresponding to some error state indicating
// which problem has occurred.
// For example, it can return -1, if the cobot motion was stopped
// during motion by an emergency stop
// It can return -2 if the cobot motion path was blocked by an
// obstacle.
return 0; // not in error state
}
// It is non-blocking operation execution, returns the status.
// Returns 0 ... the cobot is not moving at this moment
// Returns 1 ... the cobot is moving at this moment
// Returns -1 or other negative value if the last cobot motion
// operation was not successfully completed, for example, there was a
// blocking obstacle on the motion path
int
IsMoving()
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
if (1) // to be changed by correct condition
return 0; // return 0 if cobot is not moving at this moment
return 1; // return 1 if the cobot is moving at this moment
// returns negative value if the cobot got to the error state
if (0)
return -1; // error state
}
// Set speed of motion at TCP (tool center point at the mid of flange)
// in degrees per second
// Returns 0 ... on success, and negative value on failure, possibly
// indicating the problem
// It can be executed only when the cobot is not moving only.
int
SetSpeedJoints(float speed)
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
if (IsMoving()) return -1; // error
if (speed < 0) return -1; // error
// ....
return 0; // success
}
// Sets the speed of motion at TCP in m/s, acceleration and decelration in m/s^2.
// It has to be set before the motion and not during the motion.
// Returns 0 on success
// Returns -1 on failure, cobot is moving
// Returns -2 on failure, required speed is out of range
// Returns -3 on failure, required acceleration is out of range
// Returns -4 on failure, required deceleration is out of range
int
SetSpeedTCP(float speed, float accel, float decel)
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
if (IsMoving()) return -1; // error
// ....
return 0; // success
}
// This function converts the joints positions to TCP (tool center point at
// the midddle of the cobot head flange)
// Returns 0 on success
// Returns -1 on faiulre ... unable to convert, joint position out of range
// J06[] ... the six angles of cobot joints (input)
// TCP[] ... the tool center point - position + Euler angles (output)
// TCP[] is given that the position is 3 values, rotation is 3 values.
// Flag convertJointsToDeg specifies if joints angles in J06[] are given
// in degrees (true) or in radians (false)
int
ConvertJointsToTCP(float J06[6], float TCP[6], bool convertJointsToDeg = true)
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// ....
return 0; // success
}
// Converts the TCP (tool center point at midddle of the cobot head flange)
// to six joints position
// Returns 0 on success.
// Returns -1 on failure ... unable to convert, TCP in input is out of range
// TCP[] ... the tool center point - position + Euler angles (input)
// J06[] ... the angles of joints of cobot in range <-180, 180> degrees (output)
// convertJointsToDeg ... specifies if the angles in J06 should be outputed
// in degrees (true) or in radians (false)
int
ConvertTCPtoJoints(float TCP[6], float J06[6], bool convertJointsToDeg=true)
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// ....
return 0; // success
}
// Set the planned positions at joints for the next motion for the exact
// specification of time at that motion at this command
// If N=1 then only the next pose is specified.
// Intermediate positions can be set if N>1, it then requires J06[]
// array is of length N*6. The values in array timeC[] must be in range
// <0,1> and in ascending order and has length N values.
//
// Returns 0 ... on success
// Returns -1 ... on failure, the position J06[] was incorrectly specified
// Returns -2 ... on failure, the IP address was incorrectly specified
// Returns -3 ... wrong setting of timeC[] array, it is not an ascending order
// It can be successfully executed only if the last motion execution is finished.
int
SetPositionForTheNextMotion(
int N, // how many positions, at least 1
float timeC[], // event times at these positions
float J06[], // joints angles for the specified times: N*6 values in the array
float &expectedTime)
{
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// ..........
// Set the expected time for the whole planned motion for current setting
expectedTime = 0; // this line is not correct before implementation is completed
return 0; // success
}
// Returns the planned positions at joints for the next motion for the exact
// specification of time at that motion at this command.
// TimeC = 0.0 ... corresponds to the sitation before the cobot was motion was
// started
// TimeC = 1.0 ... corresponds to the sitation after the cobot was motion was
// finished
// J06[] ... joints setting for the specified times, 6 values in the array
// Returns 0 ... on success
// Returns -1 ... on failure, the time was incorrectly specified
// Returns -2 ... on failure, the IP address was incorrectly specified
// It can be sucessfully executed only if the last motion execution is finished.
int
GetPlannedPositionForNextMotion(float timeC, float J06[])
{
if ((timeC<0)||(timeC>1.0))
return -1; // the time event is out of range
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
return 0; // success
}
// Returns the exact positions at joints for the last executed motion for the
// specification of time at that motion
// TimeC = 0.0 ... corresponds to the pose before the cobot motion was started
// TimeC = 1.0 ... corresponds to the pose after the cobot motion was finished
// J06[] ... joints setting for the specified times, 6 values in the array
// Returns 0 ... on success
// Returns -1 ... on failure, the cobot is still moving now
// Returns -2 ... on failure, the cobot has not been moved yet since initialization
// It can be successfully executed only if the last motion execution is finished.
int
GetExactPositionForLastMotion(float timeC, float J06[])
{
if ((timeC<0)||(timeC>1.0))
return -1; // the time event is out of range
// TO BE IMPLEMENTED AND DELIVERED BY CONTRACTOR latest at time of goods delivery
// - accurate reading of the motion for cobotic arm
// the hardware accuracy limits that to
return 0; // success
}
// -----------------------------------------------------------------
// END OF API SPECIFICATION
// -----------------------------------------------------------------
// BEGIN OF EXAMPLE USAGE
// -----------------------------------------------------------------
// Auxiliary function
// Generate a random value in range <0,1>
double
R01() {
return ((double)rand())/(double)RAND_MAX;
//return drand48();
}
// Set the initial required position of the cobotic arm
void
SetInitialPose(int a, float JB[6])
{
a=a;
const float range=270; // minimum range is (-270,+270) degrees
// This routine can be change for the purpose of testing so
// physically the cobot does not hit the mounting desk etc.
// For example, it makes sense to restrict more JB[0] and JB[1]
//
// The principal is simple: generate a new random pose, each
// time of execution of this program different
// Set angles at joints
JB[0] = 0.0 + R01()*range;
JB[1] = 0.0 + R01()*range;
JB[2] = 0.0 + R01()*range;
JB[3] = 0.0 + R01()*range;
JB[4] = 0.0 + R01()*range;
JB[5] = 0.0 + R01()*range;
}
// Generate a new pose of the cobot arm
// Sets J[] by new randomly generated position from current JB[] position
void
GenerateNewRandomPose(const float JB[6], float range, float J[6])
{
// Set angles at joints
// This routine can be change for the purpose of testing so
// physically the cobot does not hit the mounting desk etc.
// For example, it makes sense to restrict more J[0] and J[1]
//
// The principal is simple: generate a new random pose, each
// time of execution of this program different
// minimum range by tender specification is (-270,+270) degrees
const float maxrange=270;
J[0] = JB[0] - range/2.0 + R01()*range;
if (J[0] < -maxrange) J[0] = -maxrange;
if (J[0] > maxrange) J[0] = maxrange;
J[1] = JB[1] - range/2.0 + R01()*range;
if (J[1] < -maxrange) J[1] = -maxrange;
if (J[1] > maxrange) J[1] = maxrange;
J[2] = JB[2] - range/2.0 + R01()*range;
if (J[2] < -maxrange) J[1] = -maxrange;
if (J[2] > maxrange) J[1] = maxrange;
J[3] = JB[3] - range/2.0 + R01()*range;
if (J[3] < -maxrange) J[1] = -maxrange;
if (J[3] > maxrange) J[1] = maxrange;
J[4] = JB[4] - range/2.0 + R01()*range;
if (J[4] < -maxrange) J[1] = -maxrange;
if (J[4] > maxrange) J[1] = maxrange;
J[5] = JB[5] - range/2.0 + R01()*range;
if (J[5] < -maxrange) J[1] = -maxrange;
if (J[5] > maxrange) J[1] = maxrange;
}
// -----------------------------------------------------------------
// Testing functionality of the cobot to be tested and
// delivered by CONTRACTOR
int
main(int argc, char* argv[])
{
// Example of the IP address, where the cobot controller is available
char cobotipaddress[]="192.168.88.100";
// Example of PC IP address, where the cobot controller is available
char myipaddress[]="192.168.88.142";
// Generate enough big array of values to store a single motion
float *positions = new float[10000000];
assert(positions);
float *positions2 = new float[10000000];
assert(positions2);
// Starts the communication with the cobot controller
ConnectCobotCommunication(cobotipaddress, myipaddress);
if (IsMoving()) {
cout << "Stop the cobot at the program start" << endl;
StopCobotMotion();
}
// The cobot controller can be in some error state upon startup,
// such as a failure from the last program execution when a cobotic
// arm hit an obstacle during motion
if (IsCobotInErrorState()) {
ReinitiateCobotController();
cout << "Warning: the cobot controller had to be reinitiated,"
<< " some error upon startup" << endl;
}
float Jstart[6], TCPstart[6];
// Get the cobot position at start of the application
GetCobotPosition(Jstart);
ConvertJointsToTCP(Jstart, TCPstart);
cout << "Start cobot arm position" << endl;
for (int i=0; i < 6; i++) {
cout << "TCP["<
float timeC = (double)j/(double)K2;
float JT[6], TCP[6];
// Get planned position at joints for the last motion at
// normalized time 'timeC'
GetPlannedPositionForNextMotion(timeC, JT);
positions2[IJ] = timeC; IJ++;
for (int k=0; k < 6; k++) {
// store the positions precomputed before motion
positions2[IJ] = JT[k]; IJ++;
}
// Analyze and exploit the pose data JT[] - user code by
// an application ... to be used for checking collision detection
if (1) {
// Example - convert the exact joint data to TCP and
// print them to the output
ConvertJointsToTCP(JT, TCP);
for (int k=0; k < 6; k++) {
cout << "j=" << j << "planned J[" << i << "]= "
<< JT[i] << " TCP[" << i << "]= " << TCP[i] << endl;
}
cout << "----------------------------------------" << endl;
}
} // for j
} // --------------- end of analysis for planned motion ------------------
// Get OS real time in miliseconds, real time, at the start
auto tstart = std::chrono::system_clock::now();
float eps = 2e-5; // specify in meters the convergence condition
// Now start the movement of a cobot arm to a new pose,
// non-blocking operation that allows reading of pose during motion
int ret = ExecuteCobotMotion(eps);
if (ret) {
cout << "ERROR - motion was not started err=" << ret << endl;
continue; // try with another position
}
// Check the position of cobot arm during the motion as frequently as
// possible and store it, there is a lag inaccuracy due to the communication
// via network
int K = 0;
int movingStatus = 0;
// Read pose as many times as possible during the cobotic arm motion and save
for(;;) {
// Read the position for this moment of time
auto tt = std::chrono::system_clock::now();
float JC[6];
std::chrono::duration elapsed_seconds = tt - tstart;
// returns immediate cobot position at joints at this time
int movingStatus = GetCobotPosition(JC);
// store the time and position received from controller
positions[K++] = elapsed_seconds.count();
// copy the jooints position to array
for (int j = 0; j < 6; j++)
positions[K++] = JC[j];
// Is the cobot at the end position of this pose?
if (movingStatus == 0)
break; // yes, we can finish the loop
if (movingStatus < 0) {
cout << "ERROR: an error occured during the motion from the last pose"
<< endl;
cout << "The error code is " << movingStatus << endl;
break;
}
assert(movingStatus > 0);
// Random stop of motion during the execution, with a low probability
float vrnd = R01();
const float thresholdStopMotion = 0.01; // probability 0 to 1.0
if (vrnd < thresholdStopMotion) {
// Stop the motion immediately, the emergency stop test during motion
StopCobotMotion();
break; // break this loop, cobot does not move any longer
}
} // -------------- end of online recording loop ----------------------
// Get time in miliseconds, real time
auto tstop = std::chrono::system_clock::now();
std::chrono::duration esTotal = tstop - tstart;
cout << "i=" << i << " ... duration of motion took " << esTotal.count()
<< " seconds" << endl;
if (movingStatus < 0) {
cout << "WARNING: Trying to remove the error state from the motion"
<< endl;
ReinitiateCobotController();
}
if (1) {
// Now analyze or/and save the exact data saved during the last
// motion and use them
IJ = 0; // reinitiate the index to access stored data
for(int j=0; j <= K2 ; j++) {
// normalized time value in range <0.0, 1.0>
float timeC = (double)j/(double)K2;
float JT[6], TCP[6];
// Get exact position at joints for the last executed motion
// at normalized time 'timeC' so really measured position
GetExactPositionForLastMotion(timeC, JT);
// Analyze and exploit the pose data JT[] - user code
// by an application .. to be used by the customer
if (1) {
// Example - convert the exact joint data to TCP
// and print them to the output
ConvertJointsToTCP(JT, TCP);
for (int k=0; k < 6; k++) {
cout << "j=" << j << "recorded J[" << k << "]= "
<< JT[k] << " TCP[" << k << "]= " << TCP[k] << endl;
}
cout << "----------------------------------------------" << endl;
}
if (1) {
// Compare the planned position saved before motion and the
// measured position that was recorded during motion.
cout << "time= " << positions2[IJ] << " time2= " << timeC << endl;
IJ++;
for (int k=0; k < 6; k++, IJ++) {
cout << "j=" << j << "recorded J[" << k << "]= "
<< JT[k] << " planned J[ " << positions2[IJ]
<< " error= " << JT[k] - positions2[IJ] << endl;
}
}
} // for j
} // ---------------------- end of analysis for executed motion ------
} // for i ------------ end of main testing loop ----------------
// Get the cobot position after it has stopped motion
float Jstop[6], TCPstop[6];
GetCobotPosition(Jstop);
ConvertJointsToTCP(Jstop, TCPstop);
for (int i=0; i < 6; i++) {
cout << "TCP["<