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 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 = [ "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 = [ "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 = [ "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 = [ "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 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 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 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 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 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 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 __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 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 __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]
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)
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)
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 ])
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 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)
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 __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 __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 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()
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)