六维力传感器接口

创建六维力传感器数据

int CreateTorqueSensor(const char* torqueSensorName)

/**
 * @brief 创建六维力传感器数据
 *
 * @param torqueSensorName 创建时使用的名字
 * @return int 0:成功，其他：失败
 */

已升级为CreateTorqueSensor2

创建六维力传感器数据

int CreateTorqueSensor2(const char* torqueSensorName)

/**
 * @brief 创建六维力传感器数据
 *
 * @param torqueSensorName 创建时使用的名字
 * @param fileName 配置文件名字
 * @return int 0:成功，其他：失败
 */
int MainModule()
{
    int ret=0;
    //创建传感器，系统会启动默认创建索引为0的传感器，如果使用系统创建的传感器，无需使用该接口。
    ret=CreateTorqueSensor2("mysensor", "force_sensor.ini");
    Rdebug("CreateTorqueSensor2,ret=%d\n",ret);
    //获取系统内置传感器的名字
    const char* system_sensor_name=GetTorqueSensorName(0,NULL);

    //获取自己创建的传感器名字
    const char* my_sensor_name=GetTorqueSensorName(1,NULL);//="mysensor"
    if (NULL==my_sensor_name)
    {
        Rdebug("can't find sensor\n");
    }

    int robot_index=0;
    //获取索引robot_index的机器人名称
    const char* robot_name=get_name_robot_device(get_deviceName(0,NULL), robot_index);
    //获取使用的工具
    tool to;
    gettool("tool0",&to);
    //获取使用的工件
    wobj wo;
    getwobj("wobj0",&wo);
    //设置传感器转换数据
    ret=SetSensorTorqueTransformData(system_sensor_name, robot_name, &to, &wo);
    Rdebug("setSensorTorqueTransformData,ret=%d\n",ret);

    //标定传感器,一次执行永久生效，触发式执行
    //ret=TorqueSensorCalibration(system_sensor_name);
    Rdebug("TorqueSensorCalibration,ret=%d\n",ret);

    //创建定时器
    RTimer mytimer;
    ret=initUserTimer(&mytimer, 0, 1);
    Rdebug("initUserTimer,ret=%d\n",ret);

    while(robot_ok())
    {
        //定时
        userTimer(&mytimer);

        //获取传感器的原始数据
        double tor[6];
        ret=GetSensorTorque(system_sensor_name, tor);
        if (0!=ret)
        {
            Rdebug("TorqueSensorCalibration,ret=%d\n",ret);
        }


        //记录运行采集传感器数据。快速返回，不阻塞时钟
        RSaveDataFast1("original_data_record",1, 100, 6, tor );

        double angle[10];
        //获取机器人当前关节角度
        ret=GetGroupPosition(robot_name, angle);
        if (0!=ret)
        {
            Rdebug("GetGroupPosition,ret=%d\n",ret);
        }

        //对传感器数据进行转换(转换到工具坐标系下)
        ret=SensorTorqueTransformToTool(system_sensor_name, angle, tor);
        if (0!=ret)
        {
            Rdebug("SensorTorqueTransformToTool,ret=%d\n",ret);
        }
        //记录运行采集传感器转换数据。快速返回，不阻塞时钟
        RSaveDataFast1("transform_data_record",1, 100, 6, tor );

    }

    return 0;
}

释放六维力传感器数据

int DestroyTorqueSensor(const char* torqueSensorName)

/**
 * @brief 释放六维力传感器数据
 *
 * @param torqueSensorName 创建时使用的名字
 * @return int 0:成功，其他：失败
 */

示例代码

获取六维力传感器数目

int GetTorqueSensorNum()

/**
 * @brief 获取六维力传感器数目
 *
 * @return int 传感器数目
 */

示例代码

获取六维力传感器名字

const char GetTorqueSensorName(int index, char torqueSensorName)

/**
 * @brief 获取六维力传感器名字
 *
 * @param index 传感器索引
 * @param torqueSensorName 保存传感器名字数据空间，可以为空(NULL)
 * @return char 传感器名字数据区首地址
 */

示例代码

获取六维力传感器力数据

int GetSensorTorque(const char torqueSensorName, double tor)

/**
 * @brief 获取六维力传感器力数据
 *
 * @param torqueSensorName 创建时使用的名字
 * @param tor 返回传感器数据(单位：N, Nm)
 * @return int 0:成功，其他：失败
 */

示例代码

获取六维力传感器标定

int TorqueSensorCalibration(const char* torqueSensorName)

/**
 * @brief 获取六维力传感器标定
 *
 * @param torqueSensorName 创建时使用的名字
 * @return int 0:成功，其他：失败
 */

示例代码

设置传感器转换数据(SensorTorqueTransform)

int SetSensorTorqueTransformData(const char torqueSensorName, const char serialLinkName, tool rtool, wobj rwobj)

/**
 * @brief 设置传感器转换数据(SensorTorqueTransform)
 *
 * @param torqueSensorName 创建时使用的名字
 * @param serialLinkName 机器人名字
 * @param tool 工具
 * @param wobj 工件
 * @return int 0:成功，其他：失败
 */

示例代码

获取工具或（工件）坐标系下作用力

int SensorTorqueTransformToTool(const char torqueSensorName, double angle, double* tor)

/**
 * @brief 设置传感器转换数据(SensorTorqueTransform)
 *
 * @param torqueSensorName 创建时使用的名字
 * @param serialLinkName 机器人名字
 * @param tool 工具
 * @param wobj 工件
 * @return int 0:成功，其他：失败
 */

示例代码

获取法兰坐标系下作用力(作用力产生于工具上)

int SensorTorqueTransformToFlange(const char torqueSensorName, double angle, double* tor)

/**
 * @brief 获取法兰坐标系下作用力(作用力产生于工具上)
 *
 * @param torqueSensorName 传感器名称
 * @param angle 机器人当前关节位置（rad）
 * @param tor 输入传感器力矩数据，返回法兰坐标系下作用力(单位：N, Nm)
 *
 * @return int 0:成功，其他：失败
 */

获取仿真环境中六维力传感器力数据

int GetSensorTorque_sim(const char torqueSensorName, double angle, double env_pose[4][2], double env_stiffness[4], double* tor)

/**
 * @brief 获取仿真环境中六维力传感器力数据
 *
 * @param torqueSensorName 传感器名称
 * @param angle 机器人当前关节位置（rad）
 * @param env_pose 三维位置和旋转空间环境约束的上下限制
 * @param env_stiffness 环境刚度系数,分别为x,y,z和旋转的刚度系数
 * @param tor 返回传感器检测力(单位：N, Nm)s
 *
 * @return int 0:成功，其他：失败
 */