Skip to content

Commit

Permalink
Merge remote-tracking branch 'old_wolfgang_robot_repo/feature/use_sim…
Browse files Browse the repository at this point in the history 
…info_instead_of_model_states_msg' into odometry_data_generation_setup
  • Loading branch information
@val-ba
val-ba committed Feb 6, 2024
2 parents 7cf2c55 + 39c2ab8 commit 5da7923
Show file tree
Hide file tree
Showing 5 changed files with 237 additions and 38 deletions.
5 changes: 4 additions & 1 deletion bitbots_wolfgang/wolfgang_webots_sim/launch/imu_filter_sim.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,16 @@
<arg name="bias_alpha" default="0.05"/>
<arg name="do_adaptive_gain" default="false"/>
<arg name="gain_acc" default="0.01"/>
<arg name="ns" default="imu"/>

<node pkg="imu_complementary_filter" exec="complementary_filter_node"
name="complementary_filter_gain_node" output="screen">
name="complementary_filter_gain_node_$(var ns)" output="screen">
<param name="use_sim_time" value="$(var sim)"/>
<param name="do_bias_estimation" value="$(var do_bias_estimation)"/>
<param name="bias_alpha" value="$(var bias_alpha)"/>
<param name="do_adaptive_gain" value="$(var do_adaptive_gain)"/>
<param name="gain_acc" value="$(var gain_acc)"/>
<remap from="imu/data_raw" to="$(var ns)/data_raw"/>
<remap from="imu/data" to="$(var ns)/data" />
</node>
</launch>
11 changes: 11 additions & 0 deletions bitbots_wolfgang/wolfgang_webots_sim/launch/simulation.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,17 @@
<arg name="robot_type" value="$(var robot_type)"/>
</include>
<include file="$(find-pkg-share wolfgang_webots_sim)/launch/imu_filter_sim.launch"/>
<group if="$(eval '\'$(var robot_type)\' == \'wolfgang\' ')">
<include file="$(find-pkg-share wolfgang_webots_sim)/launch/imu_filter_sim.launch">
<arg name="ns" value="imu_head"/>
</include>
<include file="$(find-pkg-share wolfgang_webots_sim)/launch/imu_filter_sim.launch">
<arg name="ns" value="imu_l_foot"/>
</include>
<include file="$(find-pkg-share wolfgang_webots_sim)/launch/imu_filter_sim.launch">
<arg name="ns" value="imu_r_foot"/>
</include>
</group>
</group>

<!-- case for multiple robots -->
Expand Down
124 changes: 124 additions & 0 deletions bitbots_wolfgang/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
72 changes: 54 additions & 18 deletions bitbots_wolfgang/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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]
Expand All @@ -810,25 +833,38 @@ 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]
if not math.isnan(gyro_vels[2]): # nao robot reports nan for yaw
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()
Expand Down
Loading

0 comments on commit 5da7923

Please sign in to comment.