/** * The primitive is not cloned, nor is it unaltered; <b>this</b> points to * the pointer returned by this method (typically <b>primitive</b>). * \return primitive <i>unless the geometry of the underlying primitive is * inconsistent, degenerate, or non-convex<i>; in that case, * a corrected primitive will be returned. */ PrimitivePtr CollisionGeometry::set_geometry(PrimitivePtr primitive) { Quatd EYE; if (_single_body.expired()) throw std::runtime_error("CollisionGeometry::set_geometry() called before single body set!"); SingleBodyPtr sb(_single_body); RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(sb); if (rb && !Quatd::rel_equal(rb->get_pose()->q, EYE)) { std::cerr << "CollisionGeometry::set_primitive() warning - rigid body's orientation is not identity." << std::endl; std::cerr << " At the rigid body's current orientation (" << AAngled(rb->get_pose()->q) << ")" << std::endl; std::cerr << " the primitive will have the orientation (" << AAngled(primitive->get_pose()->q) << ")" << std::endl; } // save the primitive _geometry = primitive; // add this to the primitive CollisionGeometryPtr cg = dynamic_pointer_cast<CollisionGeometry>(shared_from_this()); primitive->add_collision_geometry(cg); return primitive; }
/// Sets the pointer to the inboard link for this joint (and updates the spatial axes, if the outboard link has been set) void Joint::set_inboard_link(RigidBodyPtr inboard, bool update_pose) { _inboard_link = inboard; if (!inboard) return; // add this joint to the outer joints inboard->_outer_joints.insert(get_this()); // setup F's pose relative to the inboard set_inboard_pose(inboard->get_pose(), update_pose); // update articulated body pointers, if possible if (!inboard->get_articulated_body() && !_abody.expired()) inboard->set_articulated_body(ArticulatedBodyPtr(_abody)); else if (inboard->get_articulated_body() && _abody.expired()) set_articulated_body(ArticulatedBodyPtr(inboard->get_articulated_body())); // the articulated body pointers must now be equal; it is // conceivable that the user is updating the art. body pointers in an // unorthodox manner, but we'll look for this anwyway... if (!_abody.expired()) { ArticulatedBodyPtr abody1(inboard->get_articulated_body()); ArticulatedBodyPtr abody2(_abody); assert(abody1 == abody2); } }
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms void UniversalJoint::determine_q(VectorNd& q) { const unsigned X = 0, Y = 1, Z = 2; // get the outboard link RigidBodyPtr outboard = get_outboard_link(); // verify that the outboard link is set if (!outboard) throw std::runtime_error("determine_q() called on NULL outboard link!"); // set proper size for q this->q.resize(num_dof()); // get the poses of the joint and outboard link shared_ptr<const Pose3d> Fj = get_pose(); shared_ptr<const Pose3d> Fo = outboard->get_pose(); // compute transforms Transform3d wTo = Pose3d::calc_relative_pose(Fo, GLOBAL); Transform3d jTw = Pose3d::calc_relative_pose(GLOBAL, Fj); Transform3d jTo = jTw * wTo; // determine the joint transformation Matrix3d R = jTo.q; // determine q1 and q2 -- they are uniquely determined by examining the rotation matrix // (see get_rotation()) q.resize(num_dof()); q[DOF_1] = std::atan2(R(Z,Y), R(Y,Y)); q[DOF_2] = std::atan2(R(X,Z), R(X,X)); }
/// Sets the location of this joint with specified inboard and outboard links void Joint::set_location(const Point3d& point, RigidBodyPtr inboard, RigidBodyPtr outboard) { assert(inboard && outboard); // convert p to the inboard and outboard links' frames Point3d pi = Pose3d::transform_point(inboard->get_pose(), point); Point3d po = Pose3d::transform_point(outboard->get_pose(), point); // set _F's and Fb's origins _F->x = Origin3d(pi); _Fb->x = Origin3d(po); // invalidate all outboard pose vectors outboard->invalidate_pose_vectors(); // set inboard and outboard links set_inboard_link(inboard, false); set_outboard_link(outboard, false); }
/// Gets the farthest point from this geometry double CollisionGeometry::get_farthest_point_distance() const { // get the primitive from this PrimitivePtr primitive = get_geometry(); // get the vertices vector<Point3d> verts; get_vertices(verts); if (verts.empty()) return 0.0; // get the rigid body pose in P's frame RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(get_single_body()); Point3d rbX = Pose3d::transform_point(verts.front().pose, Point3d(0,0,0,rb->get_pose())); // find which point is closest double max_dist = 0.0; for (unsigned i=0; i< verts.size(); i++) max_dist = std::max(max_dist, (verts[i] - rbX).norm()); return max_dist; }