robowflex::hypercube Namespace Reference

Functions
bool	sceneToTesseractEnv (const robowflex::SceneConstPtr &scene, tesseract::tesseract_ros::KDLEnvPtr env)
	Add scene collision objects to a (previously initialized) KDL environment. More...
bool	addAttachedBodiesToTesseractEnv (const robot_state::RobotStatePtr &state, tesseract::tesseract_ros::KDLEnvPtr env)
	Add bodies attached to the robot scratch state to the KDL environment. More...
void	robotStateToManipState (const robot_state::RobotStatePtr &robot_state, const std::vector< std::string > &manip_joint_names, std::vector< double > &manip_joint_values)
	Transform a robot_state to a vector representing joint values for the manipulator (in the order given by manip_joint_names). More...
void	manipStateToRobotState (const Eigen::Ref< const Eigen::VectorXd > &manip_state, const std::string &manip, const tesseract::tesseract_ros::KDLEnvPtr &env, robot_state::RobotStatePtr robot_state)
	Transform a tesseract waypoint (manip state) to robot state. Joint values for non-manip joints are taken from env. More...
void	manipTesseractTrajToRobotTraj (const tesseract::TrajArray &tesseract_traj, const robot_state::RobotStatePtr &ref_state, const std::string &manip, const tesseract::tesseract_ros::KDLEnvPtr &env, robot_trajectory::RobotTrajectoryPtr trajectory)
	Transform a tesseract trajectory to a robot trajectory. More...
void	robotTrajToManipTesseractTraj (const robot_trajectory::RobotTrajectoryPtr &robot_traj, const std::string &manip, const tesseract::tesseract_ros::KDLEnvPtr &env, tesseract::TrajArray &trajectory)
	Transform a robot_trajectory to a tesseract manipulator trajectory. More...

Function Documentation

addAttachedBodiesToTesseractEnv()

bool robowflex::hypercube::addAttachedBodiesToTesseractEnv	(	const robot_state::RobotStatePtr &	state,
		tesseract::tesseract_ros::KDLEnvPtr	env
	)

Add bodies attached to the robot scratch state to the KDL environment.

Parameters

[in]	state	Robot state with objects attached.
[out]	env	KDL environment to add the attached objects.

Returns

True if the KDL environment was correctly updated.

Definition at line 122 of file conversions.cpp.

{
    moveit_msgs::RobotState state_msg;
    moveit::core::robotStateToRobotStateMsg(*state, state_msg);
 
    std_msgs::Int32 cotp;
    cotp.data = tesseract::CollisionObjectType::UseShapeType;
    std_msgs::Int32 cotm;
    cotm.data = tesseract::CollisionObjectType::ConvexHull;
 
    for (const auto &co : state_msg.attached_collision_objects)
    {
        // Declare an attachable object for this attached body.
        tesseract_msgs::AttachableObject obj;
        obj.name = co.object.id;
        obj.operation = tesseract_msgs::AttachableObject::ADD;
 
        // Add visual object.
        obj.visual.primitives = co.object.primitives;
        obj.visual.primitive_poses = co.object.primitive_poses;
        obj.visual.meshes = co.object.meshes;
        obj.visual.mesh_poses = co.object.mesh_poses;
        obj.visual.planes = co.object.planes;
        obj.visual.plane_poses = co.object.plane_poses;
 
        // Add collision object.
        obj.collision.primitives = co.object.primitives;
        obj.collision.primitive_poses = co.object.primitive_poses;
        obj.collision.primitive_collision_object_types.resize(co.object.primitives.size());
        std::fill(obj.collision.primitive_collision_object_types.begin(),
                  obj.collision.primitive_collision_object_types.end(), cotp);
        obj.collision.meshes = co.object.meshes;
        obj.collision.mesh_poses = co.object.mesh_poses;
        obj.collision.mesh_collision_object_types.resize(co.object.meshes.size());
        std::fill(obj.collision.mesh_collision_object_types.begin(),
                  obj.collision.mesh_collision_object_types.end(), cotm);
        obj.collision.planes = co.object.planes;
        obj.collision.plane_poses = co.object.plane_poses;
 
        // Create Tesseract attachable object.
        auto ao = std::make_shared<tesseract::AttachableObject>();
        tesseract::tesseract_ros::attachableObjectMsgToAttachableObject(ao, obj);
        env->addAttachableObject(ao);
 
        // Attach the object to the environment (to a parent frame).
        tesseract::AttachedBodyInfo attached_body_info;
        attached_body_info.object_name = co.object.id;
        attached_body_info.parent_link_name = co.link_name;
        attached_body_info.transform = Eigen::Isometry3d::Identity();
        env->attachBody(attached_body_info);
    }
 
    return true;
}

manipStateToRobotState()

