class DarwinController: def __init__(self, namespace='', node=True): self.time = 0 self.clock_msg = Clock() self.namespace = namespace self.supervisor = Supervisor() self.motor_names = [ "ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR", "FootL", "Neck", "Head" ] self.walkready = [0] * 20 self.names_webots_to_bitbots = { "ShoulderR": "RShoulderPitch", "ShoulderL": "LShoulderPitch", "ArmUpperR": "RShoulderRoll", "ArmUpperL": "LShoulderRoll", "ArmLowerR": "RElbow", "ArmLowerL": "LElbow", "PelvYR": "RHipYaw", "PelvYL": "LHipYaw", "PelvR": "RHipRoll", "PelvL": "LHipRoll", "LegUpperR": "RHipPitch", "LegUpperL": "LHipPitch", "LegLowerR": "RKnee", "LegLowerL": "LKnee", "AnkleR": "RAnklePitch", "AnkleL": "LAnklePitch", "FootR": "RAnkleRoll", "FootL": "LAnkleRoll", "Neck": "HeadPan", "Head": "HeadTilt" } self.names_bitbots_to_webots = { "RShoulderPitch": "ShoulderR", "LShoulderPitch": "ShoulderL", "RShoulderRoll": "ArmUpperR", "LShoulderRoll": "ArmUpperL", "RElbow": "ArmLowerR", "LElbow": "ArmLowerL", "RHipYaw": "PelvYR", "LHipYaw": "PelvYL", "RHipRoll": "PelvR", "LHipRoll": "PelvL", "RHipPitch": "LegUpperR", "LHipPitch": "LegUpperL", "RKnee": "LegLowerR", "LKnee": "LegLowerL", "RAnklePitch": "AnkleR", "LAnklePitch": "AnkleL", "RAnkleRoll": "FootR", "LAnkleRoll": "FootL", "HeadPan": "Neck", "HeadTilt": "Head" } self.motors = [] self.sensors = [] self.timestep = int(self.supervisor.getBasicTimeStep()) self.timestep = 10 for motor_name in self.motor_names: self.motors.append(self.supervisor.getMotor(motor_name)) self.motors[-1].enableTorqueFeedback(self.timestep) self.sensors.append( self.supervisor.getPositionSensor(motor_name + "S")) self.sensors[-1].enable(self.timestep) self.accel = self.supervisor.getAccelerometer("Accelerometer") self.accel.enable(self.timestep) self.gyro = self.supervisor.getGyro("Gyro") self.gyro.enable(self.timestep) self.camera = self.supervisor.getCamera("Camera") self.camera.enable(self.timestep) if node: rospy.init_node("webots_darwin_ros_interface", anonymous=True, argv=['clock:=/' + self.namespace + '/clock']) self.pub_js = rospy.Publisher(self.namespace + "/joint_states", JointState, queue_size=1) self.pub_imu = rospy.Publisher(self.namespace + "/imu/data", Imu, queue_size=1) self.pub_cam = rospy.Publisher(self.namespace + "/image_raw", Image, queue_size=1) self.clock_publisher = rospy.Publisher(self.namespace + "/clock", Clock, queue_size=1) rospy.Subscriber(self.namespace + "/DynamixelController/command", JointCommand, self.command_cb) self.world_info = self.supervisor.getFromDef("world_info") self.hinge_joint = self.supervisor.getFromDef("barrier_hinge") self.robot_node = self.supervisor.getFromDef("Darwin") self.translation_field = self.robot_node.getField("translation") self.rotation_field = self.robot_node.getField("rotation") def step_sim(self): self.supervisor.step(self.timestep) def step(self): self.step_sim() self.time += self.timestep / 1000 self.publish_imu() self.publish_joint_states() self.clock_msg.clock = rospy.Time.from_seconds(self.time) self.clock_publisher.publish(self.clock_msg) def command_cb(self, command: JointCommand): for i, name in enumerate(command.joint_names): try: motor_index = self.motor_names.index( self.names_bitbots_to_webots[name]) self.motors[motor_index].setPosition(command.positions[i]) except ValueError: print( f"invalid motor specified ({self.names_bitbots_to_webots[name]})" ) self.publish_joint_states() self.publish_imu() self.publish_camera() def publish_joint_states(self): js = JointState() js.name = [] js.header.stamp = rospy.get_rostime() js.position = [] js.effort = [] for i in range(len(self.sensors)): js.name.append(self.names_webots_to_bitbots[self.motor_names[i]]) value = self.sensors[i].getValue() js.position.append(value) js.effort.append(self.motors[i].getTorqueFeedback()) self.pub_js.publish(js) def publish_imu(self): msg = Imu() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = "imu_frame" accel_vels = self.accel.getValues() msg.linear_acceleration.x = ((accel_vels[0] - 512.0) / 512.0) * 3 * G msg.linear_acceleration.y = ((accel_vels[1] - 512.0) / 512.0) * 3 * G msg.linear_acceleration.z = ((accel_vels[2] - 512.0) / 512.0) * 3 * G gyro_vels = self.gyro.getValues() msg.angular_velocity.x = ((gyro_vels[0] - 512.0) / 512.0) * 1600 * ( math.pi / 180) # is 400 deg/s the real value msg.angular_velocity.y = ( (gyro_vels[1] - 512.0) / 512.0) * 1600 * (math.pi / 180) msg.angular_velocity.z = ( (gyro_vels[2] - 512.0) / 512.0) * 1600 * (math.pi / 180) self.pub_imu.publish(msg) def publish_camera(self): img_msg = Image() img_msg.header.stamp = rospy.get_rostime() img_msg.height = self.camera.getHeight() img_msg.width = self.camera.getWidth() img_msg.encoding = "bgra8" img_msg.step = 4 * self.camera.getWidth() img = self.camera.getImage() img_msg.data = img self.pub_cam.publish(img_msg) def set_gravity(self, active): if active: self.world_info.getField("gravity").setSFVec3f([0.0, -9.81, 0.0]) else: self.world_info.getField("gravity").setSFVec3f([0.0, 0.0, 0.0]) def reset_robot_pose(self, pos, quat): rpy = tf.transformations.euler_from_quaternion(quat) self.set_robot_pose_rpy(pos, rpy) self.robot_node.resetPhysics() def reset_robot_pose_rpy(self, pos, rpy): self.set_robot_pose_rpy(pos, rpy) self.robot_node.resetPhysics() def reset(self): self.supervisor.simulationReset() def node(self): s = self.supervisor.getSelected() if s is not None: print(f"id: {s.getId()}, type: {s.getType()}, def: {s.getDef()}") def set_robot_pose_rpy(self, pos, rpy): self.translation_field.setSFVec3f(pos_ros_to_webots(pos)) self.rotation_field.setSFRotation(rpy_to_axis(*rpy)) def get_robot_pose_rpy(self): pos = self.translation_field.getSFVec3f() rot = self.rotation_field.getSFRotation() return pos_webots_to_ros(pos), axis_to_rpy(*rot)