| 132 | } |
| 133 | |
| 134 | prometheus_msgs::PositionReference Controller_Test::Eight_trajectory_generation(float time_from_start) |
| 135 | { |
| 136 | Eigen::Vector3f position; |
| 137 | Eigen::Vector3f velocity; |
| 138 | Eigen::Vector3f acceleration; |
| 139 | |
| 140 | float angle = eight_omega_* time_from_start; |
| 141 | const float cos_angle = cos(angle); |
| 142 | const float sin_angle = sin(angle); |
| 143 | |
| 144 | Eigen::Vector3f eight_radial_ ; |
| 145 | Eigen::Vector3f eight_axis_ ; |
| 146 | eight_radial_ << radial, 0.0, 0.0; |
| 147 | eight_axis_ << 0.0, 0.0, 2.0; |
| 148 | |
| 149 | position = cos_angle * eight_radial_ + sin_angle * cos_angle * eight_axis_.cross(eight_radial_) |
| 150 | + (1 - cos_angle) * eight_axis_.dot(eight_radial_) * eight_axis_ + eight_origin_; |
| 151 | |
| 152 | velocity = eight_omega_ * (-sin_angle * eight_radial_ + (pow(cos_angle, 2) - pow(sin_angle, 2)) * eight_axis_.cross(eight_radial_) |
| 153 | + (sin_angle) * eight_axis_.dot(eight_radial_) * eight_axis_); |
| 154 | |
| 155 | acceleration << 0.0, 0.0, 0.0; |
| 156 | |
| 157 | prometheus_msgs::PositionReference Eight_trajectory; |
| 158 | |
| 159 | Eight_trajectory.header.stamp = ros::Time::now(); |
| 160 | |
| 161 | Eight_trajectory.time_from_start = time_from_start; |
| 162 | |
| 163 | Eight_trajectory.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY; |
| 164 | |
| 165 | Eight_trajectory.position_ref[0] = position[0]; |
| 166 | Eight_trajectory.position_ref[1] = position[1]; |
| 167 | Eight_trajectory.position_ref[2] = position[2]; |
| 168 | |
| 169 | Eight_trajectory.velocity_ref[0] = velocity[0]; |
| 170 | Eight_trajectory.velocity_ref[1] = velocity[1]; |
| 171 | Eight_trajectory.velocity_ref[2] = velocity[2]; |
| 172 | |
| 173 | Eight_trajectory.acceleration_ref[0] = 0; |
| 174 | Eight_trajectory.acceleration_ref[1] = 0; |
| 175 | Eight_trajectory.acceleration_ref[2] = 0; |
| 176 | |
| 177 | Eight_trajectory.yaw_ref = 0; |
| 178 | |
| 179 | // to be continued... |
| 180 | |
| 181 | return Eight_trajectory; |
| 182 | } |
| 183 | |
| 184 | |
| 185 | prometheus_msgs::PositionReference Controller_Test::Step_trajectory_generation(float time_from_start) |