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 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 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 = [ "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 = [ "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 = [ "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 = [ "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)
"LHipRoll": 9, "LHipPitch": 10, "LKnee": 11, "LAnklePitch": 12, "LAnkleRoll": 13, "RHipYaw": 14, "RHipRoll": 15, "RHipPitch": 16, "RKnee": 17, "RAnklePitch": 18, "RAnkleRoll": 19 } rospy.init_node("send_joint_command") pos_msg = JointCommand() pos_msg.joint_names = ids.keys() #pos_msg.joint_names = ["RShoulderRoll"] pos_msg.velocities = [-1.0] * 20 pos_msg.positions = [0.0] * 20 pos_msg.accelerations = [-1.0] * 20 pos_msg.max_currents = [-1.0] * 20 pub = rospy.Publisher("/DynamixelController/command", JointCommand, queue_size=1) print(pos_msg) while not rospy.is_shutdown(): pos_msg.header.stamp = rospy.Time.now() pub.publish(pos_msg) rospy.sleep(0.1)
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: ')) y = float(input('y: ')) point = PointStamped()
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()
# Head Part rospy.Subscriber("joint_states", JointState, joint_state_cb, queue_size=1) if rospy.get_param("sim_active", default=False): head_pub = rospy.Publisher("head_motor_goals", JointCommand, queue_size=1) else: head_pub = rospy.Publisher("DynamixelController/command", JointCommand, queue_size=1) head_msg = JointCommand() head_msg.max_currents = [-1] * 2 head_msg.velocities = [5] * 2 head_msg.accelerations = [40] * 2 head_msg.joint_names = ['HeadPan', 'HeadTilt'] head_msg.positions = [0] * 2 head_pan_step = 0.05 head_tilt_step = 0.05 try: print(msg) while (1): key = getKey() if key in moveBindings.keys(): x += moveBindings[key][0] * x_speed_step x = round(x, 2) y += moveBindings[key][1] * y_speed_step y = round(y, 2) th += moveBindings[key][2] * turn_speed_step
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)
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()