void nsMsgComposeSecure::SetErrorWithParam(nsIMsgSendReport *sendReport, const PRUnichar *bundle_string, const char *param)
{
if (!sendReport || !bundle_string || !param)
return;
if (mErrorAlreadyReported)
return;
mErrorAlreadyReported = true;
nsString errorString;
nsresult res;
const PRUnichar *params[1];
NS_ConvertASCIItoUTF16 ucs2(param);
params[0]= ucs2.get();
res = SMIMEBundleFormatStringFromName(bundle_string,
params,
1,
getter_Copies(errorString));
if (NS_SUCCEEDED(res) && !errorString.IsEmpty())
{
sendReport->SetMessage(nsIMsgSendReport::process_Current,
errorString.get(),
true);
}
}
TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
{
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
ks.update();
robot_state::RobotState ks_const(kmodel);
ks_const.setToDefaultValues();
ks_const.update();
robot_state::Transforms &tf = ps->getTransformsNonConst();
kinematic_constraints::JointConstraint jc1(kmodel);
std::map<std::string, double> state_values;
moveit_msgs::JointConstraint torso_constraint;
torso_constraint.joint_name = "torso_lift_joint";
torso_constraint.position = ks.getVariablePosition("torso_lift_joint");
torso_constraint.tolerance_above = 0.01;
torso_constraint.tolerance_below = 0.01;
torso_constraint.weight = 1.0;
EXPECT_TRUE(jc1.configure(torso_constraint));
kinematic_constraints::JointConstraint jc2(kmodel);
moveit_msgs::JointConstraint jcm2;
jcm2.joint_name = "r_elbow_flex_joint";
jcm2.position = ks.getVariablePosition("r_elbow_flex_joint");
jcm2.tolerance_above = 0.01;
jcm2.tolerance_below = 0.01;
jcm2.weight = 1.0;
EXPECT_TRUE(jc2.configure(jcm2));
moveit_msgs::PositionConstraint pcm;
pcm.link_name = "l_wrist_roll_link";
pcm.target_point_offset.x = 0;
pcm.target_point_offset.y = 0;
pcm.target_point_offset.z = 0;
pcm.constraint_region.primitives.resize(1);
pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
pcm.constraint_region.primitives[0].dimensions.resize(1);
pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
pcm.constraint_region.primitive_poses.resize(1);
pcm.constraint_region.primitive_poses[0].position.x = 0.55;
pcm.constraint_region.primitive_poses[0].position.y = 0.2;
pcm.constraint_region.primitive_poses[0].position.z = 1.25;
pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
pcm.weight = 1.0;
pcm.header.frame_id = kmodel->getModelFrame();
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "l_wrist_roll_link";
ocm.header.frame_id = kmodel->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.4;
ocm.weight = 1.0;
std::vector<kinematic_constraints::JointConstraint> js;
js.push_back(jc1);
boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp(new constraint_samplers::JointConstraintSampler(ps, "arms_and_torso"));
EXPECT_TRUE(jcsp->configure(js));
std::vector<kinematic_constraints::JointConstraint> js2;
js2.push_back(jc2);
boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp2(new constraint_samplers::JointConstraintSampler(ps, "arms"));
EXPECT_TRUE(jcsp2->configure(js2));
kinematic_constraints::PositionConstraint pc(kmodel);
EXPECT_TRUE(pc.configure(pcm, tf));
kinematic_constraints::OrientationConstraint oc(kmodel);
EXPECT_TRUE(oc.configure(ocm, tf));
boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp(new constraint_samplers::IKConstraintSampler(ps, "left_arm"));
EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
EXPECT_TRUE(iksp->isValid());
std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
cspv.push_back(jcsp2);
cspv.push_back(iksp);
cspv.push_back(jcsp);
constraint_samplers::UnionConstraintSampler ucs(ps, "arms_and_torso", cspv);
//should have reordered to place whole body first
constraint_samplers::JointConstraintSampler* jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
EXPECT_TRUE(jcs);
EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
constraint_samplers::JointConstraintSampler* jcs2 = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[1].get());
EXPECT_TRUE(jcs2);
EXPECT_EQ(jcs2->getJointModelGroup()->getName(), "arms");
for (int t = 0 ; t < 100; ++t)
{
EXPECT_TRUE(ucs.sample(ks, ks_const, 100));
ks.update();
ks_const.update();
EXPECT_TRUE(jc1.decide(ks).satisfied);
EXPECT_TRUE(jc2.decide(ks).satisfied);
EXPECT_TRUE(pc.decide(ks).satisfied);
}
//now we add a position constraint on right arm
pcm.link_name = "r_wrist_roll_link";
ocm.link_name = "r_wrist_roll_link";
cspv.clear();
kinematic_constraints::PositionConstraint pc2(kmodel);
EXPECT_TRUE(pc2.configure(pcm, tf));
kinematic_constraints::OrientationConstraint oc2(kmodel);
EXPECT_TRUE(oc2.configure(ocm, tf));
boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp2(new constraint_samplers::IKConstraintSampler(ps, "right_arm"));
EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
EXPECT_TRUE(iksp2->isValid());
//totally disjoint, so should break ties based on alphabetical order
cspv.clear();
cspv.push_back(iksp2);
cspv.push_back(iksp);
constraint_samplers::UnionConstraintSampler ucs2(ps, "arms_and_torso", cspv);
constraint_samplers::IKConstraintSampler* ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs2.getSamplers()[0].get());
ASSERT_TRUE(ikcs_test);
EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "left_arm");
//now we make left depends on right, right should stay first
pcm.link_name = "l_wrist_roll_link";
ocm.link_name = "l_wrist_roll_link";
pcm.header.frame_id = "r_wrist_roll_link";
ocm.header.frame_id = "r_wrist_roll_link";
EXPECT_TRUE(pc.configure(pcm, tf));
EXPECT_TRUE(oc.configure(ocm, tf));
ASSERT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
cspv.clear();
cspv.push_back(iksp2);
cspv.push_back(iksp);
constraint_samplers::UnionConstraintSampler ucs3(ps, "arms_and_torso", cspv);
ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs3.getSamplers()[0].get());
EXPECT_TRUE(ikcs_test);
EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "right_arm");
}
