diff --git a/bitbots_wolfgang/wolfgang_webots_sim/launch/imu_filter_sim.launch b/bitbots_wolfgang/wolfgang_webots_sim/launch/imu_filter_sim.launch
index 01a2639e3..557387239 100644
--- a/bitbots_wolfgang/wolfgang_webots_sim/launch/imu_filter_sim.launch
+++ b/bitbots_wolfgang/wolfgang_webots_sim/launch/imu_filter_sim.launch
@@ -4,13 +4,16 @@
+
+ name="complementary_filter_gain_node_$(var ns)" output="screen">
+
+
diff --git a/bitbots_wolfgang/wolfgang_webots_sim/launch/simulation.launch b/bitbots_wolfgang/wolfgang_webots_sim/launch/simulation.launch
index b9511ea3f..972b24535 100644
--- a/bitbots_wolfgang/wolfgang_webots_sim/launch/simulation.launch
+++ b/bitbots_wolfgang/wolfgang_webots_sim/launch/simulation.launch
@@ -34,6 +34,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/bitbots_wolfgang/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto b/bitbots_wolfgang/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto
index 53ad73c84..077209048 100644
--- a/bitbots_wolfgang/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto
+++ b/bitbots_wolfgang/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto
@@ -1906,6 +1906,68 @@ PROTO Wolfgang [
translation -0.069100 -0.000000 -0.026000
rotation 0.572979 0.572976 -0.585998 2.081475
children [
+ Accelerometer {
+ name "imu_r_foot accelerometer"
+ lookupTable [
+ -156.960000 -156.960000 0.000210,
+ -78.480000 -78.480000 0.000419,
+ -39.240000 -39.240000 0.000839,
+ -19.620000 -19.620000 0.001677,
+ -9.810000 -9.810000 0.003354,
+ -4.905000 -4.905000 0.006708,
+ -2.452500 -2.452500 0.013416,
+ -1.226250 -1.226250 0.026833,
+ -0.613125 -0.613125 0.053666,
+ -0.306563 -0.306563 0.107331,
+ -0.153281 -0.153281 0.214663,
+ -0.076641 -0.076641 0.429325,
+ 0 0 0
+ 0.076641 0.076641 0.429325,
+ 0.153281 0.153281 0.214663,
+ 0.306563 0.306563 0.107331,
+ 0.613125 0.613125 0.053666,
+ 1.226250 1.226250 0.026833,
+ 2.452500 2.452500 0.013416,
+ 4.905000 4.905000 0.006708,
+ 9.810000 9.810000 0.003354,
+ 19.620000 19.620000 0.001677,
+ 39.240000 39.240000 0.000839,
+ 78.480000 78.480000 0.000419,
+ 156.960000 156.960000 0.000210,
+ ]
+ resolution 0.00479003906
+ }
+ Gyro {
+ name "imu_r_foot gyro"
+ lookupTable [
+ -34.906590 -34.906590 0.000031,
+ -17.453295 -17.453295 0.000061,
+ -8.726648 -8.726648 0.000122,
+ -4.363324 -4.363324 0.000244,
+ -2.181662 -2.181662 0.000488,
+ -1.090831 -1.090831 0.000977,
+ -0.545415 -0.545415 0.001953,
+ -0.272708 -0.272708 0.003906,
+ -0.136354 -0.136354 0.007812,
+ -0.068177 -0.068177 0.015625,
+ -0.034088 -0.034088 0.031250,
+ -0.017044 -0.017044 0.062500,
+ 0 0 0
+ 0.017044 0.017044 0.062500,
+ 0.034088 0.034088 0.031250,
+ 0.068177 0.068177 0.015625,
+ 0.136354 0.136354 0.007812,
+ 0.272708 0.272708 0.003906,
+ 0.545415 0.545415 0.001953,
+ 1.090831 1.090831 0.000977,
+ 2.181662 2.181662 0.000488,
+ 4.363324 4.363324 0.000244,
+ 8.726648 8.726648 0.000122,
+ 17.453295 17.453295 0.000061,
+ 34.906590 34.906590 0.000031,
+ ]
+ resolution 0.00106526458
+ }
Transform {
translation -0.030000 0.006500 -0.034000
rotation 0.000000 1.000000 0.000000 0.000000
@@ -3326,6 +3388,68 @@ PROTO Wolfgang [
translation 0.069100 0.000000 -0.026000
rotation 0.577350 -0.577350 0.577352 2.094399
children [
+ Accelerometer {
+ name "imu_l_foot accelerometer"
+ lookupTable [
+ -156.960000 -156.960000 0.000210,
+ -78.480000 -78.480000 0.000419,
+ -39.240000 -39.240000 0.000839,
+ -19.620000 -19.620000 0.001677,
+ -9.810000 -9.810000 0.003354,
+ -4.905000 -4.905000 0.006708,
+ -2.452500 -2.452500 0.013416,
+ -1.226250 -1.226250 0.026833,
+ -0.613125 -0.613125 0.053666,
+ -0.306563 -0.306563 0.107331,
+ -0.153281 -0.153281 0.214663,
+ -0.076641 -0.076641 0.429325,
+ 0 0 0
+ 0.076641 0.076641 0.429325,
+ 0.153281 0.153281 0.214663,
+ 0.306563 0.306563 0.107331,
+ 0.613125 0.613125 0.053666,
+ 1.226250 1.226250 0.026833,
+ 2.452500 2.452500 0.013416,
+ 4.905000 4.905000 0.006708,
+ 9.810000 9.810000 0.003354,
+ 19.620000 19.620000 0.001677,
+ 39.240000 39.240000 0.000839,
+ 78.480000 78.480000 0.000419,
+ 156.960000 156.960000 0.000210,
+ ]
+ resolution 0.00479003906
+ }
+ Gyro {
+ name "imu_l_foot gyro"
+ lookupTable [
+ -34.906590 -34.906590 0.000031,
+ -17.453295 -17.453295 0.000061,
+ -8.726648 -8.726648 0.000122,
+ -4.363324 -4.363324 0.000244,
+ -2.181662 -2.181662 0.000488,
+ -1.090831 -1.090831 0.000977,
+ -0.545415 -0.545415 0.001953,
+ -0.272708 -0.272708 0.003906,
+ -0.136354 -0.136354 0.007812,
+ -0.068177 -0.068177 0.015625,
+ -0.034088 -0.034088 0.031250,
+ -0.017044 -0.017044 0.062500,
+ 0 0 0
+ 0.017044 0.017044 0.062500,
+ 0.034088 0.034088 0.031250,
+ 0.068177 0.068177 0.015625,
+ 0.136354 0.136354 0.007812,
+ 0.272708 0.272708 0.003906,
+ 0.545415 0.545415 0.001953,
+ 1.090831 1.090831 0.000977,
+ 2.181662 2.181662 0.000488,
+ 4.363324 4.363324 0.000244,
+ 8.726648 8.726648 0.000122,
+ 17.453295 17.453295 0.000061,
+ 34.906590 34.906590 0.000031,
+ ]
+ resolution 0.00106526458
+ }
Transform {
translation -0.060000 0.163500 -0.034000
rotation 0.000000 1.000000 0.000000 0.000000
diff --git a/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py b/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py
index ee7f06eaa..eb202840e 100644
--- a/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py
+++ b/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py
@@ -611,6 +611,16 @@ def __init__(
self.accel_head.enable(self.timestep)
self.gyro_head = self.robot_node.getDevice("imu_head gyro")
self.gyro_head.enable(self.timestep)
+
+ self.accel_l_foot = self.robot_node.getDevice("imu_l_foot accelerometer")
+ self.accel_l_foot.enable(self.timestep)
+ self.gyro_l_foot = self.robot_node.getDevice("imu_l_foot gyro")
+ self.gyro_l_foot.enable(self.timestep)
+
+ self.accel_r_foot = self.robot_node.getDevice("imu_r_foot accelerometer")
+ self.accel_r_foot.enable(self.timestep)
+ self.gyro_r_foot = self.robot_node.getDevice("imu_r_foot gyro")
+ self.gyro_r_foot.enable(self.timestep)
self.camera = self.robot_node.getDevice(camera_name)
self.camera_counter = 0
if self.camera_active:
@@ -638,9 +648,13 @@ def __init__(
)
self.head_imu_frame = self.ros_node.get_parameter("head_imu_frame").get_parameter_value().string_value
self.pub_js = self.ros_node.create_publisher(JointState, base_ns + "joint_states", 1)
+
self.pub_imu = self.ros_node.create_publisher(Imu, base_ns + "imu/data_raw", 1)
- self.pub_imu_head = self.ros_node.create_publisher(Imu, base_ns + "imu_head/data", 1)
+ self.pub_imu_head = self.ros_node.create_publisher(Imu, base_ns + "imu_head/data_raw", 1)
+ self.pub_imu_l_foot = self.ros_node.create_publisher(Imu, base_ns + "imu_l_foot/data_raw", 1)
+ self.pub_imu_r_foot = self.ros_node.create_publisher(Imu, base_ns + "imu_r_foot/data_raw", 1)
+
self.pub_cam = self.ros_node.create_publisher(Image, base_ns + "camera/image_proc", 1)
self.pub_cam_info = self.ros_node.create_publisher(CameraInfo, base_ns + "camera/camera_info", 1)
@@ -785,22 +799,31 @@ def get_joint_state_msg(self):
def publish_joint_states(self):
self.pub_js.publish(self.get_joint_state_msg())
- def get_imu_msg(self, head=False):
+ def get_imu_msg(self, which="torso"):
msg = Imu()
msg.header.stamp = Time(seconds=int(self.time), nanoseconds=self.time % 1 * 1e9).to_msg()
- if head:
- msg.header.frame_id = self.head_imu_frame
- else:
+ if which == "torso":
msg.header.frame_id = self.imu_frame
-
- # change order because webots has different axis
- if head:
+ accel_vels = self.accel.getValues()
+ msg.linear_acceleration.x = accel_vels[0]
+ msg.linear_acceleration.y = accel_vels[1]
+ msg.linear_acceleration.z = accel_vels[2]
+ elif which == "head":
+ # change order because rotated frames in webots are weired
+ msg.header.frame_id = self.head_imu_frame
accel_vels = self.accel_head.getValues()
msg.linear_acceleration.x = accel_vels[2]
msg.linear_acceleration.y = -accel_vels[0]
msg.linear_acceleration.z = -accel_vels[1]
- else:
- accel_vels = self.accel.getValues()
+ elif which == "l_foot":
+ msg.header.frame_id = self.l_sole_frame
+ accel_vels = self.accel_l_foot.getValues()
+ msg.linear_acceleration.x = accel_vels[0]
+ msg.linear_acceleration.y = accel_vels[1]
+ msg.linear_acceleration.z = accel_vels[2]
+ elif which == "r_foot":
+ msg.header.frame_id = self.r_sole_frame
+ accel_vels = self.accel_r_foot.getValues()
msg.linear_acceleration.x = accel_vels[0]
msg.linear_acceleration.y = accel_vels[1]
msg.linear_acceleration.z = accel_vels[2]
@@ -810,12 +833,7 @@ def get_imu_msg(self, head=False):
if msg.linear_acceleration.x == 0 and msg.linear_acceleration.y == 0 and msg.linear_acceleration.z == 0:
msg.linear_acceleration.z = 0.001
- if head:
- gyro_vels = self.gyro_head.getValues()
- msg.angular_velocity.x = gyro_vels[2]
- msg.angular_velocity.y = -gyro_vels[0]
- msg.angular_velocity.z = -gyro_vels[1]
- else:
+ if which == "torso":
gyro_vels = self.gyro.getValues()
msg.angular_velocity.x = gyro_vels[0]
msg.angular_velocity.y = gyro_vels[1]
@@ -823,12 +841,30 @@ def get_imu_msg(self, head=False):
msg.angular_velocity.z = gyro_vels[2]
else:
msg.angular_velocity.z = 0.0
+ elif which == "head":
+ gyro_vels = self.gyro_head.getValues()
+ msg.angular_velocity.x = gyro_vels[2]
+ msg.angular_velocity.y = -gyro_vels[0]
+ msg.angular_velocity.z = -gyro_vels[1]
+ elif which == "l_foot":
+ gyro_vels = self.gyro_l_foot.getValues()
+ msg.angular_velocity.x = gyro_vels[0]
+ msg.angular_velocity.y = gyro_vels[1]
+ msg.angular_velocity.z = gyro_vels[2]
+ elif which == "r_foot":
+ gyro_vels = self.gyro_r_foot.getValues()
+ msg.angular_velocity.x = gyro_vels[0]
+ msg.angular_velocity.y = gyro_vels[1]
+ msg.angular_velocity.z = gyro_vels[2]
return msg
def publish_imu(self):
- self.pub_imu.publish(self.get_imu_msg(head=False))
+ self.pub_imu.publish(self.get_imu_msg())
if self.is_wolfgang:
- self.pub_imu_head.publish(self.get_imu_msg(head=True))
+ self.pub_imu_head.publish(self.get_imu_msg(which="head"))
+ self.pub_imu_l_foot.publish(self.get_imu_msg(which="l_foot"))
+ self.pub_imu_r_foot.publish(self.get_imu_msg(which="r_foot"))
+
def publish_camera(self):
img_msg = Image()
diff --git a/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py b/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py
index 40211b63d..862b2b4b7 100644
--- a/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py
+++ b/bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py
@@ -7,6 +7,7 @@
from rclpy.time import Time
from rosgraph_msgs.msg import Clock
from std_srvs.srv import Empty
+from bitbots_msgs.msg import SimInfo
from bitbots_msgs.srv import SetObjectPose, SetObjectPosition
@@ -105,7 +106,7 @@ def __init__(
clock_topic = base_ns + "clock"
model_topic = base_ns + "model_states"
self.clock_publisher = self.ros_node.create_publisher(Clock, clock_topic, 1)
- self.model_state_publisher = self.ros_node.create_publisher(ModelStates, model_topic, 1)
+ self.model_state_publisher = self.ros_node.create_publisher(SimInfo, model_topic, 1)
self.reset_service = self.ros_node.create_service(Empty, base_ns + "reset", self.reset)
self.reset_pose_service = self.ros_node.create_service(
Empty, base_ns + "reset_pose", self.set_initial_poses
@@ -337,7 +338,7 @@ def get_link_velocities(self, link, name="amy"):
def publish_model_states(self):
if self.model_state_publisher.get_subscription_count() != 0:
- msg = ModelStates()
+ msg = SimInfo()
for robot_name, robot_node in self.robot_nodes.items():
position, orientation = self.get_robot_pose_quat(name=robot_name)
robot_pose = Pose()
@@ -355,7 +356,7 @@ def publish_model_states(self):
twist.angular.x = ang_vel[0]
twist.angular.y = ang_vel[1]
twist.angular.z = ang_vel[2]
- msg.twist.append(twist)
+ msg.base_link_twist = twist
head_node = robot_node.getFromProtoDef("head")
head_position = head_node.getPosition()
@@ -363,21 +364,45 @@ def publish_model_states(self):
head_orientation_quat = transforms3d.quaternions.mat2quat(np.reshape(head_orientation, (3, 3)))
head_pose = Pose()
head_pose.position = Point(x=head_position[0], y=head_position[1], z=head_position[2])
- head_pose.orientation = Quaternion(
- x=head_orientation_quat[1],
- y=head_orientation_quat[2],
- z=head_orientation_quat[3],
- w=head_orientation_quat[0],
- )
- msg.name.append(robot_name + "_head")
- msg.pose.append(head_pose)
-
- if self.ball is not None:
- ball_position = self.ball.getField("translation").getSFVec3f()
- ball_pose = Pose()
- ball_pose.position = Point(x=ball_position[0], y=ball_position[1], z=ball_position[2])
- ball_pose.orientation = Quaternion()
- msg.name.append("ball")
- msg.pose.append(ball_pose)
+ head_pose.orientation = Quaternion(x=head_orientation_quat[1], y=head_orientation_quat[2],
+ z=head_orientation_quat[3], w=head_orientation_quat[0])
+ # I currently do not need the head
+ # msg.name.append(robot_name + "_head")
+ # msg.pose.append(head_pose)
+
+ l_foot_node = robot_node.getFromProtoDef("l_foot")
+ l_foot_position = l_foot_node.getPosition()
+ l_foot_orientation = l_foot_node.getOrientation()
+ l_foot_orientation_quat = transforms3d.quaternions.mat2quat(np.reshape(l_foot_orientation, (3, 3)))
+ l_foot_pose = Pose()
+ l_foot_pose.position = Point(x=l_foot_position[0], y=l_foot_position[1], z=l_foot_position[2])
+ l_foot_pose.orientation = Quaternion(x=l_foot_orientation_quat[1], y=l_foot_orientation_quat[2],
+ z=l_foot_orientation_quat[3], w=l_foot_orientation_quat[0])
+ msg.l_foot_pose = l_foot_pose
+
+ r_foot_node = robot_node.getFromProtoDef("r_foot")
+ r_foot_position = r_foot_node.getPosition()
+ r_foot_orientation = r_foot_node.getOrientation()
+ r_foot_orientation_quat = transforms3d.quaternions.mat2quat(np.reshape(r_foot_orientation, (3, 3)))
+ r_foot_pose = Pose()
+ r_foot_pose.position = Point(x=r_foot_position[0], y=r_foot_position[1], z=r_foot_position[2])
+ r_foot_pose.orientation = Quaternion(x=r_foot_orientation_quat[1], y=r_foot_orientation_quat[2],
+ z=r_foot_orientation_quat[3], w=r_foot_orientation_quat[0])
+
+ msg.r_foot_pose = r_foot_pose
+
+ seconds = int(self.time)
+ nanosec = int((self.time - seconds) * 1e9)
+ msg.header.stamp = Time(seconds=seconds, nanoseconds=nanosec).to_msg()
+
+
+
+ # if self.ball is not None:
+ # ball_position = self.ball.getField("translation").getSFVec3f()
+ # ball_pose = Pose()
+ # ball_pose.position = Point(x=ball_position[0], y=ball_position[1], z=ball_position[2])
+ # ball_pose.orientation = Quaternion()
+ # msg.name.append("ball")
+ # msg.pose.append(ball_pose)
self.model_state_publisher.publish(msg)