void CartVelController::updatePosition(const VectorNd& position) { if (q_.size() != position.size()) { ROS_ERROR_STREAM("Size of joint position vector (" << position.size() << ") doesn't match chain size (" << kdl_chain_.getNrOfJoints() << ")."); return; } q_ = position; }
/// Multiplies a vector of spatial axes by a vector SVelocityd mult(const vector<SVelocityd>& t, const VectorNd& v) { const unsigned SPATIAL_DIM = 6; if (t.size() != v.size()) throw MissizeException(); // verify that the vector is not empty - we lose frame info! if (t.empty()) throw std::runtime_error("loss of frame information"); // setup the result SVelocityd result = SVelocityd::zero(); // verify that all twists are in the same pose result.pose = t.front().pose; for (unsigned i=1; i< t.size(); i++) if (t[i].pose != result.pose) throw FrameException(); // finally, do the computation const double* vdata = v.data(); for (unsigned j=0; j< SPATIAL_DIM; j++) for (unsigned i=0; i< t.size(); i++) result[j] += t[i][j]*vdata[i]; return result; }
/// Gets a list of space-delimited and/or comma-delimited vectors from the underlying string value void XMLAttrib::get_vector_value(SVector6d& v) { // indicate this attribute has been processed processed = true; VectorNd w = VectorNd::parse(value); if (w.size() != v.size()) throw MissizeException(); v = SVector6d(w[0], w[1], w[2], w[3], w[4], w[5]); }
/// Returns a quaternion value from a roll-pitch-yaw attribute Quatd XMLAttrib::get_rpy_value() { // indicate this attribute has been processed processed = true; VectorNd v = VectorNd::parse(value); if (v.size() != 3) throw std::runtime_error("Unable to parse roll-pitch-yaw from vector!"); return Quatd::rpy(v[0], v[1], v[2]); }
bool CartVelController::update(const Vector6d& command, VectorNd& velocities) { // init if (!initialized_) { ROS_ERROR("Controller wasn't initilized before calling 'update'."); return false; } setCommand(command); if (velocities.size() != kdl_chain_.getNrOfJoints()) { ROS_ERROR_STREAM("Size of velocities vector (" << velocities.size() << ") doesn't match number of joints (" << kdl_chain_.getNrOfJoints() << ")."); return false; } for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++) { velocities(i) = 0.0; } // forward kinematics // KDL::Frame frame_tip_pose; // if(chain_fk_solver_->JntToCart(*q_, frame_tip_pose) < 0) { // ROS_ERROR("Unable to compute forward kinematics"); // return false; // } // transform vel command to root frame // not necessary, command is in root frame // KDL::Frame frame_tip_pose_inv = frame_tip_pose.Inverse(); // KDL::Twist linear_twist = frame_tip_pose_inv * cmd_linear_twist_; // KDL::Twist angular_twist = frame_tip_pose_inv.M * cmd_angular_twist_; // KDL::Twist twist(linear_twist.vel, angular_twist.rot); // cartesian to joint space ConversionHelper::eigenToKdl(q_, qtmp_); ConversionHelper::eigenToKdl(cmd_twist_, twist_tmp_); if(chain_ik_solver_vel_->CartToJnt(qtmp_, twist_tmp_, joint_vel_tmp_) < 0) { ROS_ERROR("Unable to compute cartesian to joint velocity"); return false; } // assign values to output. ConversionHelper::kdlToEigen(joint_vel_tmp_, velocities); return true; }
/// Constructs a vector-valued attribute with the given name XMLAttrib::XMLAttrib(const std::string& name, const VectorNd& vector_value) { this->processed = false; this->name = name; std::ostringstream oss; // if the vector is empty, return prematurely if (vector_value.size() == 0) { this->value = ""; return; } // set the first value of the vector oss << str(vector_value[0]); // set subsequent values of the vector for (unsigned i=1; i< vector_value.size(); i++) oss << " " << str(vector_value[i]); // get the string value this->value = oss.str(); }
/// Returns an Origin3d value from the attribute Origin3d XMLAttrib::get_origin_value() { // indicate this attribute has been processed processed = true; VectorNd v = VectorNd::parse(value); if (v.size() != 3) throw std::runtime_error("Unable to parse origin from vector!"); Origin3d o; o.x() = v[0]; o.y() = v[1]; o.z() = v[2]; return o; }
/// Returns a quaternion value from the attribute Quatd XMLAttrib::get_quat_value() { // indicate this attribute has been processed processed = true; VectorNd v = VectorNd::parse(value); if (v.size() != 4) throw std::runtime_error("Unable to parse quaternion from vector!"); Quatd q; q.w = v[0]; q.x = v[1]; q.y = v[2]; q.z = v[3]; return q; }
// simulator callback void post_step_callback(Simulator* sim) { // output the sliding velocity at the contact std::ofstream out("rke.dat", std::ostream::app); out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl; out.close(); // save the generalized coordinates of the box out.open("telemetry.box", std::ostream::app); VectorNd q; box->get_generalized_coordinates_euler(q); out << sim->current_time; for (unsigned i=0; i< q.size(); i++) out << " " << q[i]; out << std::endl; out.close(); }
/// runs the simulator and updates all transforms bool step(void* arg) { // get the simulator pointer boost::shared_ptr<Simulator> s = *(boost::shared_ptr<Simulator>*) arg; // get the generalized coordinates for all bodies in alphabetical order std::vector<ControlledBodyPtr> bodies = s->get_dynamic_bodies(); std::sort(bodies.begin(), bodies.end(), compbody); VectorNd q; outfile << s->current_time; for (unsigned i=0; i< bodies.size(); i++) { shared_ptr<DynamicBodyd> db = dynamic_pointer_cast<DynamicBodyd>(bodies[i]); db->get_generalized_coordinates_euler(q); for (unsigned j=0; j< q.size(); j++) outfile << " " << q[j]; } outfile << std::endl; // output the iteration # if (OUTPUT_ITER_NUM) std::cout << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl; if (Log<OutputToFile>::reporting_level > 0) FILE_LOG(Log<OutputToFile>::reporting_level) << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl; // step the simulator and update visualization clock_t pre_sim_t = clock(); s->step(STEP_SIZE); clock_t post_sim_t = clock(); double total_t = (post_sim_t - pre_sim_t) / (double) CLOCKS_PER_SEC; TOTAL_TIME += total_t; // output the iteration / stepping rate if (OUTPUT_SIM_RATE) std::cout << "time to compute last iteration: " << total_t << " (" << TOTAL_TIME / ITER << "s/iter, " << TOTAL_TIME / s->current_time << "s/step)" << std::endl; // update the iteration # ITER++; // check that maximum number of iterations or maximum time not exceeded if (ITER >= MAX_ITER || s->current_time > MAX_TIME) return false; return true; }