nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add approximate pose fallback

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-02-15 09:33:12 +02:00
parent 0027860830
commit e673d3d244
1 changed files with 48 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -172,10 +172,10 @@ public:
// Set limits for A4 paper // Set limits for A4 paper
float xlim_lower = 0.20; float xlim_lower = 0.20;
float xlim_upper = 0.40; float xlim_upper = 0.40;
float ylim_lower = -0.20; float ylim_lower = -0.13;
float ylim_upper = 0.20; float ylim_upper = 0.15;
float zlim_lower = 0.145; float zlim_lower = 0.150;
float zlim_upper = 0.16; float zlim_upper = 0.17;
/** /**
* Function that translates an input value with a given range to a value within another range. * Function that translates an input value with a given range to a value within another range.
@@ -217,8 +217,8 @@ public:
bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory) bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory)
{ {
pose = translatePose(pose); pose = translatePose(pose);
//move_group.setPoseTarget(pose); move_group.setPoseTarget(pose);
move_group.setApproximateJointValueTarget(pose, "link_eef"); //move_group.setApproximateJointValueTarget(pose, "link_eef");
//moveit_msgs::msg::RobotTrajectory trajectory; //moveit_msgs::msg::RobotTrajectory trajectory;
//move_group.setPlanningPipelineId("PTP"); //move_group.setPlanningPipelineId("PTP");
@@ -228,6 +228,48 @@ public:
previous_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), *trajectory); previous_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), *trajectory);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED");
if (success)
{
robot_trajectory::RobotTrajectory next_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
// append trajectory, with time step of 2.0, not skipping any points
previous_trajectory.append(next_trajectory, 0.01, 0);
*trajectory = moveit_msgs::msg::RobotTrajectory();
previous_trajectory.getRobotTrajectoryMsg(*trajectory);
// Append segment to complete trajectory
//trajectory->joint_trajectory.points.insert(trajectory->joint_trajectory.points.end(),
// plan.trajectory_.joint_trajectory.points.begin(),
// plan.trajectory_.joint_trajectory.points.end());
//trajectory->joint_trajectory.joint_names = plan.trajectory_.joint_trajectory.joint_names;
}
else
{
success = addPoseToTrajectoryApproximate(pose, trajectory);
}
move_group.clearPoseTarget();
return success;
}
/**
* Creates a trajectory for a pose and appends it to a given trajectory
*/
bool addPoseToTrajectoryApproximate(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory)
{
pose = translatePose(pose);
move_group.setApproximateJointValueTarget(pose, "link_eef");
move_group.setPlannerId("PTP");
robot_trajectory::RobotTrajectory previous_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
previous_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), *trajectory);
moveit::planning_interface::MoveGroupInterface::Plan plan; moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED"); RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED");