You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
285 lines
8.6 KiB
285 lines
8.6 KiB
/*{{[PH]
|
|
****************************************************************************
|
|
Project: FRI
|
|
|
|
This material is the exclusive property of KUKA Roboter GmbH
|
|
and must be returned to KUKA Roboter GmbH immediately upon
|
|
request. This material and the information illustrated or
|
|
contained herein may not be used, reproduced, stored in a
|
|
retrieval system, or transmitted in whole or in part in any
|
|
way - electronic, mechanical, photocopying, recording, or
|
|
otherwise, without the prior written consent of KUKA Roboter GmbH.
|
|
|
|
All Rights Reserved
|
|
Copyright (C) 2009
|
|
KUKA Roboter GmbH
|
|
Augsburg, Germany
|
|
|
|
[PH]}}
|
|
*/
|
|
|
|
/*
|
|
{{[FH]
|
|
****************************************************************************
|
|
friRemote.cpp
|
|
|
|
NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change
|
|
|
|
|
|
[FH]}}
|
|
*/
|
|
/**
|
|
\addtogroup friRemoteLib
|
|
\brief Library for FRI (FastResearchInterface)
|
|
*/
|
|
/* @{ */
|
|
|
|
/** *************************************************************************
|
|
\author (Guenter Schreiber)
|
|
\file
|
|
\brief FRI Remote class encapsulating UDP package handshakes
|
|
* *
|
|
***************************************************************************/
|
|
#include "friremote.h"
|
|
|
|
#include <iostream>
|
|
#include <stdlib.h>
|
|
|
|
friRemote::friRemote(int port,char *hintToRemoteHost):remote(port,hintToRemoteHost)
|
|
{
|
|
//std::cout << __FUNCTION__ << " " <<port <<std::endl;
|
|
//std::cout << "FRI Version " << FRI_MAJOR_VERSION << "." << FRI_SUB_VERSION << "." <<FRI_DATAGRAM_ID_CMD << "." <<FRI_DATAGRAM_ID_MSR << std::endl;
|
|
memset((void*)(& msr),0x0,sizeof(msr));
|
|
memset((void*)(& cmd), 0x0,sizeof(cmd));
|
|
{
|
|
// do checks, whether the interface - and the host meets the requirements
|
|
FRI_PREPARE_CHECK_BYTE_ORDER;
|
|
if (!FRI_CHECK_BYTE_ORDER_OK)
|
|
{
|
|
std::cerr << __FUNCTION__<<"Byte order on your system is not appropriate - expect deep trouble" << std::endl;
|
|
}
|
|
if (!FRI_CHECK_SIZES_OK)
|
|
{
|
|
std::cout << __FUNCTION__<<"Sizes of datastructures not appropriate - expect even deeper trouble" << std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
friRemote::~friRemote()
|
|
{
|
|
std::cout << __FUNCTION__ << " bye for now " << std::endl;
|
|
}
|
|
|
|
int friRemote::doReceiveData()
|
|
{
|
|
return remote.Recv(&msr);
|
|
}
|
|
|
|
|
|
/** Data Exchanger -- normally update within access routine implicitely ... */
|
|
int friRemote::doSendData()
|
|
{
|
|
// received at least something
|
|
seqCount++;
|
|
cmd.head.sendSeqCount = seqCount;
|
|
cmd.head.reflSeqCount = msr.head.sendSeqCount;
|
|
cmd.head.datagramId = FRI_DATAGRAM_ID_CMD;
|
|
cmd.head.packetSize = sizeof(tFriCmdData);
|
|
int rc=remote.Send(&cmd);
|
|
return rc;
|
|
}
|
|
|
|
/** send commands based on last msr datagram and
|
|
receives a new one
|
|
*/
|
|
int friRemote::doDataExchange()
|
|
{
|
|
doSendData();
|
|
return doReceiveData();
|
|
}
|
|
|
|
/** automatically do data exchange, if not otherwise specified */
|
|
int friRemote::doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange)
|
|
{
|
|
// Helper, if not properly initialized or the like...
|
|
|
|
cmd.cmd.cmdFlags=FRI_CMD_JNTPOS;
|
|
// Note:: If the interface is not in Command mode,
|
|
// The commands have to be "mirrored" to get in sync
|
|
// Note:: If the interface is not in Command mode,
|
|
// The commands have to be "mirrored" to get in sync
|
|
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
|
|
{
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
{
|
|
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// compute new values here ...
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
{
|
|
cmd.cmd.jntPos[i]=newJntPosition[i];
|
|
}
|
|
}
|
|
|
|
if (flagDataExchange)
|
|
{
|
|
return doDataExchange();
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
/** automatically do data exchange, if not otherwise specified
|
|
if flagDataExchange is set to false, call doDataExchange()
|
|
or doReceiveData()/doSendData() on your own
|
|
IN: newJntPosition - joint positions
|
|
newJntStiff - joint stiffness (Spring factor)
|
|
newJntDamp - joint damping (Damping factor)
|
|
newJntAddTorque - additional torque
|
|
|
|
Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the
|
|
value is ignored -> the respective cmd.cmd.cmdFlags field is set properly
|
|
Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
|
|
*/
|
|
int friRemote::doJntImpedanceControl(const float newJntPosition[LBR_MNJ],
|
|
const float newJntStiff[LBR_MNJ],
|
|
const float newJntDamp[LBR_MNJ],
|
|
const float newJntAddTorque[LBR_MNJ],
|
|
bool flagDataExchange)
|
|
|
|
{
|
|
// Helper, if not properly initialized or the like...
|
|
cmd.cmd.cmdFlags=0;
|
|
if (newJntPosition)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
|
|
// Note:: If the interface is not in Command mode,
|
|
// The commands have to be "mirrored" to get in sync
|
|
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
|
|
{
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
{
|
|
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// compute new values here ...
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
cmd.cmd.jntPos[i]=newJntPosition[i];
|
|
}
|
|
}
|
|
if (newJntStiff)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF;
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
cmd.cmd.jntStiffness[i]=newJntStiff[i];
|
|
}
|
|
if (newJntDamp)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP;
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
cmd.cmd.jntDamping[i]=newJntDamp[i];
|
|
}
|
|
if (newJntAddTorque)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ;
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
cmd.cmd.addJntTrq[i]=newJntAddTorque[i];
|
|
}
|
|
|
|
if (flagDataExchange)
|
|
{
|
|
return doDataExchange();
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/** automatically do data exchange, if not otherwise specified
|
|
if flagDataExchange is set to false, call doDataExchange()
|
|
or doReceiveData()/doSendData() on your own
|
|
IN: newJntPosition - joint positions
|
|
newJntStiff - joint stiffness (Spring factor)
|
|
newJntDamp - joint damping (Damping factor)
|
|
newJntAddTorque - additional torque
|
|
|
|
Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the
|
|
value is ignored -> the respective cmd.cmd.cmdFlags field is set properly
|
|
Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
|
|
*/
|
|
int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM],
|
|
const float newCartStiff[FRI_CART_VEC],
|
|
const float newCartDamp[FRI_CART_VEC],
|
|
const float newAddTcpFT[FRI_CART_VEC],
|
|
const float newJntNullspace[LBR_MNJ],
|
|
bool flagDataExchange)
|
|
{
|
|
|
|
// Helper, if not properly initialized or the like...
|
|
cmd.cmd.cmdFlags=0;
|
|
if ( newCartPosition )
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_CARTPOS;
|
|
for ( int i = 0; i < FRI_CART_FRM_DIM; i++)
|
|
{
|
|
cmd.cmd.cartPos[i]=newCartPosition[i];
|
|
|
|
}
|
|
}
|
|
if ( newCartStiff)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_CARTSTIFF;
|
|
for ( int i = 0; i < FRI_CART_VEC; i++)
|
|
{
|
|
cmd.cmd.cartStiffness[i]=newCartStiff[i];
|
|
|
|
}
|
|
|
|
}
|
|
if ( newCartDamp)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP;
|
|
for ( int i = 0; i < FRI_CART_VEC; i++)
|
|
{
|
|
cmd.cmd.cartDamping[i]=newCartDamp[i];
|
|
|
|
}
|
|
}
|
|
if ( newAddTcpFT)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_TCPFT;
|
|
for ( int i = 0; i < FRI_CART_VEC; i++)
|
|
{
|
|
cmd.cmd.addTcpFT[i]=newAddTcpFT[i];
|
|
|
|
}
|
|
}
|
|
|
|
if (newJntNullspace)
|
|
{
|
|
cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
|
|
// Note:: If the interface is not in Command mode,
|
|
// The commands have to be "mirrored" to get in sync
|
|
|
|
// compute new values here ...
|
|
for (int i = 0; i < LBR_MNJ; i++)
|
|
cmd.cmd.jntPos[i]=newJntNullspace[i];
|
|
}
|
|
|
|
if (flagDataExchange)
|
|
{
|
|
return doDataExchange();
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* @} */
|
|
|