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)