void robowflex::hypercube::manipStateToRobotState	(	const Eigen::Ref< const Eigen::VectorXd > &	manip_state,
		const std::string &	manip,
		const tesseract::tesseract_ros::KDLEnvPtr &	env,
		robot_state::RobotStatePtr	robot_state
	)

Transform a tesseract waypoint (manip state) to robot state. Joint values for non-manip joints are taken from env.

Parameters

[in]	manip_state	Tesseract manipulator state to be transformed.
[in]	manip	Name of manipulator.
[in]	env	KDL environment with the robot (and manipulator) information already loaded.
[out]	robot_state	Robot state representing manip_state.

Definition at line 199 of file conversions.cpp.

{
    // Initialize it with the env state (includes both group and non-group joints).
    const auto &joint_values = env->getCurrentJointValues();
    std::vector<double> tmp_current_values(joint_values.data(), joint_values.data() + joint_values.size());
    robot_state->setVariablePositions(env->getJointNames(), tmp_current_values);
 
    // Set (only) group joints from tesseract waypoint.
    const auto &joint_manip_names = env->getManipulator(manip)->getJointNames();
    std::vector<double> tmp_group_values(manip_state.data(), manip_state.data() + manip_state.size());
    robot_state->setVariablePositions(joint_manip_names, tmp_group_values);
}

manipTesseractTrajToRobotTraj()

void robowflex::hypercube::manipTesseractTrajToRobotTraj	(	const tesseract::TrajArray &	tesseract_traj,
		const robot_state::RobotStatePtr &	ref_state,
		const std::string &	manip,
		const tesseract::tesseract_ros::KDLEnvPtr &	env,
		robot_trajectory::RobotTrajectoryPtr	trajectory
	)

Transform a tesseract trajectory to a robot trajectory.

Parameters

[in]	tesseract_traj	Tesseract trajectory to transform.
[in]	robot	Robot tesseract_traj belongs to.
[in]	manip	Name of manipulator.
[in]	env	KDL environment with the robot (and manipulator) information already loaded.
[out]	trajectory	Robot trajectory corresponding to tesseract_traj.

Definition at line 215 of file conversions.cpp.

{
    const robot_state::RobotState &copy = *ref_state;
    trajectory->clear();
 
    for (int i = 0; i < tesseract_traj.rows(); i++)
    {
        // Create a tmp state for every waypoint.
        auto tmp_state = std::make_shared<robot_state::RobotState>(copy);
 
        // Transform tesseract manip ith waypoint to robot state.
        manipStateToRobotState(tesseract_traj.row(i), manip, env, tmp_state);
 
        // Add waypoint to trajectory.
        trajectory->addSuffixWayPoint(tmp_state, 0.0);
    }
}

robotStateToManipState()

void robowflex::hypercube::robotStateToManipState	(	const robot_state::RobotStatePtr &	robot_state,
		const std::vector< std::string > &	manip_joint_names,
		std::vector< double > &	manip_joint_values
	)

Transform a robot_state to a vector representing joint values for the manipulator (in the order given by manip_joint_names).

Parameters

[in]	robot_state	Robot state to be transformed.
[in]	manip_joint_names	Joint names for the manipulator state.
[out]	manip_joint_values	Manipulator's joint values representing robot_state.

Definition at line 178 of file conversions.cpp.

{
    manip_joint_values.clear();
    manip_joint_values.resize(0);
 
    // Extract the state to a raw vector.
    double *raw_state_values = robot_state->getVariablePositions();
 
    // Loop over manip joints and add their values to goal_state in the correct order.
    const auto &robot_state_joint_names = robot_state->getVariableNames();
    for (const auto &joint_name : manip_joint_names)
    {
        int index = std::distance(
            robot_state_joint_names.begin(),
            std::find(robot_state_joint_names.begin(), robot_state_joint_names.end(), joint_name));
        manip_joint_values.push_back(raw_state_values[index]);
    }
}

robotTrajToManipTesseractTraj()

void robowflex::hypercube::robotTrajToManipTesseractTraj	(	const robot_trajectory::RobotTrajectoryPtr &	robot_traj,
		const std::string &	manip,
		const tesseract::tesseract_ros::KDLEnvPtr &	env,
		tesseract::TrajArray &	trajectory
	)

Transform a robot_trajectory to a tesseract manipulator trajectory.

Parameters

[in]	robot_traj	Robot Trajectory to transform.
[in]	manip	Name of manipulator in KDL env.
[in]	env	KDL environment with the robot (and manipulator) information already loaded.
[out]	trajectory	Tesseract trajectory array corresponding to robot_trajectory.

Definition at line 237 of file conversions.cpp.

