TEST_F(TestConstraintAwareKinematics, TestCollisions)
{
kinematics_msgs::GetConstraintAwarePositionIK::Request ik_request;
kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response;
ik_request.ik_request.ik_seed_state.joint_state.name = joint_names_;
ik_request.ik_request.ik_seed_state.joint_state.position.resize(joint_names_.size());
ik_request.ik_request.ik_link_name = ik_link_name_;
ik_request.timeout = ros::Duration(2.0);
ik_request.ik_request.ik_link_name = "r_wrist_roll_link";
ik_request.ik_request.pose_stamped.header.frame_id = "base_link";
ik_request.ik_request.pose_stamped.pose.position.x = 0.52;
ik_request.ik_request.pose_stamped.pose.position.y = -.2;
ik_request.ik_request.pose_stamped.pose.position.z = .8;
ik_request.ik_request.pose_stamped.pose.orientation.x = 0.0;
ik_request.ik_request.pose_stamped.pose.orientation.y = 0.7071;
ik_request.ik_request.pose_stamped.pose.orientation.z = 0.0;
ik_request.ik_request.pose_stamped.pose.orientation.w = 0.7071;
planning_models::KinematicState* kin_state = collision_models_->setPlanningScene(planning_scene_);
for(unsigned int i = 0; i < 25; i++) {
std::map<std::string, double> joint_value_map;
//first finding a valid starting robot state
while(true) {
std::vector<double> vals = generateRandomValues(min_limits_, max_limits_);
for(unsigned int i = 0; i < vals.size(); i++) {
joint_value_map[joint_names_[i]] = vals[i];
}
kin_state->setKinematicState(joint_value_map);
if(!collision_models_->isKinematicStateInCollision(*kin_state)) {
break;
}
}
planning_environment::convertKinematicStateToRobotState(*kin_state, ros::Time::now(), collision_models_->getWorldFrameId(),
planning_scene_.robot_state);
ik_request.ik_request.ik_seed_state.joint_state.position = generateRandomValues(min_limits_, max_limits_);
ASSERT_TRUE(ik_with_collision_client_.call(ik_request, ik_response));
EXPECT_EQ(ik_response.error_code.val, ik_response.error_code.SUCCESS);
}
collision_models_->revertPlanningScene(kin_state);
}
void StackedBarChart::setGroupLabels(const QStringList &groupLabels)
{
*m_groupLabels = groupLabels;
generateRandomValues();
update();
}
void StackedBarChart::setSubGroupLabels(const QStringList &subGroupLabels)
{
*m_subGroupLabels = subGroupLabels;
m_key.updateSize();
generateRandomValues();
update();
}
GroupedBarChart::GroupedBarChart(QWidget *parent)
: BarChart(parent),
m_groupLabels(std::shared_ptr<QStringList>(new QStringList({"Group 1", "Group 2", "Group 3"}))),
m_subGroupLabels(std::shared_ptr<QStringList>(new QStringList({"A", "B", "C"}))),
m_key(m_subGroupLabels, m_colors),
m_fontMetrics(m_font)
{
resize(400, 300);
setMinimumSize(200, 150);
m_title.setText("Grouped Bar Chart");
m_yAxis.setMin(0.0);
generateRandomValues();
update();
}
StackedBarChart::StackedBarChart(QWidget *parent)
: BarChart(parent),
m_groupLabels(std::shared_ptr<QStringList>(new QStringList({"G1", "G2", "G3", "G4", "G5", "G6"}))),
m_subGroupLabels(std::shared_ptr<QStringList>(new QStringList({"A", "B", "C"}))),
m_key(m_subGroupLabels, m_colors),
m_normalize(true),
m_fontMetrics(m_font)
{
resize(400, 300);
setMinimumSize(200, 150);
m_title.setText("Stacked Bar Chart");
m_key.setInvertOrder(true);
m_yAxis.setNormalized(m_normalize);
generateRandomValues();
update();
}
