void calc_angular_momenta(BEMRI * b, double * l1h, double * l2h)
{
lb = angular_momentum(R1,R2,V1,V2,m1,m2); //angular momentum of BEMRI
lh = angular_momentum(R3,R4,V3,V4,m3,(m2+m1)); //CM-BH angular momentum
*l1h = angular_momentum(R3,R1,V3,V1,m3,m1); //m1-BH angular momentum
*l2h = angular_momentum(R3,R2,V3,V2,m3,m2); //m2-BH angular momentum
return;
}
int main(int argc, char **argv){
double* q_x = calloc(2, sizeof(double));
double* q_y = calloc(2, sizeof(double));
printf("Intial position in x : ");
scanf("%lf",&q_x[0]);
printf("Intial velocity in y : ");
scanf("%lf",&q_y[1]);
FILE *f = fopen("output.txt","w");
FILE *f_1 = fopen("out3.txt","w");
//q_x[0] = 1.0; // position
q_x[1] = 0.0; // momentum
q_y[0] = 0.0;
//q_y[1] = 1.0;
int n = 10000;
double h = (2*PI)/n;
double int_E = energy(sqrt(pow(q_x[0],2) + pow(q_y[0],2)),q_x[1],q_y[1]);
double int_L = angular_momentum(q_x[0],q_y[0],q_x[1],q_y[1]);
fprintf(f,"x \t y \t R \n");
fprintf(f_1,"%s \t %s \t %s \t %s \t %s\n","Time","E","L","Deviation in E","Deviation in L");
for (int i = 0;i<=n;i++){
double radius = sqrt(pow(q_x[0],2) + pow(q_y[0],2));
double theta = atan2(q_y[0],q_x[0]);
double E = energy(radius,q_x[1],q_y[1]);
double L = angular_momentum(q_x[0],q_y[0],q_x[1],q_y[1]);
fprintf(f,"%f \t %f \t %f\n",q_x[0] ,q_y[0],radius);
fprintf(f_1,"%.5e \t %.5e \t %.5e\t",i*h,E,L);
fprintf(f_1,"%.6e \t %.16e\n",fabs(E-int_E), fabs(L-int_L));
PERLF(q_x,force_x,h,radius);
PERLF(q_y,force_y,h,radius);
}
fclose(f);
return 0;
}
void NewVizManager::animateCenterOfMass(const ItompTrajectoryConstPtr& trajectory, std::vector<RigidBodyDynamics::Model>& models)
{
const double SCALE_FORCE_LINE = 0.005;
const double SCALE_FORCE = 0.001;
const double SCALE_SPHERE = 0.01;
visualization_msgs::MarkerArray ma;
visualization_msgs::Marker marker_com;
visualization_msgs::Marker marker_com_vel;
visualization_msgs::Marker marker_com_angular_momentum;
marker_com.header.frame_id = reference_frame_;
marker_com.header.stamp = ros::Time::now();
marker_com.action = visualization_msgs::Marker::ADD;
marker_com.pose.orientation.w = 1.0;
marker_com.type = visualization_msgs::Marker::SPHERE_LIST;
marker_com.scale.x = SCALE_SPHERE;
marker_com.scale.y = SCALE_SPHERE;
marker_com.scale.z = SCALE_SPHERE;
marker_com.color = colors_[YELLOW];
marker_com_vel = marker_com;
marker_com_vel.color = colors_[BLUE];
marker_com_vel.scale.x = SCALE_FORCE_LINE;
marker_com_vel.scale.y = SCALE_FORCE_LINE;
marker_com_vel.scale.z = SCALE_FORCE_LINE;
marker_com_angular_momentum = marker_com_vel;
marker_com_angular_momentum.color = colors_[GREEN];
marker_com_vel.ns = "com";
marker_com_vel.ns = "com_vel";
marker_com_angular_momentum.ns = "com_am";
marker_com.id = 0;
marker_com_vel.id = 1;
marker_com_angular_momentum.id = 2;
for (int point = 0; point < trajectory->getNumPoints(); ++point)
{
double mass;
RigidBodyDynamics::Math::Vector3d com;
RigidBodyDynamics::Math::Vector3d com_velocity;
RigidBodyDynamics::Math::Vector3d angular_momentum;
const Eigen::VectorXd& q = trajectory->getElementTrajectory(ItompTrajectory::COMPONENT_TYPE_POSITION,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT)->getTrajectoryPoint(point);
const Eigen::VectorXd& qdot = trajectory->getElementTrajectory(ItompTrajectory::COMPONENT_TYPE_VELOCITY,
ItompTrajectory::SUB_COMPONENT_TYPE_JOINT)->getTrajectoryPoint(point);
RigidBodyDynamics::Utils::CalcCenterOfMass(models[point], q, qdot, mass, com, &com_velocity, &angular_momentum, false);
geometry_msgs::Point position;
position.x = com(0);
position.y = com(1);
position.z = com(2);
marker_com.points.push_back(position);
marker_com_vel.points.push_back(position);
marker_com_angular_momentum.points.push_back(position);
position.x = com(0) + com_velocity(0) * SCALE_FORCE;
position.y = com(1) + com_velocity(1) * SCALE_FORCE;
position.z = com(2) + com_velocity(2) * SCALE_FORCE;
marker_com_vel.points.push_back(position);
position.x = com(0) + angular_momentum(0) * SCALE_FORCE;
position.y = com(1) + angular_momentum(1) * SCALE_FORCE;
position.z = com(2) + angular_momentum(2) * SCALE_FORCE;
marker_com_angular_momentum.points.push_back(position);
}
ma.markers.push_back(marker_com);
ma.markers.push_back(marker_com_vel);
ma.markers.push_back(marker_com_angular_momentum);
vis_marker_array_publisher_center_of_mass_.publish(ma);
}
