class DarwinWebotsController: def __init__(self, namespace='', ros_active=False, ros_anonymous=True, mode='normal'): self.ros_active = ros_active 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 = { v: k for k, v in self.names_webots_to_bitbots.items() } self.motors = [] self.sensors = [] self.timestep = int(self.supervisor.getBasicTimeStep()) # self.timestep = 10 if mode == 'normal': self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) elif mode == 'paused': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE) elif mode == 'run': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_RUN) elif mode == 'fast': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST) else: self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) for motor_name in self.motor_names: self.motors.append(self.supervisor.getDevice(motor_name)) self.motors[-1].enableTorqueFeedback(self.timestep) self.sensors.append(self.supervisor.getDevice(motor_name + "S")) self.sensors[-1].enable(self.timestep) self.accel = self.supervisor.getDevice("Accelerometer") self.accel.enable(self.timestep) self.gyro = self.supervisor.getDevice("Gyro") self.gyro.enable(self.timestep) self.camera = self.supervisor.getDevice("Camera") self.camera.enable(self.timestep) if self.ros_active: rospy.init_node("webots_darwin_ros_interface", anonymous=ros_anonymous, 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.pub_clock = 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") @property def ros_time(self) -> rospy.Time: return rospy.Time.from_seconds(self.time) def step_sim(self): self.time += self.timestep / 1000 self.supervisor.step(self.timestep) def step(self): self.step_sim() if self.ros_active: self.publish_imu() self.publish_joint_states() self.publish_clock() self.publish_camera() def publish_clock(self): self.clock_msg.clock = self.ros_time self.pub_clock.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: rospy.logerr( f"invalid motor specified ({self.names_bitbots_to_webots[name]})" ) def set_head_tilt(self, pos): self.motors[-1].setPosition(pos) def set_arms_zero(self): positions = [ -0.8399999308200574, 0.7200000596634105, -0.3299999109923385, 0.35999992683575216, 0.5099999812500172, -0.5199999789619728 ] for i in range(0, 6): self.motors[i].setPosition(positions[i]) def publish_joint_states(self): msg = JointState() msg.name = [] msg.header.stamp = self.ros_time msg.position = [] msg.effort = [] for i in range(len(self.sensors)): msg.name.append(self.names_webots_to_bitbots[self.motor_names[i]]) value = self.sensors[i].getValue() msg.position.append(value) msg.effort.append(self.motors[i].getTorqueFeedback()) self.pub_js.publish(msg) def publish_imu(self): msg = Imu() msg.header.stamp = self.ros_time 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) # todo compute msg.orientation.x = 0 msg.orientation.y = 0 msg.orientation.z = 0 msg.orientation.w = 1 self.pub_imu.publish(msg) def publish_camera(self): msg = Image() msg.header.stamp = self.ros_time msg.header.frame_id = "MP_PMDCAMBOARD" msg.height = self.camera.getHeight() msg.width = self.camera.getWidth() msg.encoding = "bgra8" msg.step = 4 * self.camera.getWidth() img = self.camera.getImage() msg.data = img self.pub_cam.publish(msg) def set_gravity(self, active): if active: self.world_info.getField("gravity").setSFVec3f([0.0, -9.81, 0.0]) self.world_info.getField("gravity").setSFFloat(9.81) else: self.world_info.getField("gravity").setSFVec3f([0.0, 0.0, 0.0]) self.world_info.getField("gravity").setSFFloat(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() # reactivate camera after reset https://github.com/cyberbotics/webots/issues/1778 # self.supervisor.getSelf().restartController() # self.camera = self.supervisor.getCamera("Camera") # self.camera.enable(self.timestep) self.supervisor.simulationResetPhysics() 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 set_robot_rpy(self, rpy): 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)
class SupervisorController: def __init__(self, ros_active=False, mode='normal'): # requires WEBOTS_ROBOT_NAME to be set to "amy" or "rory" self.ros_active = ros_active self.time = 0 self.clock_msg = Clock() self.supervisor = Supervisor() self.amy_node = self.supervisor.getFromDef("amy") self.rory_node = self.supervisor.getFromDef("rory") self.jack_node = self.supervisor.getFromDef("jack") self.donna_node = self.supervisor.getFromDef("donna") self.melody_node = self.supervisor.getFromDef("melody") if mode == 'normal': self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) elif mode == 'paused': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE) elif mode == 'fast': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST) else: self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) self.motors = [] self.sensors = [] self.timestep = int(self.supervisor.getBasicTimeStep()) # resolve the node for corresponding name self.robot_names = ["amy", "rory", "jack", "donna", "melody"] self.robot_nodes = {} self.translation_fields = {} self.rotation_fields = {} # check if None for name in self.robot_names: node = self.supervisor.getFromDef(name) if node is not None: self.robot_nodes[name] = node self.translation_fields[name] = node.getField("translation") self.rotation_fields[name] = node.getField("rotation") if self.ros_active: rospy.init_node("webots_ros_supervisor", argv=['clock:=/clock']) self.clock_publisher = rospy.Publisher("/clock", Clock, queue_size=1) self.model_state_publisher = rospy.Publisher("/model_states", ModelStates, queue_size=1) self.reset_service = rospy.Service("reset", Empty, self.reset) self.initial_poses_service = rospy.Service("initial_pose", Empty, self.set_initial_poses) self.set_robot_position_service = rospy.Service( "set_robot_position", SetRobotPose, self.robot_pose_callback) self.reset_ball_service = rospy.Service("reset_ball", Empty, self.reset_ball) self.world_info = self.supervisor.getFromDef("world_info") self.ball = self.supervisor.getFromDef("ball") def step_sim(self): self.time += self.timestep / 1000 self.supervisor.step(self.timestep) def step(self): self.step_sim() if self.ros_active: self.publish_clock() self.publish_model_states() def publish_clock(self): self.clock_msg.clock = rospy.Time.from_seconds(self.time) self.clock_publisher.publish(self.clock_msg) def set_gravity(self, active): if active: self.world_info.getField("gravity").setSFFloat(9.81) else: self.world_info.getField("gravity").setSFFloat(0) def reset_robot_pose(self, pos, quat, name="amy"): self.set_robot_pose_quat(pos, quat, name) if name in self.robot_nodes: self.robot_nodes[name].resetPhysics() def reset_robot_pose_rpy(self, pos, rpy, name="amy"): self.set_robot_pose_rpy(pos, rpy, name) if name in self.robot_nodes: self.robot_nodes[name].resetPhysics() def reset(self, req=None): self.supervisor.simulationReset() self.supervisor.simulationResetPhysics() def set_initial_poses(self, req=None): self.reset_robot_pose_rpy([-1, 3, 0.42], [0, 0.24, -1.57], name="amy") self.reset_robot_pose_rpy([-1, -3, 0.42], [0, 0.24, 1.57], name="rory") self.reset_robot_pose_rpy([-3, 3, 0.42], [0, 0.24, -1.57], name="jack") self.reset_robot_pose_rpy([-3, -3, 0.42], [0, 0.24, 1.57], name="donna") self.reset_robot_pose_rpy([0, 6, 0.42], [0, 0.24, -1.57], name="melody") def robot_pose_callback(self, req=None): self.reset_robot_pose_rpy( [req.position.x, req.position.y, req.position.z], [0, 0, 0], req.robot_name) def reset_ball(self, req=None): self.ball.getField("translation").setSFVec3f([0, 0, 0.0772]) self.ball.getField("rotation").setSFRotation([0, 0, 1, 0]) self.ball.resetPhysics() 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_axis_angle(self, axis, angle, name="amy"): if name in self.rotation_fields: self.rotation_fields[name].setSFRotation( list(np.append(axis, angle))) def set_robot_rpy(self, rpy, name="amy"): axis, angle = transforms3d.euler.euler2axangle(rpy[0], rpy[1], rpy[2], axes='sxyz') self.set_robot_axis_angle(axis, angle, name) def set_robot_quat(self, quat, name="amy"): axis, angle = transforms3d.quaternions.quat2axangle( [quat[3], quat[0], quat[1], quat[2]]) self.set_robot_axis_angle(axis, angle, name) def set_robot_position(self, pos, name="amy"): if name in self.translation_fields: self.translation_fields[name].setSFVec3f(list(pos)) def set_robot_pose_rpy(self, pos, rpy, name="amy"): self.set_robot_position(pos, name) self.set_robot_rpy(rpy, name) def set_robot_pose_quat(self, pos, quat, name="amy"): self.set_robot_position(pos, name) self.set_robot_quat(quat, name) def get_robot_position(self, name="amy"): if name in self.translation_fields: return self.translation_fields[name].getSFVec3f() def get_robot_orientation_axangles(self, name="amy"): if name in self.rotation_fields: return self.rotation_fields[name].getSFRotation() def get_robot_orientation_rpy(self, name="amy"): ax_angle = self.get_robot_orientation_axangles(name) return list( transforms3d.euler.axangle2euler(ax_angle[:3], ax_angle[3], axes='sxyz')) def get_robot_orientation_quat(self, name="amy"): ax_angle = self.get_robot_orientation_axangles(name) # transforms 3d uses scalar (i.e. the w part in the quaternion) first notation of quaternions, ros uses scalar last quat_scalar_first = transforms3d.quaternions.axangle2quat( ax_angle[:3], ax_angle[3]) quat_scalar_last = np.append(quat_scalar_first[1:], quat_scalar_first[0]) return list(quat_scalar_last) def get_robot_pose_rpy(self, name="amy"): return self.get_robot_position(name), self.get_robot_orientation_rpy( name) def get_robot_pose_quat(self, name="amy"): return self.get_robot_position(name), self.get_robot_orientation_quat( name) def publish_model_states(self): msg = ModelStates() for robot_name, robot_node in self.robot_nodes.items(): position, orientation = self.get_robot_pose_quat(name=robot_name) robot_pose = Pose() robot_pose.position = Point(*position) robot_pose.orientation = Quaternion(*orientation) msg.name.append(robot_name) msg.pose.append(robot_pose) head_node = robot_node.getFromProtoDef("head") head_position = head_node.getPosition() head_orientation = head_node.getOrientation() head_orientation_quat = transforms3d.quaternions.mat2quat( np.reshape(head_orientation, (3, 3))) head_pose = Pose() head_pose.position = Point(*head_position) head_pose.orientation = Quaternion(head_orientation_quat[1], head_orientation_quat[2], head_orientation_quat[3], head_orientation_quat[0]) msg.name.append(robot_name + "_head") msg.pose.append(head_pose) ball_position = self.ball.getField("translation").getSFVec3f() ball_pose = Pose() ball_pose.position = Point(*ball_position) ball_pose.orientation = Quaternion() msg.name.append("ball") msg.pose.append(ball_pose) self.model_state_publisher.publish(msg)
class SupervisorController: def __init__(self, ros_active=False, mode='normal', do_ros_init=True, base_ns='', model_states_active=True): """ The SupervisorController, a Webots controller that can control the world. Set the environment variable WEBOTS_ROBOT_NAME to "supervisor_robot" if used with 1_bot.wbt or 4_bots.wbt. :param ros_active: Whether to publish ROS messages :param mode: Webots mode, one of 'normal', 'paused', or 'fast' :param do_ros_init: Whether rospy.init_node should be called :param base_ns: The namespace of this node, can normally be left empty """ # requires WEBOTS_ROBOT_NAME to be set to "supervisor_robot" self.ros_active = ros_active self.model_states_active = model_states_active self.time = 0 self.clock_msg = Clock() self.supervisor = Supervisor() self.keyboard_activated = False if mode == 'normal': self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) elif mode == 'paused': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE) elif mode == 'fast': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST) else: self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) self.motors = [] self.sensors = [] self.timestep = int(self.supervisor.getBasicTimeStep()) self.robot_nodes = {} self.translation_fields = {} self.rotation_fields = {} self.joint_nodes = {} self.link_nodes = {} root = self.supervisor.getRoot() children_field = root.getField('children') children_count = children_field.getCount() for i in range(children_count): node = children_field.getMFNode(i) name_field = node.getField('name') if name_field is not None and node.getType() == Node.ROBOT: # this is a robot name = name_field.getSFString() if name == "supervisor_robot": continue self.robot_nodes[name] = node self.translation_fields[name] = node.getField("translation") self.rotation_fields[name] = node.getField("rotation") self.joint_nodes[name], self.link_nodes[ name] = self.collect_joint_and_link_node_references( node, {}, {}) if self.ros_active: # need to handle these topics differently or we will end up having a double // if base_ns == "": clock_topic = "/clock" model_topic = "/model_states" else: clock_topic = base_ns + "clock" model_topic = base_ns + "model_states" if do_ros_init: rospy.init_node("webots_ros_supervisor", argv=['clock:=' + clock_topic]) self.clock_publisher = rospy.Publisher(clock_topic, Clock, queue_size=1) self.model_state_publisher = rospy.Publisher(model_topic, ModelStates, queue_size=1) self.reset_service = rospy.Service(base_ns + "reset", Empty, self.reset) self.reset_pose_service = rospy.Service(base_ns + "reset_pose", Empty, self.set_initial_poses) self.set_robot_pose_service = rospy.Service( base_ns + "set_robot_pose", SetObjectPose, self.robot_pose_callback) self.reset_ball_service = rospy.Service(base_ns + "reset_ball", Empty, self.reset_ball) self.set_ball_position_service = rospy.Service( base_ns + "set_ball_position", SetObjectPosition, self.ball_pos_callback) self.world_info = self.supervisor.getFromDef("world_info") self.ball = self.supervisor.getFromDef("ball") def collect_joint_and_link_node_references(self, node, joint_dict, link_dict): # this is a recursive function that iterates through the whole robot as this seems to be the only way to # get all joints # add node if it is a joint if node.getType() == Node.SOLID: name = node.getDef() link_dict[name] = node if node.getType() == Node.HINGE_JOINT: name = node.getDef() # substract the "Joint" keyword due to naming convention name = name[:-5] joint_dict[name] = node # the joints dont have children but an "endpoint" that we need to search through if node.isProto(): endpoint_field = node.getProtoField('endPoint') else: endpoint_field = node.getField('endPoint') endpoint_node = endpoint_field.getSFNode() self.collect_joint_and_link_node_references( endpoint_node, joint_dict, link_dict) # needs to be done because Webots has two different getField functions for proto nodes and normal nodes if node.isProto(): children_field = node.getProtoField('children') else: children_field = node.getField('children') if children_field is not None: for i in range(children_field.getCount()): child = children_field.getMFNode(i) self.collect_joint_and_link_node_references( child, joint_dict, link_dict) return joint_dict, link_dict def step_sim(self): self.time += self.timestep / 1000 self.supervisor.step(self.timestep) def step(self): self.step_sim() if self.ros_active: self.publish_clock() if self.model_states_active: self.publish_model_states() def handle_gui(self): if not self.keyboard_activated: self.keyboard = Keyboard() self.keyboard.enable(100) self.keyboard_activated = True key = self.keyboard.getKey() if key == ord('R'): self.reset() elif key == ord('P'): self.set_initial_poses() elif key == Keyboard.SHIFT + ord('R'): try: self.reset_ball() except AttributeError: print("No ball in simulation that can be reset") return key def publish_clock(self): self.clock_msg.clock = rospy.Time.from_seconds(self.time) self.clock_publisher.publish(self.clock_msg) def set_gravity(self, active): if active: self.world_info.getField("gravity").setSFFloat(9.81) else: self.world_info.getField("gravity").setSFFloat(0) def set_self_collision(self, active, name="amy"): self.robot_nodes[name].getField("selfCollision").setSFBool(active) def reset_robot_pose(self, pos, quat, name="amy"): self.set_robot_pose_quat(pos, quat, name) if name in self.robot_nodes: self.robot_nodes[name].resetPhysics() def reset_robot_pose_rpy(self, pos, rpy, name="amy"): self.set_robot_pose_rpy(pos, rpy, name) if name in self.robot_nodes: self.robot_nodes[name].resetPhysics() def reset(self, req=None): self.supervisor.simulationReset() self.supervisor.simulationResetPhysics() return EmptyResponse() def reset_robot_init(self, name="amy"): self.robot_nodes[name].loadState('__init__') self.robot_nodes[name].resetPhysics() def set_initial_poses(self, req=None): self.reset_robot_pose_rpy([-1, 3, 0.42], [0, 0.24, -1.57], name="amy") self.reset_robot_pose_rpy([-1, -3, 0.42], [0, 0.24, 1.57], name="rory") self.reset_robot_pose_rpy([-3, 3, 0.42], [0, 0.24, -1.57], name="jack") self.reset_robot_pose_rpy([-3, -3, 0.42], [0, 0.24, 1.57], name="donna") self.reset_robot_pose_rpy([0, 6, 0.42], [0, 0.24, -1.57], name="melody") return EmptyResponse() def robot_pose_callback(self, req=None): self.reset_robot_pose( [req.pose.position.x, req.pose.position.y, req.pose.position.z], [ req.pose.orientation.x, req.pose.orientation.y, req.pose.orientation.z, req.pose.orientation.w ], req.object_name) return SetObjectPoseResponse() def reset_ball(self, req=None): self.ball.getField("translation").setSFVec3f([0, 0, 0.0772]) self.ball.getField("rotation").setSFRotation([0, 0, 1, 0]) self.ball.resetPhysics() return EmptyResponse() def ball_pos_callback(self, req=None): self.set_ball_pose([req.position.x, req.position.y, req.position.z]) return SetObjectPositionResponse() def set_ball_pose(self, pos): self.ball.getField("translation").setSFVec3f(list(pos)) self.ball.resetPhysics() def get_ball_pose(self): return self.ball.getField("translation").getSFVec3f() def get_ball_velocity(self): if self.ball: return self.ball.getVelocity() 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_axis_angle(self, axis, angle, name="amy"): if name in self.rotation_fields: self.rotation_fields[name].setSFRotation( list(np.append(axis, angle))) def set_robot_rpy(self, rpy, name="amy"): axis, angle = transforms3d.euler.euler2axangle(rpy[0], rpy[1], rpy[2], axes='sxyz') self.set_robot_axis_angle(axis, angle, name) def set_robot_quat(self, quat, name="amy"): axis, angle = transforms3d.quaternions.quat2axangle( [quat[3], quat[0], quat[1], quat[2]]) self.set_robot_axis_angle(axis, angle, name) def set_robot_position(self, pos, name="amy"): if name in self.translation_fields: self.translation_fields[name].setSFVec3f(list(pos)) def set_robot_pose_rpy(self, pos, rpy, name="amy"): self.set_robot_position(pos, name) self.set_robot_rpy(rpy, name) def set_robot_pose_quat(self, pos, quat, name="amy"): self.set_robot_position(pos, name) self.set_robot_quat(quat, name) def get_robot_position(self, name="amy"): if name in self.translation_fields: return self.translation_fields[name].getSFVec3f() def get_robot_orientation_axangles(self, name="amy"): if name in self.rotation_fields: return self.rotation_fields[name].getSFRotation() def get_robot_orientation_rpy(self, name="amy"): ax_angle = self.get_robot_orientation_axangles(name) return list( transforms3d.euler.axangle2euler(ax_angle[:3], ax_angle[3], axes='sxyz')) def get_robot_orientation_quat(self, name="amy"): ax_angle = self.get_robot_orientation_axangles(name) # transforms 3d uses scalar (i.e. the w part in the quaternion) first notation of quaternions, ros uses scalar last quat_scalar_first = transforms3d.quaternions.axangle2quat( ax_angle[:3], ax_angle[3]) quat_scalar_last = np.append(quat_scalar_first[1:], quat_scalar_first[0]) return list(quat_scalar_last) def get_robot_pose_rpy(self, name="amy"): return self.get_robot_position(name), self.get_robot_orientation_rpy( name) def get_robot_pose_quat(self, name="amy"): return self.get_robot_position(name), self.get_robot_orientation_quat( name) def get_robot_velocity(self, name="amy"): velocity = self.robot_nodes[name].getVelocity() return velocity[:3], velocity[3:] def get_link_pose(self, link, name="amy"): link_node = self.robot_nodes[name].getFromProtoDef(link) if not link_node: return None link_position = link_node.getPosition() mat = link_node.getOrientation() link_orientation = transforms3d.quaternions.mat2quat(np.array(mat)) return link_position, np.append(link_orientation[1:], link_orientation[0]) def get_link_velocities(self, link, name="amy"): """Returns linear and angular velocities""" link_node = self.robot_nodes[name].getFromProtoDef(link) velocity = link_node.getVelocity() return velocity[:3], velocity[3:] def publish_model_states(self): if self.model_state_publisher.get_num_connections() != 0: msg = ModelStates() for robot_name, robot_node in self.robot_nodes.items(): position, orientation = self.get_robot_pose_quat( name=robot_name) robot_pose = Pose() robot_pose.position = Point(*position) robot_pose.orientation = Quaternion(*orientation) msg.name.append(robot_name) msg.pose.append(robot_pose) lin_vel, ang_vel = self.get_robot_velocity(robot_name) twist = Twist() twist.linear.x = lin_vel[0] twist.linear.y = lin_vel[1] twist.linear.z = lin_vel[2] twist.angular.x = ang_vel[0] twist.angular.y = ang_vel[1] twist.angular.z = ang_vel[2] msg.twist.append(twist) head_node = robot_node.getFromProtoDef("head") head_position = head_node.getPosition() head_orientation = head_node.getOrientation() head_orientation_quat = transforms3d.quaternions.mat2quat( np.reshape(head_orientation, (3, 3))) head_pose = Pose() head_pose.position = Point(*head_position) head_pose.orientation = Quaternion(head_orientation_quat[1], head_orientation_quat[2], head_orientation_quat[3], 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(*ball_position) ball_pose.orientation = Quaternion() msg.name.append("ball") msg.pose.append(ball_pose) self.model_state_publisher.publish(msg)
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)