def test_ypr_to_quaternion_proto(yrp, wxyz):
euler_zxy = geometry.EulerZXY(yaw=yrp[0], roll=yrp[1], pitch=yrp[2])
quat_initial = Quaternion(w=wxyz[0], x=wxyz[1], y=wxyz[2], z=wxyz[3])
quat = euler_zxy.to_quaternion()
assert (_is_near(abs(_dot(quat, quat_initial)), 1.0, 1e-6))
# Do another circular conversion, make sure everything looks ok.
euler_zxy_final = quat.to_euler_zxy()
quat_final = euler_zxy_final.to_quaternion()
assert (_is_near(abs(_dot(quat_final, quat_initial)), 1.0, 1e-6))
def bodyPoseCallback(self, data):
"""Callback for cmd_vel command"""
q = Quaternion()
q.x = data.orientation.x
q.y = data.orientation.y
q.z = data.orientation.z
q.w = data.orientation.w
euler_zxy = q.to_euler_zxy()
self.spot_wrapper.set_mobility_params(data.position.z, euler_zxy)
def to_quaternion(self):
cy = math.cos(0.5 * self.yaw)
cr = math.cos(0.5 * self.roll)
cp = math.cos(0.5 * self.pitch)
sy = math.sin(0.5 * self.yaw)
sr = math.sin(0.5 * self.roll)
sp = math.sin(0.5 * self.pitch)
w = cp * cr * cy - sp * sr * sy
x = cp * cy * sr - sp * cr * sy
y = cp * sr * sy + cr * cy * sp
z = cp * cr * sy + sp * cy * sr
return Quaternion(w=w, x=x, y=y, z=z)
def bodyPoseCallback(self, data):
"""Callback for cmd_vel command"""
q = Quaternion()
q.x = data.orientation.x
q.y = data.orientation.y
q.z = data.orientation.z
q.w = data.orientation.w
position = geometry_pb2.Vec3(z=data.position.z)
pose = geometry_pb2.SE3Pose(position=position, rotation=q)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.body_control.CopyFrom(body_control)
self.spot_wrapper.set_mobility_params(mobility_params)
