//--------------------------------------------------------------------------------------------------
void Camera::updatePickLines()
{
// Now draw lines from the pick points
double halfVerticalAngle = 0.5*Utilities::degToRad( mpVtkCamera->GetViewAngle() );
double verticalLength = tan( halfVerticalAngle );
double horizontalLength = verticalLength*(mImageWidth/mImageHeight);
Eigen::Vector3d cameraPos;
Eigen::Vector3d cameraAxisX;
Eigen::Vector3d cameraAxisY;
Eigen::Vector3d cameraAxisZ;
mpVtkCamera->GetPosition( cameraPos[ 0 ], cameraPos[ 1 ], cameraPos[ 2 ] );
mpVtkCamera->GetDirectionOfProjection( cameraAxisZ[ 0 ], cameraAxisZ[ 1 ], cameraAxisZ[ 2 ] );
mpVtkCamera->GetViewUp( cameraAxisY[ 0 ], cameraAxisY[ 1 ], cameraAxisY[ 2 ] );
cameraAxisX = cameraAxisY.cross( cameraAxisZ );
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
for ( uint32_t pickPointIdx = 0; pickPointIdx < mPickPoints.size(); pickPointIdx++ )
{
const Eigen::Vector2d& p = mPickPoints[ pickPointIdx ];
Eigen::Vector3d startPos = cameraPos;
Eigen::Vector3d rayDir = cameraAxisZ
- p[ 0 ]*horizontalLength*cameraAxisX
- p[ 1 ]*verticalLength*cameraAxisY;
rayDir.normalize();
Eigen::Vector3d endPos = startPos + 2.0*rayDir;
// Create the line
points->InsertNextPoint( startPos.data() );
points->InsertNextPoint( endPos.data() );
vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
line->GetPointIds()->SetId( 0, 2*pickPointIdx );
line->GetPointIds()->SetId( 1, 2*pickPointIdx + 1 );
// Store the line
lines->InsertNextCell( line );
}
// Create a polydata to store everything in
vtkSmartPointer<vtkPolyData> linesPolyData = vtkSmartPointer<vtkPolyData>::New();
// Add the points and lines to the dataset
linesPolyData->SetPoints( points );
linesPolyData->SetLines( lines );
mpPickLinesMapper->SetInput( linesPolyData );
}
IGL_INLINE int igl::unproject(
const Eigen::PlainObjectBase<Derivedwin> & win,
Eigen::PlainObjectBase<Derivedobj> & obj)
{
Eigen::Vector3d dwin(win(0),win(1),win(2));
Eigen::Vector3d dobj;
int ret = unproject(dwin(0),dwin(1),dwin(2),
&dobj.data()[0],
&dobj.data()[1],
&dobj.data()[2]);
obj(0) = dobj(0);
obj(1) = dobj(1);
obj(2) = dobj(2);
return ret;
}
void
ExtendedHandEyeCalibration::refine(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& H1,
const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& H2,
Eigen::Matrix4d& H_12,
double& s) const
{
Eigen::Quaterniond q(H_12.block<3,3>(0,0));
Eigen::Vector3d t = H_12.block<3,1>(0,3);
ceres::Problem problem;
for (size_t i = 0; i < H1.size(); i++)
{
ceres::CostFunction* costFunction =
new ceres::AutoDiffCostFunction<PoseError, 6, 4, 3, 1>(
new PoseError(H1.at(i), H2.at(i)));
problem.AddResidualBlock(costFunction, 0, q.coeffs().data(), t.data(), &s);
}
ceres::LocalParameterization* quaternionParameterization =
new EigenQuaternionParameterization;
problem.SetParameterization(q.coeffs().data(), quaternionParameterization);
ceres::Solver::Options options;
options.gradient_tolerance = 1e-12;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 500;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
H_12.block<3,3>(0,0) = q.toRotationMatrix();
H_12.block<3,1>(0,3) = t;
}
IGL_INLINE int igl::opengl2::project(
const Eigen::PlainObjectBase<Derivedobj> & obj,
Eigen::PlainObjectBase<Derivedwin> & win)
{
assert(obj.size() >= 3);
Eigen::Vector3d dobj(obj(0),obj(1),obj(2));
Eigen::Vector3d dwin;
int ret = igl::opengl2::project(dobj(0),dobj(1),dobj(2),
&dwin.data()[0],
&dwin.data()[1],
&dwin.data()[2]);
win(0) = dwin(0);
win(1) = dwin(1);
win(2) = dwin(2);
return ret;
}
/** \brief compute the 3 eigen values and eigenvectors for a 3x3 covariance matrix
* \param covariance_matrix a 3x3 covariance matrix in eigen2::matrix3d format
* \param eigen_values the resulted eigenvalues in eigen2::vector3d
* \param eigen_vectors a 3x3 matrix in eigen2::matrix3d format, containing each eigenvector on a new line
*/
bool
eigen_cov (Eigen::Matrix3d covariance_matrix, Eigen::Vector3d &eigen_values, Eigen::Matrix3d &eigen_vectors)
{
char jobz = 'V'; // 'V': Compute eigenvalues and eigenvectors
char uplo = 'U'; // 'U': Upper triangle of A is stored
int n = 3, lda = 3, info = -1;
int lwork = 3 * n - 1;
double *work = new double[lwork];
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
eigen_vectors (i, j) = covariance_matrix (i, j);
dsyev_ (&jobz, &uplo, &n, eigen_vectors.data (), &lda, eigen_values.data (), work, &lwork, &info);
delete work;
return (info == 0);
}
void drawTiltPlane(
const Eigen::Vector3d& normal,
const Eigen::Vector3d& tilt,
double offset, double roffset, double width){
Eigen::Vector3d tangent1, tangent2;
tangent1 = normal.cross(Eigen::Vector3d{1,0,0});
if(tangent1.isZero(1e-3)){
tangent1 = normal.cross(Eigen::Vector3d{0,0,1});
if(tangent1.isZero(1e-3)){
tangent1 = normal.cross(Eigen::Vector3d{0,1,0});
}
}
tangent1.normalize();
tangent2 = normal.cross(tangent1);
tangent2.normalize(); //probably not necessary
Eigen::AngleAxisd t(roffset,tilt);
tangent1 = t * tangent1;
tangent2 = t * tangent2;
const double sos = normal.dot(normal);
const Eigen::Vector3d supportPoint{normal.x()*offset/sos,
normal.y()*offset/sos,
normal.z()*offset/sos};
const double size = width;
glBegin(GL_QUADS);
glNormal3dv(normal.data());
glVertex3dv((supportPoint + size*(tangent1 + tangent2)).eval().data());
glVertex3dv((supportPoint + size*(-tangent1 + tangent2)).eval().data());
glVertex3dv((supportPoint + size*(-tangent1 - tangent2)).eval().data());
glVertex3dv((supportPoint + size*(tangent1 - tangent2)).eval().data());
glEnd();
}
Eigen::Vector3d zsw::AdVectorIntegrator::operator()(const double* pos) const
{
if(vfs_.size() < 3) {
Eigen::Vector3d vel;
vfs_[vfs_.size()-1]->val(pos, vel.data());
return vel*h_;
}
Eigen::Vector3d ori_pos, tmp_pos;
std::copy(pos, pos+3, ori_pos.data());
Eigen::Vector3d k1;
vfs_[vfs_.size()-3]->val(pos, k1.data());
Eigen::Vector3d k2;
tmp_pos = ori_pos + h_*k1;
vfs_[vfs_.size()-2]->val(tmp_pos.data(), k2.data());
Eigen::Vector3d k3;
tmp_pos = ori_pos +2*h_*(-k1+2*k2);
vfs_[vfs_.size()-1]->val(tmp_pos.data(), k3.data());
return (k1+4*k2+k3)/6*h_;
}
Eigen::Vector3d Quaternion2Euler(float x, float y, float z, float w) {
KDL::Rotation kdl_rotation = KDL::Rotation::Quaternion(x, y, z, w);
Eigen::Vector3d result;
kdl_rotation.GetRPY(result.data()[2], result.data()[1], result.data()[0]);
return result;
}
// order: A, B, C
Eigen::Vector3d Quaternion2Euler(double x, double y, double z, double w) {
KDL::Rotation kdl_rotation = KDL::Rotation::Quaternion(x, y, z, w);
Eigen::Vector3d result;
kdl_rotation.GetRPY(result.data()[2], result.data()[1], result.data()[0]);
return result;
}
void EstimateConductionVelocity(vtkDataSet* mesh, const char* isoch_array)
{
vtkDataArray* isoc = mesh->GetPointData()->GetArray(isoch_array);
vtkSmartPointer<vtkFloatArray> velocity = vtkSmartPointer<vtkFloatArray>::New();
velocity->SetName("Velocity");
velocity->SetNumberOfComponents(1);
velocity->SetNumberOfTuples(mesh->GetNumberOfCells());
for(vtkIdType cellid=0; cellid<mesh->GetNumberOfCells(); cellid++)
{
if( cellid % 100000 == 0 )
{
std::cout<<"Processing point "<<cellid<<"/"<<mesh->GetNumberOfCells()<<"\r"<<std::flush;
}
vtkCell *cell = mesh->GetCell(cellid);
Eigen::Vector3d direction;
//calculate the gradient within the cell
double pcenter[3];
int subId = cell->GetParametricCenter(pcenter);
//save the isochrone values in the same order as the vertices
Eigen::VectorXd iso_values(cell->GetNumberOfPoints());
for(int i=0; i<iso_values.size(); i++) iso_values[i] = isoc->GetTuple1(cell->GetPointId(i));
cell->Derivatives(0, pcenter, iso_values.data(), 1, direction.data());
direction.normalize();
//std::cout<<"Cell: "<<cellid<<std::endl;
//std::cout<<"Gradient: "<<direction<<std::endl;
//project all the vertices onto the gradient direction
//get cell center
Eigen::Vector3d center;
Eigen::VectorXd weights(cell->GetNumberOfPoints());
cell->EvaluateLocation(subId, pcenter, center.data(), weights.data());
//prepare storage for the projection coordinates.
//center of the cell will be the origin
Eigen::VectorXd vertex_projections( cell->GetNumberOfPoints() );
//find the longest projection of the direction on the edges
for(int ptid = 0; ptid < cell->GetNumberOfPoints(); ptid++)
{
Eigen::Vector3d vertex;
mesh->GetPoint(cell->GetPointId(ptid), vertex.data());
Eigen::Vector3d vertex_vector = vertex-center;
vertex_projections[ptid] = vertex_vector.dot(direction);
}
//find the projections with the smallest and largest coordinate
int max_vertex_id = 0;
int min_vertex_id = 1;
double max_time = iso_values[max_vertex_id];
double min_time = iso_values[min_vertex_id];
//std::cout<<"Projection Coordinates: "<<vertex_projections<<std::endl;
//std::cout<<"Time values: "<<iso_values<<std::endl;
for(int i=0; i<iso_values.size(); i++)
{
if( max_time<iso_values[i] )
{
max_time = iso_values[i];
max_vertex_id = i;
}
if( min_time>iso_values[i] )
{
min_time = iso_values[i];
min_vertex_id = i;
}
}
//std::cout<<"Min id: "<<min_vertex_id<<", max id: "<<max_vertex_id<<std::endl;
//std::cout<<"Min time:"<<min_time<<", max time: "<<max_time<<std::endl;
double v=0;
double max_time_diff = max_time-min_time;
double coordinate_diff = fabs(vertex_projections[max_vertex_id] - vertex_projections[min_vertex_id]);
if(max_time_diff>0)
{
v = coordinate_diff/max_time_diff;
//std::cout<<"Velocity:"<<v<<std::endl;
}
velocity->SetTuple1(cellid, v);
}
std::cout<<std::endl;
mesh->GetCellData()->AddArray(velocity);
}
void Light::setPosition(const Eigen::Vector3d& position)
{
setPosition(position.data());
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
// DEBUG
// mexPrintf("constructModelmex: START\n");
// END_DEBUG
mxArray* pm;
if (nrhs != 1) {
mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs",
"Usage model_ptr = constructModelmex(obj)");
}
if (isa(prhs[0], "DrakeMexPointer")) { // then it's calling the destructor
destroyDrakeMexPointer<RigidBodyTree*>(prhs[0]);
return;
}
const mxArray* pRBM = prhs[0];
RigidBodyTree* model = new RigidBodyTree();
model->bodies.clear(); // a little gross: the default constructor makes a
// body "world". zap it because we will construct one
// again below
// model->robot_name = get_strings(mxGetPropertySafe(pRBM, 0,"name"));
const mxArray* pBodies = mxGetPropertySafe(pRBM, 0, "body");
if (!pBodies)
mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs",
"the body array is invalid");
int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies));
const mxArray* pFrames = mxGetPropertySafe(pRBM, 0, "frame");
if (!pFrames)
mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs",
"the frame array is invalid");
int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames));
for (int i = 0; i < num_bodies; i++) {
// DEBUG
// mexPrintf("constructModelmex: body %d\n", i);
// END_DEBUG
std::unique_ptr<RigidBody> b(new RigidBody());
b->set_body_index(i);
b->set_name(mxGetStdString(mxGetPropertySafe(pBodies, i, "linkname")));
pm = mxGetPropertySafe(pBodies, i, "robotnum");
b->set_model_instance_id((int)mxGetScalar(pm) - 1);
pm = mxGetPropertySafe(pBodies, i, "mass");
b->set_mass(mxGetScalar(pm));
pm = mxGetPropertySafe(pBodies, i, "com");
Eigen::Vector3d com;
if (!mxIsEmpty(pm)) {
memcpy(com.data(), mxGetPrSafe(pm), sizeof(double) * 3);
b->set_center_of_mass(com);
}
pm = mxGetPropertySafe(pBodies, i, "I");
if (!mxIsEmpty(pm)) {
drake::SquareTwistMatrix<double> I;
memcpy(I.data(), mxGetPrSafe(pm), sizeof(double) * 6 * 6);
b->set_spatial_inertia(I);
}
pm = mxGetPropertySafe(pBodies, i, "position_num");
b->set_position_start_index((int)mxGetScalar(pm) - 1); // zero-indexed
pm = mxGetPropertySafe(pBodies, i, "velocity_num");
b->set_velocity_start_index((int)mxGetScalar(pm) - 1); // zero-indexed
pm = mxGetPropertySafe(pBodies, i, "parent");
if (!pm || mxIsEmpty(pm)) {
b->set_parent(nullptr);
} else {
int parent_ind = static_cast<int>(mxGetScalar(pm)) - 1;
if (parent_ind >= static_cast<int>(model->bodies.size()))
mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs",
"bad body.parent %d (only have %d bodies)",
parent_ind, model->bodies.size());
if (parent_ind >= 0) b->set_parent(model->bodies[parent_ind].get());
}
if (b->has_parent_body()) {
string joint_name =
mxGetStdString(mxGetPropertySafe(pBodies, i, "jointname"));
// mexPrintf("adding joint %s\n", joint_name.c_str());
pm = mxGetPropertySafe(pBodies, i, "Ttree");
// todo: check that the size is 4x4
Isometry3d transform_to_parent_body;
memcpy(transform_to_parent_body.data(), mxGetPrSafe(pm),
sizeof(double) * 4 * 4);
int floating =
(int)mxGetScalar(mxGetPropertySafe(pBodies, i, "floating"));
Eigen::Vector3d joint_axis;
pm = mxGetPropertySafe(pBodies, i, "joint_axis");
memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3);
double pitch = mxGetScalar(mxGetPropertySafe(pBodies, i, "pitch"));
std::unique_ptr<DrakeJoint> joint;
switch (floating) {
case 0: {
if (pitch == 0.0) {
RevoluteJoint* revolute_joint = new RevoluteJoint(
joint_name, transform_to_parent_body, joint_axis);
joint = std::unique_ptr<RevoluteJoint>(revolute_joint);
setLimits(pBodies, i, revolute_joint);
setDynamics(pBodies, i, revolute_joint);
} else if (std::isinf(static_cast<double>(pitch))) {
PrismaticJoint* prismatic_joint = new PrismaticJoint(
joint_name, transform_to_parent_body, joint_axis);
joint = std::unique_ptr<PrismaticJoint>(prismatic_joint);
setLimits(pBodies, i, prismatic_joint);
setDynamics(pBodies, i, prismatic_joint);
} else {
joint = std::unique_ptr<HelicalJoint>(new HelicalJoint(
joint_name, transform_to_parent_body, joint_axis, pitch));
}
break;
}
case 1: {
joint = std::unique_ptr<RollPitchYawFloatingJoint>(
new RollPitchYawFloatingJoint(joint_name,
transform_to_parent_body));
break;
}
case 2: {
joint = std::unique_ptr<QuaternionFloatingJoint>(
new QuaternionFloatingJoint(joint_name,
transform_to_parent_body));
break;
}
default: {
std::ostringstream stream;
stream << "floating type " << floating << " not recognized.";
throw std::runtime_error(stream.str());
}
}
b->setJoint(std::move(joint));
}
// DEBUG
// mexPrintf("constructModelmex: About to parse collision geometry\n");
// END_DEBUG
pm = mxGetPropertySafe(pBodies, i, "collision_geometry");
Isometry3d T;
if (!mxIsEmpty(pm)) {
for (int j = 0; j < mxGetNumberOfElements(pm); j++) {
// DEBUG
// cout << "constructModelmex: Body " << i << ", Element " << j << endl;
// END_DEBUG
mxArray* pShape = mxGetCell(pm, j);
char* group_name_cstr =
mxArrayToString(mxGetPropertySafe(pShape, 0, "name"));
string group_name;
if (group_name_cstr) {
group_name = group_name_cstr;
} else {
group_name = "default";
}
// Get element-to-link transform from MATLAB object
memcpy(T.data(), mxGetPrSafe(mxGetPropertySafe(pShape, 0, "T")),
sizeof(double) * 4 * 4);
auto shape = (DrakeShapes::Shape) static_cast<int>(
mxGetScalar(mxGetPropertySafe(pShape, 0, "drake_shape_id")));
vector<double> params_vec;
RigidBodyCollisionElement element(T, b.get());
switch (shape) {
case DrakeShapes::BOX: {
double* params = mxGetPrSafe(mxGetPropertySafe(pShape, 0, "size"));
element.setGeometry(
DrakeShapes::Box(Vector3d(params[0], params[1], params[2])));
} break;
case DrakeShapes::SPHERE: {
double r(*mxGetPrSafe(mxGetPropertySafe(pShape, 0, "radius")));
element.setGeometry(DrakeShapes::Sphere(r));
} break;
case DrakeShapes::CYLINDER: {
double r(*mxGetPrSafe(mxGetPropertySafe(pShape, 0, "radius")));
double l(*mxGetPrSafe(mxGetPropertySafe(pShape, 0, "len")));
element.setGeometry(DrakeShapes::Cylinder(r, l));
} break;
case DrakeShapes::MESH: {
string filename(
mxArrayToString(mxGetPropertySafe(pShape, 0, "filename")));
element.setGeometry(DrakeShapes::Mesh(filename, filename));
} break;
case DrakeShapes::MESH_POINTS: {
mxArray* pPoints;
mexCallMATLAB(1, &pPoints, 1, &pShape, "getPoints");
int n_pts = static_cast<int>(mxGetN(pPoints));
Map<Matrix3Xd> pts(mxGetPrSafe(pPoints), 3, n_pts);
element.setGeometry(DrakeShapes::MeshPoints(pts));
mxDestroyArray(pPoints);
// The element-to-link transform is applied in
// RigidBodyMesh/getPoints - don't apply it again!
T = Matrix4d::Identity();
} break;
case DrakeShapes::CAPSULE: {
double r(*mxGetPrSafe(mxGetPropertySafe(pShape, 0, "radius")));
double l(*mxGetPrSafe(mxGetPropertySafe(pShape, 0, "len")));
element.setGeometry(DrakeShapes::Capsule(r, l));
} break;
default:
// intentionally do nothing..
// DEBUG
// cout << "constructModelmex: SHOULD NOT GET HERE" << endl;
// END_DEBUG
break;
}
// DEBUG
// cout << "constructModelmex: geometry = " << geometry.get() << endl;
// END_DEBUG
model->addCollisionElement(element, *b, group_name);
}
// NOTE: the following should not be necessary since the same thing is
// being done in RigidBodyTree::compile, which is called below.
// if (!model->bodies[i]->has_parent_body()) {
// model->updateCollisionElements(model->bodies[i], cache); //
// update static objects only once - right here on load
// }
// Set collision filtering bitmasks
pm = mxGetPropertySafe(pBodies, i, "collision_filter");
DrakeCollision::bitmask group, mask;
mxArray* belongs_to = mxGetField(pm, 0, "belongs_to");
mxArray* ignores = mxGetField(pm, 0, "ignores");
if (!(mxIsLogical(belongs_to)) || !isMxArrayVector(belongs_to)) {
cout << "is logical: " << mxIsLogical(belongs_to) << endl;
cout << "number of dimensions: " << mxGetNumberOfDimensions(belongs_to)
<< endl;
mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct",
"The 'belongs_to' field of the 'collision_filter' "
"struct must be a logical vector.");
}
if (!(mxIsLogical(ignores)) || !isMxArrayVector(ignores)) {
mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct",
"The 'ignores' field of the 'collision_filter' "
"struct must be a logical vector.");
}
size_t numel_belongs_to(mxGetNumberOfElements(belongs_to));
size_t numel_ignores(mxGetNumberOfElements(ignores));
size_t num_collision_filter_groups = max(numel_belongs_to, numel_ignores);
if (num_collision_filter_groups > MAX_NUM_COLLISION_FILTER_GROUPS) {
mexErrMsgIdAndTxt(
"Drake:constructModelmex:TooManyCollisionFilterGroups",
"The total number of collision filter groups (%d) "
"exceeds the maximum allowed number (%d)",
num_collision_filter_groups, MAX_NUM_COLLISION_FILTER_GROUPS);
}
mxLogical* logical_belongs_to = mxGetLogicals(belongs_to);
for (int j = 0; j < numel_belongs_to; ++j) {
if (static_cast<bool>(logical_belongs_to[j])) {
group.set(j);
}
}
mxLogical* logical_ignores = mxGetLogicals(ignores);
for (int j = 0; j < numel_ignores; ++j) {
if (static_cast<bool>(logical_ignores[j])) {
mask.set(j);
}
}
b->setCollisionFilter(group, mask);
}
model->bodies.push_back(std::move(b));
}
// THIS IS UGLY: I'm sending the terrain contact points into the
// contact_pts field of the cpp RigidBody objects
// DEBUG
// mexPrintf("constructModelmex: Parsing contact points (calling
// getTerrainContactPoints)\n");
// cout << "constructModelmex: Get struct" << endl;
// END_DEBUG
mxArray* contact_pts_struct[1];
if (!mexCallMATLAB(1, contact_pts_struct, 1, const_cast<mxArray**>(&pRBM),
"getTerrainContactPoints")) {
// DEBUG
// mexPrintf("constructModelmex: Got terrain contact points struct\n");
// if (contact_pts_struct) {
// cout << "constructModelmex: Struct pointer: " << contact_pts_struct <<
// endl;
//} else {
// cout << "constructModelmex: Struct pointer NULL" << endl;
//}
// cout << "constructModelmex: Get numel of struct" << endl;
// END_DEBUG
const int n_bodies_w_contact_pts =
static_cast<int>(mxGetNumberOfElements(contact_pts_struct[0]));
// DEBUG
// cout << "constructModelmex: Got numel of struct:" <<
// n_bodies_w_contact_pts << endl;
// END_DEBUG
mxArray* pPts;
int body_idx;
int n_pts;
for (int j = 0; j < n_bodies_w_contact_pts; j++) {
// DEBUG
// cout << "constructModelmex: Loop: Iteration " << j << endl;
// cout << "constructModelmex: Get body_idx" << endl;
// END_DEBUG
body_idx =
(int)mxGetScalar(mxGetField(contact_pts_struct[0], j, "idx")) - 1;
// DEBUG
// cout << "constructModelmex: Got body_idx: " << body_idx << endl;
// cout << "constructModelmex: Get points" << endl;
// END_DEBUG
pPts = mxGetField(contact_pts_struct[0], j, "pts");
// DEBUG
// cout << "constructModelmex: Get points" << endl;
// cout << "constructModelmex: Get number of points" << endl;
// END_DEBUG
n_pts = static_cast<int>(mxGetN(pPts));
// DEBUG
// cout << "constructModelmex: Got number of points: " << n_pts << endl;
// cout << "constructModelmex: Set contact_pts of body" << endl;
// END_DEBUG
Map<Matrix3Xd> pts(mxGetPrSafe(pPts), 3, n_pts);
model->bodies[body_idx]->set_contact_points(pts);
// DEBUG
// mexPrintf("constructModelmex: created %d contact points for body %d\n",
// n_pts, body_idx);
// cout << "constructModelmex: Contact_pts of body: " << endl;
// cout << model->bodies[body_idx]->contact_pts << endl;
// END_DEBUG
}
}
// FRAMES
// DEBUG
// mexPrintf("constructModelmex: Parsing frames\n");
// END_DEBUG
for (int i = 0; i < num_frames; i++) {
shared_ptr<RigidBodyFrame> fr(new RigidBodyFrame());
fr->set_name(mxGetStdString(mxGetPropertySafe(pFrames, i, "name")));
pm = mxGetPropertySafe(pFrames, i, "body_ind");
fr->set_rigid_body(model->bodies[(int)mxGetScalar(pm) - 1].get());
pm = mxGetPropertySafe(pFrames, i, "T");
memcpy(fr->get_mutable_transform_to_body()->data(), mxGetPrSafe(pm),
sizeof(double) * 4 * 4);
fr->set_frame_index(-i - 2);
model->frames.push_back(fr);
}
const mxArray* a_grav_array = mxGetPropertySafe(pRBM, 0, "gravity");
if (a_grav_array && mxGetNumberOfElements(a_grav_array) == 3) {
double* p = mxGetPrSafe(a_grav_array);
model->a_grav[3] = p[0];
model->a_grav[4] = p[1];
model->a_grav[5] = p[2];
} else {
mexErrMsgIdAndTxt(
"Drake:constructModelmex:BadGravity",
"Couldn't find a 3 element gravity vector in the object.");
}
// LOOP CONSTRAINTS
// DEBUG
// mexPrintf("constructModelmex: Parsing loop constraints\n");
// END_DEBUG
const mxArray* pLoops = mxGetPropertySafe(pRBM, 0, "loop");
int num_loops = static_cast<int>(mxGetNumberOfElements(pLoops));
model->loops.clear();
for (int i = 0; i < num_loops; i++) {
pm = mxGetPropertySafe(pLoops, i, "frameA");
int frame_A_ind = static_cast<int>(-mxGetScalar(pm) - 1);
if (frame_A_ind < 0 || frame_A_ind >= model->frames.size())
mexErrMsgIdAndTxt(
"Drake:constructModelmex:BadFrameNumber",
"Something is wrong, this doesn't point to a valid frame");
pm = mxGetPropertySafe(pLoops, i, "frameB");
int frame_B_ind = static_cast<int>(-mxGetScalar(pm) - 1);
if (frame_B_ind < 0 || frame_B_ind >= model->frames.size())
mexErrMsgIdAndTxt(
"Drake:constructModelmex:BadFrameNumber",
"Something is wrong, this doesn't point to a valid frame");
pm = mxGetPropertySafe(pLoops, i, "axis");
Vector3d axis;
memcpy(axis.data(), mxGetPrSafe(pm), 3 * sizeof(double));
// cout << "loop " << i << ": frame_A = " <<
// model->frames[frame_A_ind]->name << ", frame_B = " <<
// model->frames[frame_B_ind]->name << endl;
model->loops.push_back(RigidBodyLoop(model->frames[frame_A_ind],
model->frames[frame_B_ind], axis));
}
// ACTUATORS
// LOOP CONSTRAINTS
// DEBUG
// mexPrintf("constructModelmex: Parsing actuators\n");
// END_DEBUG
const mxArray* pActuators = mxGetPropertySafe(pRBM, 0, "actuator");
int num_actuators = static_cast<int>(mxGetNumberOfElements(pActuators));
model->actuators.clear();
for (int i = 0; i < num_actuators; i++) {
string name = mxGetStdString(mxGetPropertySafe(pActuators, i, "name"));
pm = mxGetPropertySafe(pActuators, i, "joint");
size_t joint_index = static_cast<size_t>(mxGetScalar(pm) - 1);
double reduction =
mxGetScalar(mxGetPropertySafe(pActuators, i, "reduction"));
double effort_min =
mxGetScalar(mxGetPropertySafe(pBodies, joint_index, "effort_min"));
double effort_max =
mxGetScalar(mxGetPropertySafe(pBodies, joint_index, "effort_max"));
model->actuators.push_back(
RigidBodyActuator(name, model->bodies[joint_index].get(), reduction,
effort_min, effort_max));
}
// LOOP CONSTRAINTS
// DEBUG
// mexPrintf("constructModelmex: Calling compile\n");
// END_DEBUG
model->compile();
// mexPrintf("constructModelmex: Creating DrakeMexPointer\n");
plhs[0] = createDrakeMexPointer((void*)model, "RigidBodyTree",
DrakeMexPointerTypeId<RigidBodyTree>::value);
// DEBUG
// mexPrintf("constructModelmex: END\n");
// END_DEBUG
}
bool setChannels(const std::string& iInputChannel,
const std::string& iOutputChannel) {
if (mSubscription != NULL) {
mLcm->unsubscribe(mSubscription);
}
mInputChannel = iInputChannel;
mOutputChannel = iOutputChannel;
BotCamTrans* inputCamTrans =
bot_param_get_new_camtrans(mBotWrapper->getBotParam(),
mInputChannel.c_str());
if (inputCamTrans == NULL) {
std::cout << "error: cannot find camera " << mInputChannel << std::endl;
return false;
}
BotCamTrans* outputCamTrans =
bot_param_get_new_camtrans(mBotWrapper->getBotParam(),
mOutputChannel.c_str());
if (outputCamTrans == NULL) {
std::cout << "error: cannot find camera " << mOutputChannel << std::endl;
return false;
}
int inputWidth = bot_camtrans_get_width(inputCamTrans);
int inputHeight = bot_camtrans_get_height(inputCamTrans);
mOutputWidth = bot_camtrans_get_width(outputCamTrans);
mOutputHeight = bot_camtrans_get_height(outputCamTrans);
// precompute warp field
mWarpFieldIntX.resize(mOutputWidth*mOutputHeight);
mWarpFieldIntY.resize(mWarpFieldIntX.size());
mWarpFieldFrac00.resize(mWarpFieldIntX.size());
mWarpFieldFrac01.resize(mWarpFieldIntX.size());
mWarpFieldFrac10.resize(mWarpFieldIntX.size());
mWarpFieldFrac11.resize(mWarpFieldIntX.size());
Eigen::Isometry3d outputToInput;
if (!mBotWrapper->getTransform(mOutputChannel, mInputChannel,
outputToInput)) {
std::cout << "error: cannot get transform from " << mOutputChannel <<
" to " << mInputChannel << std::endl;
return false;
}
Eigen::Matrix3d rotation = outputToInput.rotation();
Eigen::Vector3d ray;
for (int i = 0, pos = 0; i < mOutputHeight; ++i) {
for (int j = 0; j < mOutputWidth; ++j, ++pos) {
mWarpFieldIntX[pos] = -1;
if (0 != bot_camtrans_unproject_pixel(outputCamTrans, j, i,
ray.data())) {
continue;
}
ray = rotation*ray;
double pix[3];
if (0 != bot_camtrans_project_point(inputCamTrans, ray.data(), pix)) {
continue;
}
if ((pix[2] < 0) ||
(pix[0] < 0) || (pix[0] >= inputWidth-1) ||
(pix[1] < 0) || (pix[1] >= inputHeight-1)) {
continue;
}
mWarpFieldIntX[pos] = (int)pix[0];
mWarpFieldIntY[pos] = (int)pix[1];
double fracX = pix[0] - mWarpFieldIntX[pos];
double fracY = pix[1] - mWarpFieldIntY[pos];
mWarpFieldFrac00[pos] = (1-fracX)*(1-fracY);
mWarpFieldFrac01[pos] = (1-fracX)*fracY;
mWarpFieldFrac10[pos] = fracX*(1-fracY);
mWarpFieldFrac11[pos] = fracX*fracY;
}
}
mLcm->subscribe(mInputChannel, &ImageWarper::onImage, this);
return true;
}
