// Calculates dot product of two AD<double> vectors and two TapeDouble vectors.
// Vectors are declared as independent.
void performanceTestWithTape()
{
std::cout << std::endl << "Test performance for dot product calculation with tape recording" << std::endl;
std::vector<CppAD::AD<double>> leftAD = vectorCast<CppAD::AD<double>>(leftVector);
std::vector<CppAD::AD<double>> rightAD = vectorCast<CppAD::AD<double>>(rightVector);
std::vector<cl::TapeDouble> leftTapeDouble = vectorCast<cl::TapeDouble>(leftVector);
std::vector<cl::TapeDouble> rightTapeDouble = vectorCast<cl::TapeDouble>(rightVector);
CppAD::AD<double> ADResult = 0;
cl::TapeDouble TapeDoubleResult = 0;
CppAD::Independent(leftAD);
boost::timer timer;
ADResult = dotProduct<CppAD::AD<double>>(leftAD, rightAD);
CppAD::ADFun<double> adfun(leftAD, std::vector<CppAD::AD<double>>({ ADResult }));
double ADtime = timer.elapsed();
cl::Independent(leftTapeDouble);
timer.restart();
TapeDoubleResult = dotProduct<cl::TapeDouble >(leftTapeDouble, rightTapeDouble);
cl::TapeFunction<double> fcl(leftTapeDouble, std::vector<cl::TapeDouble>({ TapeDoubleResult }));
double CppTime = timer.elapsed();
std::cout << "\tTime for AD<double> " << ADtime << " s" << std::endl;
std::cout << "\tTime for TapeDouble " << CppTime << " s" << std::endl;
std::cout << "\tThe relative difference " << 1.0 * std::abs(CppTime - ADtime) / std::max(CppTime, ADtime)
<< std::endl ;
}
void ForceControlLaw::updateHook() {
port_generator_active_.write(true);
// current wrench determination
geometry_msgs::Wrench current_end_effector_wrench;
port_current_end_effector_wrench_.read(current_end_effector_wrench);
KDL::Wrench input_force;
tf::wrenchMsgToKDL(current_end_effector_wrench, input_force);
force_control_msgs::ForceControl fcl_param;
port_current_fcl_param_.read(fcl_param);
KDL::Twist target_vel;
target_vel.vel[0] = fcl(fcl_param.reciprocaldamping.translation.x,
fcl_param.inertia.translation.x,
input_force.force.x(), fcl_param.wrench.force.x,
fcl_param.twist.linear.x, p_vel_.vel[0]);
target_vel.vel[1] = fcl(fcl_param.reciprocaldamping.translation.y,
fcl_param.inertia.translation.y,
input_force.force.y(), fcl_param.wrench.force.y,
fcl_param.twist.linear.y, p_vel_.vel[1]);
target_vel.vel[2] = fcl(fcl_param.reciprocaldamping.translation.z,
fcl_param.inertia.translation.z,
input_force.force.z(), fcl_param.wrench.force.z,
fcl_param.twist.linear.z, p_vel_.vel[2]);
target_vel.rot[0] = fcl(fcl_param.reciprocaldamping.rotation.x,
fcl_param.inertia.rotation.x, input_force.torque.x(),
fcl_param.wrench.torque.x, fcl_param.twist.angular.x,
p_vel_.rot[0]);
target_vel.rot[1] = fcl(fcl_param.reciprocaldamping.rotation.y,
fcl_param.inertia.rotation.y, input_force.torque.y(),
fcl_param.wrench.torque.y, fcl_param.twist.angular.y,
p_vel_.rot[1]);
target_vel.rot[2] = fcl(fcl_param.reciprocaldamping.rotation.z,
fcl_param.inertia.rotation.z, input_force.torque.z(),
fcl_param.wrench.torque.z, fcl_param.twist.angular.z,
p_vel_.rot[2]);
p_vel_ = target_vel;
target_vel = cl_ef_pose_kdl_.M * target_vel;
cl_ef_pose_kdl_ = KDL::addDelta(cl_ef_pose_kdl_, target_vel, step_duration_);
geometry_msgs::Pose cl_ef_pose;
tf::poseKDLToMsg(cl_ef_pose_kdl_, cl_ef_pose);
port_output_end_effector_pose_.write(cl_ef_pose);
}
// Calculates dot product of two AD<double> vectors and two tape_double vectors.
// Vectors are declared as independent.
void performanceTestWithTape()
{
std::ofstream out("..\\..\\..\\..\\..\\..\\..\\output\\en-us\\Tape\\CppAD\\Tests\\DotProductTest\\Log.csv",
std::ofstream::out | std::ofstream::app);
out << std::endl << "Test performance for dot product calculation with tape recording" << std::endl;
out << "Dot product of two vectors (size " << dimension << " ) is being calculated" << std::endl;
std::vector<CppAD::AD<double>> leftAD = vectorCast<CppAD::AD<double>>(leftVector);
std::vector<CppAD::AD<double>> rightAD = vectorCast<CppAD::AD<double>>(rightVector);
std::vector<cl::tape_double> lefttape_double = vectorCast<cl::tape_double>(leftVector);
std::vector<cl::tape_double> righttape_double = vectorCast<cl::tape_double>(rightVector);
CppAD::AD<double> ADResult = 0;
cl::tape_double tape_doubleResult = 0;
CppAD::Independent(leftAD);
out << "Start of calculating: " << currentTime() << std::endl;
boost::timer timer;
ADResult = dotProduct<CppAD::AD<double>>(leftAD, rightAD);
CppAD::ADFun<double> adfun(leftAD, std::vector<CppAD::AD<double>>({ ADResult }));
double ADtime = timer.elapsed();
cl::Independent(lefttape_double);
timer.restart();
tape_doubleResult = dotProduct<cl::tape_double >(lefttape_double, righttape_double);
cl::tape_function<double> fcl(lefttape_double, std::vector<cl::tape_double>({ tape_doubleResult }));
double CppTime = timer.elapsed();
out << "End of calculating" << std::endl;
out << "\tTime for AD<double> " << ADtime << " s" << std::endl;
out << "\tTime for tape_double " << CppTime << " s" << std::endl;
out << "\tThe relative difference " << 1.0 * std::abs(CppTime - ADtime) / std::max(CppTime, ADtime)
<< std::endl ;
}
