baxter_multi_target.cpp File Reference

#include <robowflex_library/io/visualization.h>
#include <robowflex_library/log.h>
#include <robowflex_library/robot.h>
#include <robowflex_library/util.h>

Go to the source code of this file.

Functions
int	main (int argc, char **argv)

Variables
static const std::string	BOTH_ARMS = "both_arms"
static const std::string	LEFT_ARM = "left_arm"
static const std::string	LEFT_EE = "left_gripper_base"
static const std::string	RIGHT_EE = "right_gripper_base"

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 25 of file baxter_multi_target.cpp.

{
    // Startup ROS
    ROS ros(argc, argv);
 
    // Create a Baxter robot.
    auto baxter = std::make_shared<Robot>("baxter");
    baxter->initializeFromYAML("package://robowflex_resources/baxter.yml");
    baxter->loadKinematics(BOTH_ARMS);
 
    // Visualize the Baxter robot
    IO::RVIZHelper rviz(baxter);
    rviz.visualizeCurrentState();
    RBX_INFO("Default robot state is visualized. Press enter to continue...");
    std::cin.ignore();
 
    // EE offset that makes the arm point upwards.
    const auto &ee_offset = TF::createPoseXYZ(0, 0, -1.3, constants::pi, 0, 0);
 
    // Use IK to shift robot arm over by desired amount.
    const RobotPose &goal_pose_left = baxter->getLinkTF(LEFT_EE) * ee_offset;
    const RobotPose &goal_pose_right = baxter->getLinkTF(RIGHT_EE) * ee_offset;
 
    // Visualizing the target poses.
    rviz.addTransformMarker("pose_right", "map", goal_pose_right);
    rviz.addTransformMarker("pose_left", "map", goal_pose_left);
    rviz.updateMarkers();
    RBX_INFO("Target IK poses are visualized. Press enter to continue...");
    std::cin.ignore();
 
    Robot::IKQuery query(BOTH_ARMS, {goal_pose_left, goal_pose_right}, {LEFT_EE, RIGHT_EE});
 
    if (not baxter->setFromIK(query))
    {
        RBX_ERROR("IK query failed!");
        return 1;
    }
 
    // Visualize resulting state.
    rviz.visualizeCurrentState();
 
    RBX_INFO("Robot IK solution visualized,  Press enter to continue...");
    std::cin.ignore();
 
    query = Robot::IKQuery(LEFT_ARM, goal_pose_left);
    if (not baxter->setFromIK(query))
    {
        RBX_ERROR("Single IK query failed!");
        return 1;
    }
 
    // Visualize resulting state.
    rviz.visualizeCurrentState();
 
    RBX_INFO("Robot left arm IK(ee:default) is visualized. Press enter to continue ...");
    std::cin.ignore();
 
    query = Robot::IKQuery(LEFT_ARM, {goal_pose_left}, {LEFT_EE});
    query.options.return_approximate_solution = true;  // Enable approximate IK solutions.
    query.validate = true;        // Use to verify approximate solutions are within tolerance.
    query.valid_distance = 0.05;  // Tuned distance threshold that is appropriate for query.
 
    if (not baxter->setFromIK(query))
    {
        RBX_ERROR("Single Approximate IK query failed!");
        return 1;
    }
 
    // Visualize resulting state.
    rviz.visualizeCurrentState();
    RBX_INFO("Robot left arm IK(ee:left_gripper_base) is visualized. Press enter to exit...");
    std::cin.ignore();
 
    return 0;
}

Variable Documentation

BOTH_ARMS

const std::string BOTH_ARMS = "both_arms"

static

Definition at line 20 of file baxter_multi_target.cpp.

LEFT_ARM

const std::string LEFT_ARM = "left_arm"

static

Definition at line 21 of file baxter_multi_target.cpp.

LEFT_EE

const std::string LEFT_EE = "left_gripper_base"

static

Definition at line 22 of file baxter_multi_target.cpp.

RIGHT_EE

const std::string RIGHT_EE = "right_gripper_base"

static

Definition at line 23 of file baxter_multi_target.cpp.