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;
}
