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 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)
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() point.header.stamp = rospy.Time.now() point.header.frame_id = 'base_footprint' point.point.x = x
y = 0 th = 0 status = 0 # 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
"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)