Record &operator=(const Record &rhs)
{
fields.resize(rhs.fields.size());
data_ = rhs.data_;
next_field_ix_ = 0;
next_ix_ = 0;
auto ptr = data_.data();
for (; next_field_ix_ != rhs.next_field_ix_; ++next_field_ix_)
{
const auto dim = rhs.fields[next_field_ix_].size();
fields[next_field_ix_] = Range(ptr, ptr+dim);
ptr += dim;
next_ix_ += dim;
}
return *this;
}
virtual void SetUp()
{
unsigned int num_constraints = 3;
unsigned int num_joints = 7;
true_constraints.resize(num_constraints, num_joints);
false_constraints.resize(num_constraints, num_joints);
std::vector<double> task_values;
Ranges true_cmd, false_cmd;
true_cmd.resize(num_constraints);
false_cmd.resize(num_constraints);
for(unsigned int i=0; i<num_constraints; i++)
{
true_cmd.pos_lo(i) = -1.0;
true_cmd.pos_hi(i) = 2.5;
task_values.push_back(0.25);
false_cmd.pos_lo(i) = -1.0;
false_cmd.pos_hi(i) = 2.5;
}
true_constraints.setTaskValues(task_values);
task_values[0] = (0.0);
task_values[1] = (-2.0);
task_values[2] = (5.5);
false_constraints.setTaskValues(task_values);
true_constraints.setCommands(true_cmd);
false_constraints.setCommands(false_cmd);
Feature pan, bottle;
pan.name = "pan";
pan.pos = KDL::Vector(0.0, 0.0, 0.0);
pan.dir = KDL::Vector(0.0, 0.0, 1.0);
pan.contact_dir = KDL::Vector(1.0, 0.0, 0.0);
bottle.name = "bottle";
bottle.pos = KDL::Vector(0.0, 0.0, 0.0);
bottle.dir = KDL::Vector(0.0, 0.0, 1.0);
bottle.contact_dir = KDL::Vector(1.0, 0.0, 0.0);
Constraint c1, c2, c3;
c1.name = "height of bottle over pan";
c1.setFunction("height");
c1.tool_feature = bottle;
c1.object_feature = pan;
c2.name = "distance of bottle over pan";
c2.setFunction("distance");
c2.tool_feature = bottle;
c2.object_feature = pan;
c3.name = "bottle upright";
c3.setFunction("perpendicular");
c3.tool_feature = bottle;
c3.object_feature = pan;
constraints.resize(3, num_joints);
std::vector<Constraint> constraint_vector;
constraint_vector.push_back(c1);
constraint_vector.push_back(c2);
constraint_vector.push_back(c3);
constraints.setConstraints(constraint_vector);
constraints.setObjectPose(KDL::Frame(KDL::Rotation::RotZ(M_PI/2.0), KDL::Vector(1.0, 1.0, 0.75)));
Ranges cmd;
cmd.resize(3);
// height
cmd.weight(0) = 1.0;
cmd.pos_lo(0) = 0.1;
cmd.pos_hi(0) = 2.0;
// distance
cmd.weight(1) = 1.0;
cmd.pos_lo(1) = 0.0;
cmd.pos_hi(1) = 0.1;
// upright
cmd.weight(2) = 1.0;
cmd.pos_lo(2) = 0.95;
cmd.pos_hi(2) = 1.2;
constraints.setCommands(cmd);
}