{
    trajectory.resize(0, 0);
    trajectory.resize(robot_traj->getWayPointCount(), robot_traj->getGroup()->getVariableCount());
 
    for (unsigned int i = 0; i < robot_traj->getWayPointCount(); ++i)
    {
        // Transform each traj waypoint to tesseract manip state.
        std::vector<double> manip_joint_values;
        robotStateToManipState(robot_traj->getWayPointPtr(i), env->getManipulator(manip)->getJointNames(),
                               manip_joint_values);
 
        // Push manip state to manip tesseract trajectory .
        trajectory.row(i) = Eigen::VectorXd::Map(manip_joint_values.data(), manip_joint_values.size());
    }
}

sceneToTesseractEnv()

bool robowflex::hypercube::sceneToTesseractEnv	(	const robowflex::SceneConstPtr &	scene,
		tesseract::tesseract_ros::KDLEnvPtr	env
	)

Add scene collision objects to a (previously initialized) KDL environment.

Parameters

[in]	scene	Scene to load.
[out]	env	KDL environment to add scene objects to.

Returns

True if the KDL environment was correctly loaded from scene.

Definition at line 17 of file conversions.cpp.

{
    if (env->checkInitialized())
    {
        // Clear environment from scene objects.
        env->clearAttachableObjects();
        env->clearAttachedBodies();
 
        // Loop over collision objects in the scene.
        const auto &scene_msg = scene->getMessage();
        for (const auto &collision_object : scene_msg.world.collision_objects)
        {
            // Add collision and visual objects
            tesseract_msgs::AttachableObject obj;
            obj.name = collision_object.id;
            obj.operation = tesseract_msgs::AttachableObject::ADD;
 
            for (const shape_msgs::SolidPrimitive &primitive : collision_object.primitives)
            {
                obj.visual.primitives.emplace_back(primitive);
                obj.collision.primitives.emplace_back(primitive);
                std_msgs::Int32 cot;
                cot.data = tesseract::CollisionObjectType::UseShapeType;
                obj.collision.primitive_collision_object_types.emplace_back(cot);
            }
 
            for (const shape_msgs::Mesh &mesh : collision_object.meshes)
            {
                obj.visual.meshes.emplace_back(mesh);
                obj.collision.meshes.emplace_back(mesh);
                std_msgs::Int32 cot;
                cot.data = tesseract::CollisionObjectType::ConvexHull;
                obj.collision.mesh_collision_object_types.emplace_back(cot);
            }
 
            for (const shape_msgs::Plane &plane : collision_object.planes)
            {
                obj.visual.planes.emplace_back(plane);
                obj.collision.planes.emplace_back(plane);
            }
 
            for (const geometry_msgs::Pose &pose : collision_object.primitive_poses)
            {
                obj.visual.primitive_poses.emplace_back(pose);
                obj.collision.primitive_poses.emplace_back(pose);
            }
 
            for (const geometry_msgs::Pose &pose : collision_object.mesh_poses)
            {
                obj.visual.mesh_poses.emplace_back(pose);
                obj.collision.mesh_poses.emplace_back(pose);
            }
 
            for (const geometry_msgs::Pose &pose : collision_object.plane_poses)
            {
                obj.visual.plane_poses.emplace_back(pose);
                obj.collision.plane_poses.emplace_back(pose);
            }
 
            // Add collision object as attachable object.
            auto ao = std::make_shared<tesseract::AttachableObject>();
            tesseract::tesseract_ros::attachableObjectMsgToAttachableObject(ao, obj);
            env->addAttachableObject(ao);
 
            // Attach the object to the environment (to a parent frame).
            tesseract::AttachedBodyInfo attached_body_info;
            attached_body_info.object_name = collision_object.id;
            attached_body_info.parent_link_name = env->getRootLinkName();
            attached_body_info.transform = Eigen::Isometry3d::Identity();
            env->attachBody(attached_body_info);
        }
 
        // Add octomap information.
        if (scene_msg.world.octomap.octomap.data.size() > 0)
        {
            const std::string &name = "octomap";
            tesseract_msgs::AttachableObject obj;
            obj.name = name;
            obj.operation = tesseract_msgs::AttachableObject::ADD;
            obj.collision.octomaps.emplace_back(scene_msg.world.octomap.octomap);
            obj.collision.octomap_poses.emplace_back(scene_msg.world.octomap.origin);
            std_msgs::Int32 cot;
            cot.data = tesseract::CollisionObjectType::UseShapeType;
            obj.collision.octomap_collision_object_types.emplace_back(cot);
 
            auto ao = std::make_shared<tesseract::AttachableObject>();
            tesseract::tesseract_ros::attachableObjectMsgToAttachableObject(ao, obj);
            env->addAttachableObject(ao);
 
            tesseract::AttachedBodyInfo attached_body_info;
            attached_body_info.object_name = name;
            attached_body_info.parent_link_name = scene_msg.world.octomap.header.frame_id;
            attached_body_info.transform = Eigen::Isometry3d::Identity();
            env->attachBody(attached_body_info);
        }
 
        return true;
    }
 
    RBX_ERROR("Tesseract environment not initialized");
 
    return false;
}