(client,dyaw=0.0)
| 37 | return [qx, qy, qz, qw] |
| 38 | |
| 39 | def rotateMultirotor(client,dyaw=0.0): |
| 40 | # init multirotor pose |
| 41 | pose = client.simGetVehiclePose() |
| 42 | roll, pitch, yaw = euler_from_quaternion(pose.orientation) |
| 43 | print('initial pose_quaternion:{}'.format(pose)) |
| 44 | print('initial pose_euler:{} {} {}'.format(roll, pitch, yaw)) |
| 45 | yaw += dyaw |
| 46 | qx, qy, qz, qw = euler_to_quaternion(yaw, pitch, roll) |
| 47 | pose.orientation.w_val = qw |
| 48 | pose.orientation.z_val = qz |
| 49 | pose.orientation.y_val = qy |
| 50 | pose.orientation.x_val = qx |
| 51 | client.simSetVehiclePose(pose, ignore_collison=True) |
| 52 | print('changed pose_quaternion:{}'.format(pose)) |
| 53 | print('changed pose_euler:{} {} {}'.format(roll, pitch, yaw)) |
nothing calls this directly
no test coverage detected