def goto_frame(self):
'''
Plays one single frame
'''
self.set_all_joints_stiff()
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k,v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
for k, v in self._checkBoxesPower[self._widget.frameList.currentItem().text()].items():
torque_msg.joint_names.append(k)
torque_msg.on.append(v)
self.effort_pub.publish(torque_msg)
self._joint_pub.publish(pos_msg)
def set_to_walkready(self):
"""Set the robot to walkready position"""
walkready = {
"LAnklePitch": -30,
"LAnkleRoll": 0,
"LHipPitch": 30,
"LHipRoll": 0,
"LHipYaw": 0,
"LKnee": 60,
"RAnklePitch": 30,
"RAnkleRoll": 0,
"RHipPitch": -30,
"RHipRoll": 0,
"RHipYaw": 0,
"RKnee": -60,
"LShoulderPitch": 0,
"LShoulderRoll": 0,
"LElbow": 45,
"RShoulderPitch": 0,
"RShoulderRoll": 0,
"RElbow": -45,
"HeadPan": 0,
"HeadTilt": 0
}
msg = JointCommand()
for name, pos in walkready.items():
msg.joint_names.append(name)
msg.positions.append(math.radians(pos))
msg.velocities.append(-1)
msg.accelerations.append(-1)
self.sim.set_joints(msg)
def cb(msg):
msg2 = JointCommand()
msg2.joint_names = msg.position.joint_names
msg2.positions = msg.position.points[0].positions
msg2.velocities = [-1] * len(msg2.joint_names)
msg2.accelerations = [-1] * len(msg2.joint_names)
msg2.max_currents = [-1] * len(msg2.joint_names)
pub.publish(msg2)
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LShoulderPitch", "RShoulderPitch", 'LShoulderRoll',
'RShoulderRoll'
]
joint_command_msg.positions = [1.57, 1.57, 0.3, -0.3]
return joint_command_msg
def animation_callback(self, msg):
""" The animation server is sending us goal positions for the next keyframe"""
self.blackboard.last_animation_goal_time = msg.header.stamp.to_sec()
if msg.request:
rospy.loginfo(
"Got Animation request. HCM will try to get controllable now.")
# animation has to wait
# state machine should try to become controllable
self.blackboard.animation_requested = True
return
if msg.first:
if msg.hcm:
# comming from ourselves
# we don't have to do anything, since we must be in the right state
pass
else:
# comming from outside
# check if we can run an animation now
if self.blackboard.current_state != STATE_CONTROLABLE:
rospy.logwarn(
"HCM is not controllable, animation refused.")
return
else:
# we're already controllable, go to animation running
self.blackboard.external_animation_running = True
if msg.last:
if msg.hcm:
# This was an animation from the DSD
self.blackboard.hcm_animation_finished = True
pass
else:
# this is the last frame, we want to tell the DSD, that we're finished with the animations
self.blackboard.external_animation_running = False
if msg.position is None:
# probably this was just to tell us we're finished
# we don't need to set another position to the motors
return
# forward positions to motors, if some where transmitted
if len(msg.position.points) > 0:
out_msg = JointCommand()
out_msg.positions = msg.position.points[0].positions
out_msg.joint_names = msg.position.joint_names
out_msg.accelerations = [-1] * len(out_msg.joint_names)
out_msg.velocities = [-1] * len(out_msg.joint_names)
out_msg.max_currents = [-1] * len(out_msg.joint_names)
if self.blackboard.shut_down_request:
# there are sometimes transmittions errors during shutdown due to race conditions
# there is nothing we can do so just ignore the errors in this case
try:
self.joint_goal_publisher.publish(out_msg)
except:
pass
else:
self.joint_goal_publisher.publish(out_msg)
def __init__(self):
log_level = rospy.DEBUG if rospy.get_param("/debug_active",
False) else rospy.INFO
rospy.init_node("joy_to_twist", log_level=log_level, anonymous=False)
self.head_pan_pos = 0
self.head_tilt_pos = 0
# --- Initialize Topics ---
rospy.Subscriber("/joy", Joy, self.joy_cb, queue_size=1)
rospy.Subscriber("joint_states",
JointState,
self.joint_state_cb,
queue_size=1)
self.speak_pub = rospy.Publisher('speak', Speak, queue_size=1)
self.speak_msg = Speak()
self.speak_msg.priority = 1
self.walk_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.walk_msg = Twist()
self.last_walk_msg = Twist()
self.walk_msg.linear.x = 0.0
self.walk_msg.linear.y = 0.0
self.walk_msg.linear.z = 0.0
self.walk_msg.angular.x = 0.0
self.walk_msg.angular.y = 0.0
self.walk_msg.angular.z = 0.0
self.head_pub = rospy.Publisher("/head_motor_goals",
JointCommand,
queue_size=1)
self.head_msg = JointCommand()
self.head_msg.max_currents = [-1] * 2
self.head_msg.velocities = [5] * 2
self.head_msg.accelerations = [40] * 2
# --- init animation action ---
self.anim_client = actionlib.SimpleActionClient(
'animation', humanoid_league_msgs.msg.PlayAnimationAction)
self.anim_goal = humanoid_league_msgs.msg.PlayAnimationGoal()
self.anim_goal.hcm = False
first_try = self.anim_client.wait_for_server(rospy.Duration(1))
if not first_try:
rospy.logerr(
"Animation Action Server not running! Teleop can not work without animation action server. "
"Will now wait until server is accessible!")
self.anim_client.wait_for_server()
rospy.logwarn("Animation server running, will go on.")
rospy.spin()
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LElbow", "RElbow", "LShoulderPitch", "RShoulderPitch"
]
joint_command_msg.positions = [
math.radians(35.86),
math.radians(-36.10),
math.radians(75.27),
math.radians(-75.58)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LElbow", "RElbow", "LShoulderPitch", "RShoulderPitch"
]
joint_command_msg.positions = [
math.radians(-60),
math.radians(60),
math.radians(120.0),
math.radians(-120.0)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"LeftElbow", "RightElbow", "LeftShoulderPitch [shoulder]",
"RightShoulderPitch [shoulder]"
]
joint_command_msg.positions = [
math.radians(-90.0),
math.radians(90.0),
math.radians(45.0),
math.radians(-45.0)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"left_arm_motor_0 [shoulder]", "right_arm_motor_0 [shoulder]",
"left_arm_motor_1", "right_arm_motor_1"
]
joint_command_msg.positions = [
math.radians(0),
math.radians(0),
math.radians(170),
math.radians(170)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"Shoulder-L [shoulder]", "Shoulder-R [shoulder]", "UpperArm-L",
"UpperArm-R", "LowerArm-L", "LowerArm-R"
]
joint_command_msg.positions = [
math.radians(60.0),
math.radians(-60.0),
math.radians(10.0),
math.radians(-10.0),
math.radians(-135.0),
math.radians(135.0)
]
return joint_command_msg
def __init__(self):
rclpy.init(args=None)
super().__init__("animation")
self.joint_publisher = self.create_publisher(
JointCommand, "DynamixelController/command", 1)
self.joint_command_msg = JointCommand()
self.joint_command_msg.joint_names = JOINT_NAMES
self.joint_command_msg.positions = [0.0] * len(JOINT_NAMES)
self.joint_command_msg.velocities = [-1.0] * len(JOINT_NAMES)
self.joint_command_msg.accelerations = [-1.0] * len(JOINT_NAMES)
self.joint_command_msg.max_currents = [-1.0] * len(JOINT_NAMES)
self.create_subscription(Animation, "animation", self.animation_cb, 10)
rclpy.spin(self)
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"l_el", "r_el", "l_sho_pitch", "r_sho_pitch", "l_sho_roll",
"r_sho_roll"
]
joint_command_msg.positions = [
math.radians(-140),
math.radians(140),
math.radians(-135),
math.radians(135),
math.radians(-90),
math.radians(90)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"left_shoulder_pitch [shoulder]",
"right_shoulder_pitch [shoulder]", "left_shoulder_roll",
"right_shoulder_roll", "left_elbow", "right_elbow"
]
joint_command_msg.positions = [
math.radians(60.0),
math.radians(60.0),
math.radians(10.0),
math.radians(10.0),
math.radians(-135.0),
math.radians(-135.0)
]
return joint_command_msg
def get_arm_pose(self):
joint_command_msg = JointCommand()
joint_command_msg.joint_names = [
"leftElbowYaw", "rightElbowYaw", "leftShoulderPitch[shoulder]",
"rightShoulderPitch[shoulder]", "leftShoulderYaw",
"rightShoulderYaw"
]
joint_command_msg.positions = [
math.radians(-160),
math.radians(160),
math.radians(75.27),
math.radians(75.58),
math.radians(-75.58),
math.radians(75.58)
]
return joint_command_msg
def box_ticked(self):
'''
Controls whether a checkbox has been clicked, and reacts.
'''
for k, v in self._treeItems.items():
self._motorSwitched[k] = (v.checkState(0) == 2)
for k, v in self._motorSwitched.items():
self._textFields[k].setEnabled(v)
self._sliders[k].setEnabled(v)
self.set_sliders_and_text_fields(manual=False)
## Wolfgang part
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
if self._widget.treeModeSelector.currentIndex() == 0:
for k, v in sorted(self._treeItems.items()):
torque_msg.joint_names.append(k)
torque_msg.on.append(v.checkState(0) == 2)
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k, v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
self._joint_pub.publish(pos_msg)
self.effort_pub.publish(torque_msg)
if not self._widget.frameList.currentItem() == None:
if not self._widget.frameList.currentItem().text(
) == "#CURRENT_FRAME":
self.treeModeChanged(
self._widget.treeModeSelector.currentIndex())
self.record(keep=True)
def goto_init(self):
'''
Plays init frame, sets all motor values to 0
'''
self.set_all_joints_stiff()
if self._widget.frameList.currentItem().text() == "#CURRENT_FRAME":
for k, v in self._workingValues.iteritems():
self._workingValues[k] = 0.0
for k, v in self._currentGoals.iteritems():
self._currentGoals[k] = 0.0
self.set_sliders_and_text_fields(manual=False)
pos_msg = JointCommand()
pos_msg.joint_names = self._initial_joints.name
pos_msg.velocities = [1.0] * len(self._initial_joints.name)
pos_msg.positions = [0.0] * len(self._initial_joints.name)
pos_msg.accelerations = [-1.0] * len(self._initial_joints.name)
pos_msg.max_currents = [-1.0] * len(self._initial_joints.name)
self._joint_pub.publish(pos_msg)
def __init__(self, blackboard):
self.blackboard = blackboard
# possible variables
self.head_mode = None
# preparing message for more performance
self.pos_msg = JointCommand()
self.pos_msg.joint_names = ["HeadPan", "HeadTilt"]
self.pos_msg.positions = [0, 0]
self.pos_msg.velocities = [0, 0]
self.pos_msg.accelerations = [17, 17]
self.pos_msg.max_currents = [-1, -1]
self.position_publisher = None # type: rospy.Publisher
self.visual_compass_record_trigger = None # type: rospy.Publisher
self.tf_buffer = tf2.Buffer(rospy.Duration(5))
# tf_listener is necessary, even though unused!
self.tf_listener = tf2.TransformListener(self.tf_buffer)
self.current_head_position = [0, 0]
from bitbots_msgs.msg import JointCommand
from geometry_msgs.msg import Twist, Point, Pose, Quaternion
from bitbots_test.mocks import MockSubscriber
from bitbots_test.test_case import WebotsTestCase
from nav_msgs.msg import Odometry
walkready = JointCommand(
joint_names=[
"HeadPan", "HeadTilt", "LShoulderPitch", "LShoulderRoll", "LElbow",
"RShoulderPitch", "RShoulderRoll", "RElbow"
],
velocities=[5.0] * 8,
accelerations=[-1.0] * 8,
max_currents=[-1.0] * 8,
positions=[
0.0, # HeadPan
0.0, # HeadTilt
math.radians(75.27), # LShoulderPitch
0.0, # LShoulderRoll
math.radians(35.86), # LElbow
math.radians(-75.58), # RShoulderPitch
0.0, # RShoulderRoll
math.radians(-36.10), # RElbow
])
class TestWalk(WebotsTestCase):
def initialize_everything(self):
# get arms walkready
joint_pub = rospy.Publisher("DynamixelController/command",
JointCommand,
def __init__(self):
# create node
super().__init__("TeleopKeyboard")
self.settings = termios.tcgetattr(sys.stdin)
parser = argparse.ArgumentParser()
parser.add_argument('--robot-type',
help="Which robot type is used {wolfgang, op2, op3, nao, rfc, chape, mrl_hsl}",
default="wolfgang")
args, unknown = parser.parse_known_args()
robot_type = args.robot_type
if robot_type not in ["wolfgang", "op2", "op3", "nao", "rfc", "chape", "mrl_hsl"]:
self.get_logger().fatal(
f"Robot type {robot_type} not known. Should be one of [wolfgang, op2, op3, nao, rfc, chape, mrl_hsl]")
exit()
# generate walkready command
joint_names = []
joint_positions = []
for joint_tuple in __walkready_joints__[robot_type]:
joint_names.append(joint_tuple[0])
joint_positions.append(joint_tuple[1])
self.walkready = JointCommand(
joint_names=joint_names,
velocities=[__velocity__] * len(joint_names),
accelerations=[__accelerations__] * len(joint_names),
max_currents=[__max_currents__] * len(joint_names),
positions=joint_positions)
# Walking Part
self.pub = self.create_publisher(Twist, 'cmd_vel', 1)
self.head_pan_pos = 0
self.head_tilt_pos = 0
self.x_speed_step = 0.01
self.y_speed_step = 0.01
self.turn_speed_step = 0.01
self.x = 0
self.y = 0
self.a_x = -1
self.th = 0
self.status = 0
# Head Part
self.create_subscription(JointState, "joint_states", self.joint_state_cb, 1)
if self.get_parameter("use_sim_time").get_parameter_value().bool_value:
self.head_pub = self.create_publisher(JointCommand, "head_motor_goals", 1)
else:
self.head_pub = self.create_publisher(JointCommand, "DynamixelController/command", 1)
self.head_msg = JointCommand()
self.head_msg.max_currents = [float(-1)] * 2
self.head_msg.velocities = [float(5)] * 2
self.head_msg.accelerations = [float(40)] * 2
self.head_msg.joint_names = ['HeadPan', 'HeadTilt']
self.head_msg.positions = [float(0)] * 2
self.head_pan_step = 0.05
self.head_tilt_step = 0.05
self.walk_kick_pub = self.create_publisher(Bool, "kick", 1)
self.joint_pub = self.create_publisher(JointCommand, "DynamixelController/command", 1)
self.reset_robot = self.create_client(Empty, "/reset_pose")
self.reset_ball = self.create_client(Empty, "/reset_ball")
print(msg)
self.frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/"
self.client = ActionClient(self, Kick, 'dynamic_kick')
if __name__ == "__main__":
rospy.init_node('test_look_at')
head_tf_frame = "base_link"
tf_buffer = tf2.Buffer(rospy.Duration(5))
tf_listener = tf2.TransformListener(tf_buffer)
request = IKRequest()
request.group_name = "Head"
request.timeout.secs = 1
request.approximate = True
request.look_at_goals.append(LookAtGoal())
request.look_at_goals[0].link_name = "camera"
request.look_at_goals[0].weight = 1
request.look_at_goals[0].axis.x = 1
pos_msg = JointCommand()
pos_msg.joint_names = ["HeadPan", "HeadTilt"]
pos_msg.positions = [0, 0]
pos_msg.velocities = [1.5, 1.5]
pos_msg.accelerations = [-1, -1]
pos_msg.max_currents = [-1, -1]
rospy.wait_for_service("/bio_ik/get_bio_ik")
bio_ik = rospy.ServiceProxy('/bio_ik/get_bio_ik', GetIK)
publish_motor_goals = rospy.Publisher('/head_motor_goals',
JointCommand,
queue_size=10)
while not rospy.is_shutdown():
x = float(input('x: '))
def shutdown_plugin(self):
'''Clean up by sending the HCM a signal that we are no longer recording'''
self._joint_pub.publish(JointCommand())
rospy.sleep(1)
def __init__(self,
ros_active=False,
robot='wolfgang',
do_ros_init=True,
robot_node=None,
base_ns='',
recognize=False,
camera_active=True,
foot_sensors_active=True):
"""
The RobotController, a Webots controller that controls a single robot.
The environment variable WEBOTS_ROBOT_NAME should be set to "amy", "rory", "jack" or "donna" if used with
4_bots.wbt or to "amy" if used with 1_bot.wbt.
:param ros_active: Whether ROS messages should be published
:param robot: The name of the robot to use, currently one of wolfgang, darwin, nao, op3
:param do_ros_init: Whether to call rospy.init_node (only used when ros_active is True)
:param external_controller: Whether an external controller is used, necessary for RobotSupervisorController
:param base_ns: The namespace of this node, can normally be left empty
"""
self.ros_active = ros_active
self.recognize = recognize
self.camera_active = camera_active
self.foot_sensors_active = foot_sensors_active
if robot_node is None:
self.robot_node = Robot()
else:
self.robot_node = robot_node
self.walkready = [0] * 20
self.time = 0
self.motors = []
self.sensors = []
# for direct access
self.motors_dict = {}
self.sensors_dict = {}
self.timestep = int(self.robot_node.getBasicTimeStep())
self.switch_coordinate_system = True
self.is_wolfgang = False
self.pressure_sensors = None
if robot == 'wolfgang':
self.is_wolfgang = True
self.proto_motor_names = [
"RShoulderPitch [shoulder]", "LShoulderPitch [shoulder]",
"RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow",
"RHipYaw", "LHipYaw", "RHipRoll [hip]", "LHipRoll [hip]",
"RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan",
"HeadTilt"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.initial_joint_positions = {
"LAnklePitch": -30,
"LAnkleRoll": 0,
"LHipPitch": 30,
"LHipRoll": 0,
"LHipYaw": 0,
"LKnee": 60,
"RAnklePitch": 30,
"RAnkleRoll": 0,
"RHipPitch": -30,
"RHipRoll": 0,
"RHipYaw": 0,
"RKnee": -60,
"LShoulderPitch": 75,
"LShoulderRoll": 0,
"LElbow": 36,
"RShoulderPitch": -75,
"RShoulderRoll": 0,
"RElbow": -36,
"HeadPan": 0,
"HeadTilt": 0
}
self.sensor_suffix = "_sensor"
accel_name = "imu accelerometer"
gyro_name = "imu gyro"
camera_name = "camera"
self.pressure_sensor_names = []
if self.foot_sensors_active:
self.pressure_sensor_names = [
"llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"
]
self.pressure_sensors = []
for name in self.pressure_sensor_names:
sensor = self.robot_node.getDevice(name)
sensor.enable(self.timestep)
self.pressure_sensors.append(sensor)
elif robot == 'darwin':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
elif robot == 'nao':
self.proto_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch",
"LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch",
"LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw",
"HeadPitch"
]
self.external_motor_names = self.proto_motor_names
self.sensor_suffix = "S"
accel_name = "accelerometer"
gyro_name = "gyro"
camera_name = "CameraTop"
self.switch_coordinate_system = False
elif robot == 'op3':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll",
"r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll",
"l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee",
"r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll",
"head_pan", "head_tilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
self.switch_coordinate_system = False
self.motor_names_to_external_names = {}
self.external_motor_names_to_motor_names = {}
for i in range(len(self.proto_motor_names)):
self.motor_names_to_external_names[
self.proto_motor_names[i]] = self.external_motor_names[i]
self.external_motor_names_to_motor_names[
self.external_motor_names[i]] = self.proto_motor_names[i]
self.current_positions = {}
self.joint_limits = {}
for motor_name in self.proto_motor_names:
motor = self.robot_node.getDevice(motor_name)
motor.enableTorqueFeedback(self.timestep)
self.motors.append(motor)
self.motors_dict[
self.motor_names_to_external_names[motor_name]] = motor
sensor = self.robot_node.getDevice(motor_name + self.sensor_suffix)
sensor.enable(self.timestep)
self.sensors.append(sensor)
self.sensors_dict[
self.motor_names_to_external_names[motor_name]] = sensor
self.current_positions[self.motor_names_to_external_names[
motor_name]] = sensor.getValue()
# min, max and middle position (precomputed since it will be used at each step)
self.joint_limits[
self.motor_names_to_external_names[motor_name]] = (
motor.getMinPosition(), motor.getMaxPosition(),
0.5 * (motor.getMinPosition() + motor.getMaxPosition()))
self.accel = self.robot_node.getDevice(accel_name)
self.accel.enable(self.timestep)
self.gyro = self.robot_node.getDevice(gyro_name)
self.gyro.enable(self.timestep)
if self.is_wolfgang:
self.accel_head = self.robot_node.getDevice(
"imu_head accelerometer")
self.accel_head.enable(self.timestep)
self.gyro_head = self.robot_node.getDevice("imu_head gyro")
self.gyro_head.enable(self.timestep)
self.camera = self.robot_node.getDevice(camera_name)
self.camera_counter = 0
if self.camera_active:
self.camera.enable(self.timestep * CAMERA_DIVIDER)
if self.recognize:
self.camera.recognitionEnable(self.timestep)
self.last_img_saved = 0.0
self.img_save_dir = "/tmp/webots/images" + \
time.strftime("%Y-%m-%d-%H-%M-%S") + \
os.getenv('WEBOTS_ROBOT_NAME')
if not os.path.exists(self.img_save_dir):
os.makedirs(self.img_save_dir)
self.imu_frame = rospy.get_param("~imu_frame", "imu_frame")
if self.ros_active:
if base_ns == "":
clock_topic = "/clock"
else:
clock_topic = base_ns + "clock"
if do_ros_init:
rospy.init_node("webots_ros_interface",
argv=['clock:=' + clock_topic])
self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole")
self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole")
self.camera_optical_frame = rospy.get_param(
"~camera_optical_frame", "camera_optical_frame")
self.head_imu_frame = rospy.get_param("~head_imu_frame",
"imu_frame_2")
self.pub_js = rospy.Publisher(base_ns + "joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher(base_ns + "imu/data_raw",
Imu,
queue_size=1)
self.pub_imu_head = rospy.Publisher(base_ns + "imu_head/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher(base_ns + "camera/image_proc",
Image,
queue_size=1)
self.pub_cam_info = rospy.Publisher(base_ns + "camera/camera_info",
CameraInfo,
queue_size=1,
latch=True)
self.pub_pres_left = rospy.Publisher(base_ns +
"foot_pressure_left/raw",
FootPressure,
queue_size=1)
self.pub_pres_right = rospy.Publisher(base_ns +
"foot_pressure_right/raw",
FootPressure,
queue_size=1)
self.cop_l_pub_ = rospy.Publisher(base_ns + "cop_l",
PointStamped,
queue_size=1)
self.cop_r_pub_ = rospy.Publisher(base_ns + "cop_r",
PointStamped,
queue_size=1)
rospy.Subscriber(base_ns + "DynamixelController/command",
JointCommand, self.command_cb)
# publish camera info once, it will be latched
self.cam_info = CameraInfo()
self.cam_info.header.stamp = rospy.Time.from_seconds(self.time)
self.cam_info.header.frame_id = self.camera_optical_frame
self.cam_info.height = self.camera.getHeight()
self.cam_info.width = self.camera.getWidth()
f_y = self.mat_from_fov_and_resolution(
self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height,
self.cam_info.width), self.cam_info.height)
f_x = self.mat_from_fov_and_resolution(self.camera.getFov(),
self.cam_info.width)
self.cam_info.K = [
f_x, 0, self.cam_info.width / 2, 0, f_y,
self.cam_info.height / 2, 0, 0, 1
]
self.cam_info.P = [
f_x, 0, self.cam_info.width / 2, 0, 0, f_y,
self.cam_info.height / 2, 0, 0, 0, 1, 0
]
self.pub_cam_info.publish(self.cam_info)
if robot == "op3":
# start pose
command = JointCommand()
command.joint_names = ["r_sho_roll", "l_sho_roll"]
command.positions = [-math.tau / 8, math.tau / 8]
self.command_cb(command)
# needed to run this one time to initialize current position, otherwise velocity will be nan
self.get_joint_values(self.external_motor_names)
def __init__(self):
# get rid of additional ROS args when used in launch file
args0 = rospy.myargv()
parser = argparse.ArgumentParser()
parser.add_argument("--walking",
"-w",
help="Directly get walking motor goals",
action="store_true")
parser.add_argument("--animation",
"-a",
help="Directly get animation motor goals",
action="store_true")
parser.add_argument("--head",
help="Directly get head motor goals",
action="store_true")
parser.add_argument("--kick",
help="Directly get kick motor goals",
action="store_true")
parser.add_argument("--all",
help="Directly get all motor goals",
action="store_true")
parser.add_argument("--gazebo",
help="Publish for Gazebo instead of rviz",
action="store_true")
args = parser.parse_args(args0[1:])
rospy.init_node("motor_viz_helper", anonymous=False)
if args.gazebo:
self.joint_publisher = rospy.Publisher(
'JointGroupController/command',
Float64MultiArray,
queue_size=10,
tcp_nodelay=True)
else:
self.joint_publisher = rospy.Publisher('joint_states',
JointState,
queue_size=10,
tcp_nodelay=True)
if args.walking or args.all:
rospy.Subscriber("walking_motor_goals",
JointCommand,
self.joint_command_cb,
queue_size=10,
tcp_nodelay=True)
if args.animation or args.all:
rospy.Subscriber("animation",
Animation,
self.animation_cb,
queue_size=10,
tcp_nodelay=True)
if args.head or args.all:
rospy.Subscriber("head_motor_goals",
JointCommand,
self.joint_command_cb,
queue_size=10,
tcp_nodelay=True)
if args.kick or args.all:
rospy.Subscriber("kick_motor_goals",
JointCommand,
self.joint_command_cb,
queue_size=10,
tcp_nodelay=True)
rospy.Subscriber("/DynamixelController/command",
JointCommand,
self.joint_command_cb,
queue_size=10,
tcp_nodelay=True)
self.joint_state_msg = JointState()
self.joint_state_msg.header.stamp = rospy.Time.now()
self.joint_state_msg.name = JOINT_NAMES
self.joint_state_msg.position = JOINT_GOALS
if args.gazebo:
self.joint_publisher.publish(self.get_float_array())
else:
self.joint_publisher.publish(self.joint_state_msg)
self.joint_command_msg = JointCommand()
self.joint_command_msg.joint_names = JOINT_NAMES
self.joint_command_msg.positions = [0] * 20
self.joint_command_msg.velocities = [-1] * 20
rate = rospy.Rate(100)
self.update_time = rospy.Time.now()
while not rospy.is_shutdown():
self.update_joint_states(self.joint_command_msg)
self.joint_state_msg.header.stamp = rospy.Time.now()
if args.gazebo:
self.joint_publisher.publish(self.get_float_array())
else:
self.joint_publisher.publish(self.joint_state_msg)
rate.sleep()
def set_joints_dict(self, dict):
msg = JointCommand()
for key in dict.keys():
msg.joint_names.append(key)
msg.positions.append(dict[key])
self.set_joints(msg)
from rclpy.node import Node
import math
from bitbots_msgs.msg import JointCommand
DYNAMIXEL_CMD_TOPIC = "/DynamixelController/command"
JOINT_NAME = "LAnkleRoll"
PUBLISH_RATE = 1000
# sin function
FREQUENCY = 0.5
AMPLITUDE = 72 #degree
if __name__ == "__main__":
msg = JointCommand(joint_names=[JOINT_NAME],
velocities=[-1],
accelerations=[-1],
max_currents=[-1])
rclpy.init(args=None)
pub = self.create_publisher(JointCommand, DYNAMIXEL_CMD_TOPIC, 1)
rate = self.create_rate(PUBLISH_RATE)
while rclpy.ok():
time = self.get_clock().now()
position = math.radians(AMPLITUDE) * math.sin(
2 * math.pi * FREQUENCY * time.to_sec())
msg.header.stamp = time
msg.positions = [position]
pub.publish(msg)
rate.sleep()
def shutdown_plugin(self):
"""Clean up by sending the HCM a signal that we are no longer recording and by stopping publishers"""
self._joint_pub.publish(JointCommand())
rospy.sleep(1)
def __init__(self):
super().__init__('MotorGoalsVizHelper')
# get rid of additional ROS args when used in launch file
parser = argparse.ArgumentParser()
parser.add_argument("--robot-type", "-t", help="What type of robot to use", default="wolfgang")
parser.add_argument("--walking", "-w", help="Directly get walking motor goals", action="store_true")
parser.add_argument("--animation", "-a", help="Directly get animation motor goals", action="store_true")
parser.add_argument("--head", help="Directly get head motor goals", action="store_true")
parser.add_argument("--kick", help="Directly get kick motor goals", action="store_true")
parser.add_argument("--dynup", help="Directly get Dynup motor goals", action="store_true")
parser.add_argument("--all", help="Directly get all motor goals", action="store_true")
parser.add_argument("--gazebo", help="Publish for Gazebo instead of rviz", action="store_true")
parser.add_argument("--ros-args", help="just to filter ros args", action="store_true")
parser.add_argument("-r", help="just to filter ros args", action="store_true")
parser.add_argument("__node", help="just to filter ros args", action="store_true")
argv = sys.argv[1:]
try:
argv.remove("--ros-args")
argv.remove("-r")
argv.remove("__node:=joint_goal_viz")
except:
pass
self.args = parser.parse_args(argv)
if self.args.robot_type == "wolfgang":
# List of all joint names. Do not change the order as it is important for Gazebo
self.joint_names = ['HeadPan', 'HeadTilt', 'LShoulderPitch', 'LShoulderRoll', 'LElbow', 'RShoulderPitch',
'RShoulderRoll', 'RElbow', 'LHipYaw', 'LHipRoll', 'LHipPitch', 'LKnee', 'LAnklePitch',
'LAnkleRoll', 'RHipYaw', 'RHipRoll', 'RHipPitch', 'RKnee', 'RAnklePitch', 'RAnkleRoll']
self.joint_goals = [float(0), float(0), float(0), float(0), float(0), float(0), float(0), float(0),
float(0), float(0), float(0.7), float(-1), float(-0.4), float(0), float(0), float(0),
float(-0.7), float(1), float(0.4), float(0)]
elif self.args.robot_type == "itandroids":
self.joint_names = ["rightShoulderPitch[shoulder]", "leftShoulderPitch[shoulder]",
"rightShoulderYaw", "leftShoulderYaw", "rightElbowYaw", "leftElbowYaw", "rightHipYaw",
"leftHipYaw", "rightHipRoll[hip]", "leftHipRoll[hip]", "rightHipPitch",
"leftHipPitch", "rightKneePitch", "leftKneePitch", "rightAnklePitch", "leftAnklePitch",
"rightAnkleRoll", "leftAnkleRoll", "neckYaw", "neckPitch"]
self.joint_goals = [float(0), float(0), float(0), float(0), float(0), float(0), float(0), float(0),
float(0), float(0), float(0), float(0), float(0), float(0), float(0), float(0),
float(0), float(0), float(0), float(0)]
else:
print(f"Unknown robot type {self.args.robot_type}")
exit()
if self.args.gazebo:
self.joint_publisher = self.create_publisher(Float64MultiArray, 'JointGroupController/command', 10)
else:
self.joint_publisher = self.create_publisher(JointState, 'joint_states', 10)
if self.args.walking or self.args.all:
self.create_subscription(JointCommand, "walking_motor_goals", self.joint_command_cb, 10)
if self.args.animation or self.args.all:
self.create_subscription(Animation, "animation", self.animation_cb, 10)
if self.args.head or self.args.all:
self.create_subscription(JointCommand, "head_motor_goals", self.joint_command_cb, 10)
if self.args.kick or self.args.all:
self.create_subscription(JointCommand, "kick_motor_goals", self.joint_command_cb, 10)
if self.args.dynup or self.args.all:
self.create_subscription(JointCommand, "dynup_motor_goals", self.joint_command_cb, 10)
self.create_subscription(JointCommand, "/DynamixelController/command", self.joint_command_cb, 10)
self.joint_state_msg = JointState()
self.joint_state_msg.header.stamp = self.get_clock().now().to_msg()
self.joint_state_msg.name = self.joint_names
self.joint_state_msg.position = self.joint_goals
if self.args.gazebo:
self.joint_publisher.publish(self.get_float_array())
else:
self.joint_publisher.publish(self.joint_state_msg)
self.joint_command_msg = JointCommand()
self.joint_command_msg.joint_names = self.joint_names
self.joint_command_msg.positions = [float(0)] * 20
self.joint_command_msg.velocities = [float(-1)] * 20
def reset(self):
# completly reset pybullet, since it is strange
if self.sim_type == "pybullet":
pass # self.sim.reset_simulation()
# reset Dynup. send emtpy message to just cancel all goals
self.dynup_cancel_pub.publish(GoalID())
if self.direction == "front":
self.hand_ground_time = self.dynup_params["time_hands_side"] + \
self.dynup_params["time_hands_rotate"] + \
self.dynup_params["time_foot_close"] + \
self.dynup_params["time_hands_front"] + \
self.dynup_params["time_foot_ground_front"] + \
self.dynup_params["time_torso_45"]
self.rise_phase_time = self.hand_ground_time + \
self.dynup_params["time_to_squat"]
self.in_squat_time = self.rise_phase_time + \
self.dynup_params["wait_in_squat_front"]
self.total_trial_length = self.in_squat_time + \
self.dynup_params["rise_time"]
print(self.total_trial_length)
elif self.direction == "back":
self.hand_ground_time = self.dynup_params["time_legs_close"] + \
self.dynup_params["time_foot_ground_back"]
self.rise_phase_time = self.hand_ground_time + \
self.dynup_params["time_full_squat_hands"] + \
self.dynup_params["time_full_squat_legs"]
self.in_squat_time = self.rise_phase_time + \
self.dynup_params["wait_in_squat_back"]
self.total_trial_length = self.in_squat_time + \
self.dynup_params["rise_time"]
else:
print(f"Direction {self.direction} not known")
# trial continues for 3 sec after dynup completes
if self.real_robot:
input("Please hold robot in the air and then press any key")
else:
# reset simulation
self.sim.set_gravity(False)
self.sim.set_self_collision(False)
self.sim.reset_robot_pose((0, 0, 1), (0, 0, 0, 1))
time = self.get_time()
while self.get_time() - time < 10:
msg = JointCommand()
if self.robot == "wolfgang":
msg.joint_names = [
"HeadPan", "HeadTilt", "LElbow", "LShoulderPitch",
"LShoulderRoll", "RElbow", "RShoulderPitch",
"RShoulderRoll", "LHipYaw", "LHipRoll", "LHipPitch",
"LKnee", "LAnklePitch", "LAnkleRoll", "RHipYaw",
"RHipRoll", "RHipPitch", "RKnee", "RAnklePitch",
"RAnkleRoll"
]
if self.direction == "back":
msg.positions = [
0, 0, 0.82, 0.89, 0, -0.82, -0.89, 0, -0.01, 0.06,
0.47, 1.01, -0.45, 0.06, 0.01, -0.06, -0.47, -1.01,
0.45, -0.06
] # walkready
elif self.direction == "front":
msg.positions = [
0, 0.78, 0.78, 1.36, 0, -0.78, -1.36, 0, 0.11, 0.07,
-0.19, 0.23, -0.63, 0.07, 0.11, -0.07, 0.19, -0.23,
0.63, -0.07
] # falling_front
elif self.robot == "robotis_op2":
msg.joint_names = [
"head_pan", "head_tilt", "LElbow", "l_sho_pitch",
"l_sho_roll", "RElbow", "r_sho_pitch", "r_sho_roll",
"LHipYaw", "l_hip_roll", "l_hip_pitch", "l_knee",
"l_ank_pitch", "l_ank_roll", "r_hip_yaw", "r_hip_roll",
"r_hip_pitch", "r_knee", "r_ank_pitch", "RAnkleRoll"
]
if self.direction == "back":
msg.positions = [
0, 0, 0.79, 0, 0, -0.79, 0, 0, -0.01, 0.06, 0.47, 1.01,
-0.45, 0.06, 0.01, -0.06, -0.47, -1.01, 0.45, -0.06
] # walkready
elif self.direction == "front":
msg.positions = [
0, 0.78, 0.78, 1.36, 0, -0.78, -1.36, 0, 0.11, 0.07,
-0.19, 0.23, -0.63, 0.07, 0.11, -0.07, 0.19, -0.23,
0.63, -0.07
] # falling_front
elif self.robot == "sigmaban":
msg.joint_names = [
"head_yaw", "head_pitch", "LElbow", "left_shoulder_pitch",
"left_shoulder_roll", "RElbow", "right_shoulder_pitch",
"right_shoulder_roll", "left_hip_yaw", "left_hip_roll",
"left_hip_pitch", "left_knee", "left_ankle_pitch",
"left_ankle_roll", "right_hip_yaw", "right_hip_roll",
"right_hip_pitch", "right_knee", "right_ankle_pitch",
"right_ankle_roll"
]
if self.direction == "back":
msg.positions = [
0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0, 0.0, 0
] # walkready
elif self.direction == "front":
msg.positions = [
0, 0.0, 0.0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0, 0.0,
0.0, 0, 0, 0, 0, 0
] # falling_front
self.dynamixel_controller_pub.publish(msg)
if not self.real_robot:
self.sim.step_sim()
if self.real_robot:
input("Please put the robot on the ground and then press any key")
else:
self.reset_position()
self.sim.set_gravity(True)
self.sim.set_self_collision(True)
time = self.get_time()
while not self.get_time() - time > 2:
self.sim.step_sim()
class PredefinedCommands:
__ids__ = [
"HeadPan",
"HeadTilt",
"LShoulderPitch",
"LShoulderRoll",
"LElbow",
"RShoulderPitch",
"RShoulderRoll",
"RElbow",
"LHipYaw",
"LHipRoll",
"LHipPitch",
"LKnee",
"LAnklePitch",
"LAnkleRoll",
"RHipYaw",
"RHipRoll",
"RHipPitch",
"RKnee",
"RAnklePitch",
"RAnkleRoll"
]
__velocity__ = 5.0
__accelerations__ = -1.0
__max_currents__ = -1.0
Zero = JointCommand(
joint_names=__ids__,
velocities=[__velocity__] * len(__ids__),
accelerations=[__accelerations__] * len(__ids__),
max_currents=[__max_currents__] * len(__ids__),
positions=[0.0] * len(__ids__))
Walkready = JointCommand(
joint_names=__ids__,
velocities=[__velocity__] * len(__ids__),
accelerations=[__accelerations__] * len(__ids__),
max_currents=[__max_currents__] * len(__ids__),
positions=[
0.0, # HeadPan
0.0, # HeadTilt
0.0, # LShoulderPitch
0.0, # LShoulderRoll
0.79, # LElbow
0.0, # RShoulderPitch
0.0, # RShoulderRoll
-0.79, # RElbow
-0.0112, # LHipYaw
0.0615, # LHipRoll
0.4732, # LHipPitch
1.0058, # LKnee
-0.4512, # LAnklePitch
0.0625, # LAnkleRoll
0.0112, # RHipYaw
-0.0615, # RHipRoll
-0.4732, # RHipPitch
-1.0059, # RKnee
0.4512, # RAnklePitch
-0.0625, # RAnkleRoll
])