Clopema measurements (package)

Overview

In the Clopema project various type of measurements have to be done. Most of the measurements have the same scenario: follow predefined path - measure data - follow next path - etc. .
This package contains useful scripts, defined data types and set of examples for measuring data using predefined trajectories.
Using this tools will lead to unifed and defined data types for all experiments in the clopema project.

Measurement is divided into two parts: generating trajectories and measuring on the generated trajectories.
Dividing to the trajectory generation and measurement allow us to measure different data types repeatedly always at the same position with the same traveled path.
All already implemented data types are described in Measurement data types as well as tutorial how to add new measurement type.

Tutorial

Source codes can be found in clopema_tutorials_measurements package.
In tutorial replace clopema_tutorials_measurements with your package name.

Create package

Your package have to depend on clopema_measurements package.

catkin_create_pkg clopema_tutorials_measurements clopema_measurements

Trajectory generation

Lets say we want to measure some data at few points in cartesian space. For now, it is not important which data we want to measure we will chose them in next part of the tutorial.
Source code of node which will generate trajectories throught three points:

#include <ros/ros.h>
#include <clopema_measurements/MeasureData.h>
#include <clopema_robot/robot_commander.h>
#include <geometry_msgs/Pose.h>

using std::vector;
using geometry_msgs::Pose;
using clopema_robot::ClopemaRobotCommander;

vector<Pose> create_poses();

int main(int argc, char **argv) {
    ros::init(argc, argv, "gen_traj_example");
    ros::NodeHandle node("~");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    std::string trajectories_file;
    if (!node.getParam("trajectories_file", trajectories_file)) {
        ROS_ERROR_STREAM("Parameter trajectories_file must be set.");
        return 1;
    }
    vector<Pose> poses = create_poses();

    { //Move to the home position
        ClopemaRobotCommander g("arms");
        g.setNamedTarget("home_arms");
        g.setJointValueTarget("r1_joint_s", -M_PI_2);
        if (!g.move()) {
            ROS_ERROR_STREAM("Cannot move home");
            return 0;
        }
    }

    //Generate trajectories and save them to the "md" object
    ClopemaRobotCommander g("r2_arm");
    g.setStartStateToCurrentState();
    MeasureData md("");
    robot_state::RobotState current_state = *g.getCurrentState();

    for (unsigned int i = 0; i < poses.size(); ++i) {
        if (!md.msg.trajectories.empty()) {
            unsigned int jj = md.msg.trajectories.size() - 1;
            md.set_state_to_last_point(current_state, md.msg.trajectories[jj]);
        } else { //set current state as start state
            sensor_msgs::JointState jmsg;
            jmsg.name = current_state.getVariableNames();
            for (unsigned int i = 0; i < jmsg.name.size(); ++i) {
                double v = current_state.getVariablePosition(jmsg.name[i]);
                jmsg.position.push_back(v);
            }
            md.msg.start_state = jmsg;
        }

        ROS_INFO_STREAM(i << "/" << poses.size());

        ClopemaRobotCommander::Plan plan;
        if (!g.setJointValueTarget(poses[i], "r2_ee")) {
            ROS_ERROR_STREAM("Cannot set joint value target from pose");
            continue;
        }
        g.setStartState(current_state);
        if (!g.plan(plan)) {
            ROS_INFO_STREAM("Cannot plan.");
            continue;
        }

        md.msg.trajectories.push_back(plan.trajectory_.joint_trajectory); //trajectory
        md.msg.measure_after.push_back(true); //whether measure data after trajectory
        md.msg.wait_after.push_back(ros::Duration(0.1)); //Wait before measurement
    }

    md.save_trajectories(trajectories_file);
    g.setServoPowerOff(false);
    return 0;
}

vector<Pose> create_poses() {
    vector<Pose> poses;
    Pose p;
    p.orientation.y = 1;
    p.position.z = 1.2;
    for (double xx = 0.25; xx >= -0.75; xx -= 0.25) {
        for (double yy = -0.75; yy >= -1.25; yy -= 0.5) {
            p.position.x = xx;
            p.position.y = yy;
            poses.push_back(p);
        }
    }

    return poses;
}

Measure on generated trajectories

To measure on the trajectories we generate in the previous step we just need to create launch file with list of sensors we want to measure.
Following launch file will load trajectories and save Time, Joints, three Links, and Forces from both sensor to the folder $(arg folder).

        <!-- measure group -->
        <param name="clopema_measure_data/save_time" value="true" />
        <param name="clopema_measure_data/save_joints" value="true" />
        <param name="clopema_measure_data/save_links" value="true" />
        <rosparam param="clopema_measure_data/links/link_names">[r1_tip_link, r2_tip_link, r2_ee]</rosparam>

        <param name="clopema_measure_data/save_forces" value="true" />
        <rosparam param="clopema_measure_data/forces/topics">[/r1_force_data, /r2_force_data]</rosparam>
        <rosparam param="clopema_measure_data/forces/save_all">[true, false]</rosparam>

        <include file="$(find clopema_measurements)/launch/clopema_measure_data.launch">
            <arg name="folder" value="$(arg folder)"/>
            <arg name="trajectories_file" value="$(arg file)"/>
            <arg name="from" value="$(arg from)"/>
            <arg name="to" value="$(arg to)"/>
        </include>

Puting it together to launch file

It is nice to have all scripts in one launch file, so we will create launch file with optional argument 'gen'. Based on this argument we will decide if we want to measure (default) or generate trajectories. There are two additional parameters 'from' and 'to' which are optional and it allows us to specify range of trajectories which will be measured. If 'from' is set to '-1' it will generate folder structure for data thus it it is important to run script with 'from' equal to '-1' at the beginning.

<?xml version="1.0"?>
<launch>
    <arg name="gen" default="false"/>
    <arg name="from" default="-1"/>
    <arg name="to" default="-1"/>

    <arg name="folder" value="$(env HOME)/.ros/tmp_measure_data"/>
    <arg name="file" value="$(env HOME)/.ros/tmp_measure_trajectories.bag"/>

    <group unless="$(arg gen)">
        <!-- measure group -->
        <param name="clopema_measure_data/save_time" value="true" />
        <param name="clopema_measure_data/save_joints" value="true" />
        <param name="clopema_measure_data/save_links" value="true" />
        <rosparam param="clopema_measure_data/links/link_names">[r1_tip_link, r2_tip_link, r2_ee]</rosparam>

        <param name="clopema_measure_data/save_forces" value="true" />
        <rosparam param="clopema_measure_data/forces/topics">[/r1_force_data, /r2_force_data]</rosparam>
        <rosparam param="clopema_measure_data/forces/save_all">[true, false]</rosparam>

        <include file="$(find clopema_measurements)/launch/clopema_measure_data.launch">
            <arg name="folder" value="$(arg folder)"/>
            <arg name="trajectories_file" value="$(arg file)"/>
            <arg name="from" value="$(arg from)"/>
            <arg name="to" value="$(arg to)"/>
        </include>
    </group>

    <group if="$(arg gen)">
        <!-- generate trajectories -->
        <node pkg="clopema_tutorials_measurements" type="clopema_measure_example_gen" name="gen_traj" 
            output="screen" clear_params="true">
            <param name="trajectories_file" value="$(arg file)"/>
            <rosparam command="load" file="$(find clopema_moveit_config)/config/kinematics.yaml"/>
        </node>
    </group>

</launch>