def joint_state_publisher(self): try: joint_state_msg = JointState() joint_fb_msg = FollowJointTrajectoryFeedback() time = rospy.Time.now() with self.lock: #Joint states joint_state_msg.header.stamp = time joint_state_msg.name = self.joint_names joint_state_msg.position = self.motion_ctrl.get_joint_positions() #self.joint_state_pub.publish(joint_state_msg) #Joint feedback joint_fb_msg.header.stamp = time joint_fb_msg.joint_names = self.joint_names joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions() #self.joint_feedback_pub.publish(joint_fb_msg) except Exception as e: rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def feedback_states_publisher(rate): global current_joints global current_velocity publisher = rospy.Publisher('feedback_states', FollowJointTrajectoryFeedback, queue_size = conf_rate) sleeper = rospy.Rate(rate) while not rospy.is_shutdown(): msg = FollowJointTrajectoryFeedback() msg.header.stamp = rospy.Time.now() msg.header.frame_id = conf_base_name msg.joint_names = conf_joint_names msg.actual = JointTrajectoryPoint() # ----------------------------------------------------------- # Critical Section # ----------------------------------------------------------- with lock: msg.actual.positions = current_joints msg.actual.velocities = current_velocity msg.actual.effort = current_effort msg.actual.time_from_start = rospy.get_rostime() - trajectory_start # ----------------------------------------------------------- publisher.publish(msg) sleeper.sleep()
def _state_publisher(self): try: joint_state_msg = JointState() joint_fb_msg = FollowJointTrajectoryFeedback() with self._lock: time = rospy.Time.now() q_actual, q_desired, q_error = self._rsi_connection.get_joint_states( ) #Joint states joint_state_msg.header.stamp = time joint_state_msg.name = self._joint_names joint_state_msg.position = q_actual self._joint_state_pub.publish(joint_state_msg) #Joint feedback joint_fb_msg.header.stamp = time joint_fb_msg.joint_names = self._joint_names joint_fb_msg.actual.positions = q_actual joint_fb_msg.desired.positions = q_desired joint_fb_msg.error.positions = q_error self._joint_feedback_pub.publish(joint_fb_msg) except Exception as e: rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def joint_state_publisher(self): joint_state_msg = JointState() joint_state_msg.header.stamp = rospy.Time.now() joint_state_msg.name = self.ABB_joint_name_list + self.gripper3F_joint_name_list joint_state_msg.position = list(self.ABB_joint_states_list) + list( self.gripper3F_joint_states_list) joint_state_msg.velocity = [] joint_state_msg.effort = [] control_state_msg = FollowJointTrajectoryFeedback() control_state_msg.header.stamp = rospy.Time.now() control_state_msg.joint_names = self.ABB_joint_name_list + self.gripper3F_joint_name_list control_state_msg.actual.positions = list( self.ABB_feedback_states_list) + list( self.gripper3F_feedback_states_list) control_state_msg.desired.positions = list( self.ABB_feedback_states_list) + list( self.gripper3F_feedback_states_list) control_state_msg.error.positions = [ 0 for i in control_state_msg.joint_names ] self.joint_state_pub.publish(joint_state_msg) self.control_state_pub.publish(control_state_msg)
def joint_state_publisher(self): if self.EnableFlag == 1 and self.motion_ctrl.positionUpdatedFlag == '1': try: joint_state_msg = JointState() joint_fb_msg = FollowJointTrajectoryFeedback() time = rospy.Time.now() with self.lock: #Joint states joint_state_msg.header.stamp = time joint_state_msg.name = self.joint_names joint_state_msg.position = self.motion_ctrl.get_joint_positions( ) # self.joint_state_pub.publish(joint_state_msg) #Joint feedback joint_fb_msg.header.stamp = time joint_fb_msg.joint_names = self.joint_names joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions( ) self.joint_feedback_pub.publish(joint_fb_msg) except Exception as e: rospy.loginfo( 'Unexpected exception in joint state publisher: %s', e)
def execute_cb(self, goal): rospy.loginfo("Received new goal:\n%s"%goal) success = True start_time = rospy.Time.now() i = 1 previouspoint = None for point in goal.trajectory.points: time_to_wait = start_time + point.time_from_start - rospy.Time.now() #previouspoint=point if time_to_wait > rospy.Duration(0): #rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec()) rospy.loginfo("Sleeping for 0.3 second") rospy.sleep(0.3) # only publish feedback if we have received some position recently if previouspoint and self.latestpositions: fb = FollowJointTrajectoryFeedback() fb.desired = previouspoint fb.actual = JointTrajectoryPoint() fb.actual.positions = self.latestpositions fb.error = JointTrajectoryPoint() fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions) self.action_server.publish_feedback(fb) # only use positions and velocities (velocities are always positive) point.velocities = map(abs, point.velocities) point.accelerations = [] point.effort = [] rospy.loginfo("Go to point %d:\n%s"%(i, point)) serial_command=Float64MultiArray() previous_position=Float64MultiArray() #serial_command.data= tuple(numpy.subtract(point.positions,previouspoint.positions)) serial_command.data=point.positions self.pub_command.publish(serial_command) self.latestpositions = None self.pub_trajpoint.publish(point) previouspoint = point i += 1 if success: rospy.loginfo('%s: Succeeded' % self.action_name) self.result = FollowJointTrajectoryResult() self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL self.action_server.set_succeeded(self.result) self.traj_complete.publish(1)
def read_joint_state_data(self, msg): joint_fb_msg = FollowJointTrajectoryFeedback() joint_fb_msg.header.stamp = rospy.Time.now() joint_fb_msg.joint_names = msg.name joint_fb_msg.actual.positions = msg.position joint_fb_msg.actual.velocities = msg.velocity self.joint_feedback_pub.publish(joint_fb_msg)
def on_goal(self, goal_handle): """Handle a new goal trajectory command.""" # Checks if the joints are just incorrect if set(goal_handle.get_goal().trajectory.joint_names) != set( self.prefixedJointNames): rospy.logerr( "Received a goal with incorrect joint names: (%s)" % ', '.join(goal_handle.get_goal().trajectory.joint_names)) goal_handle.set_rejected() return if not trajectory_is_finite(goal_handle.get_goal().trajectory): rospy.logerr("Received a goal with infinites or NaNs") goal_handle.set_rejected( text="Received a goal with infinites or NaNs") return # Checks that the trajectory has velocities if not has_velocities(goal_handle.get_goal().trajectory): rospy.logerr("Received a goal without velocities") goal_handle.set_rejected(text="Received a goal without velocities") return feedback = FollowJointTrajectoryFeedback() result = FollowJointTrajectoryResult() feedback.joint_names = goal_handle.get_goal().trajectory.joint_names # Orders the joints of the trajectory according to joint_names reorder_trajectory_joints(goal_handle.get_goal().trajectory, self.prefixedJointNames) # Inserts the current setpoint at the head of the trajectory now = self.robot.getTime() print 'trajectory_t0: {}'.format(self.trajectory_t0) point0 = sample_trajectory(self.trajectory, now - self.trajectory_t0) point0.time_from_start = rospy.Duration(0.0) goal_handle.get_goal().trajectory.points.insert(0, point0) feedback.desired.positions = point0 self.trajectory_t0 = now print "i am here" print 'point0: {}'.format(point0.positions) # Replaces the goal self.goal_handle = goal_handle self.trajectory = goal_handle.get_goal().trajectory self.feedback = feedback self.result = result goal_handle.set_accepted()
def __init__(self, limb, reconfig_server, rate=100.0): self._dyn = reconfig_server self._ns = 'robot/limb/' + limb + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() self._limb = baxter_interface.Limb(limb) # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments and dynamic reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._goal_error = dict() self._error_threshold = dict() self._dflt_vel = dict() # Create our PID controllers self._pid = dict() for joint in self._limb.joint_names(): self._pid[joint] = baxter_control.PID() # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) self._pub_rate.publish(self._control_rate)
def initialize(self): # subscribe to joint states to check if the arm is moving, based on velocities of each joint rospy.Subscriber("/joint_states", JointState, self.jointStatesCallback, queue_size=1) ns = self.controller_namespace + '/joint_trajectory_action_node/constraints' self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0) self.stopped_velocity_tolerance = rospy.get_param(ns + '/stopped_velocity_tolerance', 0.01) self.goal_constraints = [] self.trajectory_constraints = [] self.min_velocity = rospy.get_param(self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1) for joint in self.joint_names: self.goal_constraints.append(rospy.get_param(ns + '/' + joint + '/goal', -1.0)) self.trajectory_constraints.append(rospy.get_param(ns + '/' + joint + '/trajectory', -1.0)) # Message containing current state for all controlled joints self.msg = FollowJointTrajectoryFeedback() self.msg.joint_names = self.joint_names self.msg.desired.positions = [0.0] * self.num_joints self.msg.desired.velocities = [0.0] * self.num_joints self.msg.desired.accelerations = [0.0] * self.num_joints self.msg.actual.positions = [0.0] * self.num_joints self.msg.actual.velocities = [0.0] * self.num_joints self.msg.error.positions = [0.0] * self.num_joints self.msg.error.velocities = [0.0] * self.num_joints return True
def __init__(self, limb, reconfig_server, rate=100.0, mode='position_w_id', interpolation='minjerk'): self._dyn = reconfig_server self._ns = 'robot/limb/' + limb self._fjt_ns = self._ns + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._limb = intera_interface.Limb(limb, synchronous_pub=True) self._enable = intera_interface.RobotEnable() self._name = limb self._interpolation = interpolation self._cuff = intera_interface.Cuff(limb=limb) # Verify joint control mode self._mode = mode if (self._mode != 'position' and self._mode != 'position_w_id' and self._mode != 'velocity'): rospy.logerr("%s: Action Server Creation Failed - " "Provided Invalid Joint Control Mode '%s' (Options: " "'position_w_id', 'position', 'velocity')" % ( self._action_name, self._mode, )) return self._server.start() self._alive = True # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic # reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() # Create our PID controllers self._pid = dict() for joint in self._limb.joint_names(): self._pid[joint] = intera_control.PID() # Create our spline coefficients self._coeff = [None] * len(self._limb.joint_names()) # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate', UInt16, queue_size=10) self._pub_rate.publish(self._control_rate)
def initialize(self): ns = self.controller_namespace + '/joint_trajectory_action_node/constraints' self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0) self.stopped_velocity_tolerance = rospy.get_param( ns + '/stopped_velocity_tolerance', 0.01) self.goal_constraints = [] self.trajectory_constraints = [] self.min_velocity = rospy.get_param( self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1) # Topic name for publish joint_states self.joint_states_topic = rospy.get_param( self.controller_namespace + '/joint_trajectory_action_node/joint_states_topic', '/joint_states') for joint in self.joint_names: self.goal_constraints.append( rospy.get_param(ns + '/' + joint + '/goal', -1.0)) self.trajectory_constraints.append( rospy.get_param(ns + '/' + joint + '/trajectory', -1.0)) # Message containing current state for all controlled joints self.msg = FollowJointTrajectoryFeedback() self.msg.joint_names = self.joint_names self.msg.desired.positions = [0.0] * self.num_joints self.msg.desired.velocities = [0.0] * self.num_joints self.msg.desired.accelerations = [0.0] * self.num_joints self.msg.actual.positions = [0.0] * self.num_joints self.msg.actual.velocities = [0.0] * self.num_joints self.msg.error.positions = [0.0] * self.num_joints self.msg.error.velocities = [0.0] * self.num_joints return True
def state_msg(self, ref_state, type_msg): if type_msg == 'state': st = JointTrajectoryControllerState() elif type_msg == 'feedback': st = FollowJointTrajectoryFeedback() st.joint_names = [self.gripper_joint_name] st.header = ref_state.header st.desired.positions.extend( [p * 2.0 for p in ref_state.desired.positions[:1]]) st.desired.velocities.extend(ref_state.desired.velocities[:1]) st.desired.accelerations.extend(ref_state.desired.accelerations[:1]) st.desired.effort.extend(ref_state.desired.effort[:1]) st.desired.time_from_start = ref_state.desired.time_from_start st.actual.positions.extend( [p * 2.0 for p in ref_state.actual.positions[:1]]) st.actual.velocities.extend(ref_state.actual.velocities[:1]) st.actual.accelerations.extend(ref_state.actual.accelerations[:1]) st.actual.effort.extend(ref_state.actual.effort[:1]) st.actual.time_from_start = ref_state.actual.time_from_start st.error.positions.extend( [p * 2.0 for p in ref_state.error.positions[:1]]) st.error.velocities.extend(ref_state.error.velocities[:1]) st.error.accelerations.extend(ref_state.error.accelerations[:1]) st.error.effort.extend(ref_state.error.effort[:1]) return st
def __init__(self, reconfig_server, rate=100.0, interpolation='minjerk'): self._dyn = reconfig_server self._fjt_ns = "position_joint_trajectory_controller/follow_joint_trajectory" self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._arm = franka_interface.ArmInterface(synchronous_pub=True) self._enable = franka_interface.RobotEnable() self._interpolation = interpolation # Verify joint control mode self._server.start() self._alive = True # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic # reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._stopped_velocity = 0.0001 self._goal_error = dict() self._path_thresh = dict() # Create our PID controllers self._pid = dict() for joint in self._arm.joint_names(): self._pid[joint] = pid.PID() # Create our spline coefficients self._coeff = [None] * len(self._arm.joint_names())
def __init__(self, topic_in): pub_topic = rospy.get_param(node_name + '/pub_topic', '/feedback_states') rospy.Subscriber(topic_in, JointState, self._callBack) self.joint_trajectory_pub = rospy.Publisher( pub_topic, FollowJointTrajectoryFeedback, queue_size=1) self.follow_joint_trajectory_feedback_ = FollowJointTrajectoryFeedback( )
def joint_state_publisher(self): LOG.info( "QWERQWERQWERQWEQWRQWERQWERWQERQWERQWERQWERQWERQWERERQWQREWQWREQWERQWER!" ) print('comecomecomecomecomecomecomecomecomecomecome') joint_state_msg = JointState() joint_state_msg.header.stamp = rospy.Time.now() if self.robot_name == 'NRMK-IndyRP2': joint_state_msg.name = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6' ] else: joint_state_msg.name = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5' ] cur_joint_pos = self.indy.get_joint_pos() joint_state_msg.position = utils_transf.degs2rads(cur_joint_pos) joint_state_msg.velocity = [self.vel] joint_state_msg.effort = [] control_state_msg = FollowJointTrajectoryFeedback() control_state_msg.header.stamp = rospy.Time.now() if self.robot_name == 'NRMK-IndyRP2': control_state_msg.joint_names = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6' ] else: control_state_msg.joint_names = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5' ] control_state_msg.actual.positions = utils_transf.degs2rads( cur_joint_pos) control_state_msg.desired.positions = utils_transf.degs2rads( cur_joint_pos) control_state_msg.error.positions = [ 0 for i in control_state_msg.joint_names ] self.joint_state_pub.publish(joint_state_msg) self.control_state_pub.publish(control_state_msg)
def __init__(self, node): self.node = node self.server = actionlib.SimpleActionServer( '/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False) self.feedback = FollowJointTrajectoryFeedback() self.result = FollowJointTrajectoryResult() r = self.node.robot head_pan_range_ticks = r.head.motors['head_pan'].params['range_t'] head_pan_range_rad = (r.head.motors['head_pan'].ticks_to_world_rad( head_pan_range_ticks[1]), r.head.motors['head_pan'].ticks_to_world_rad( head_pan_range_ticks[0])) head_tilt_range_ticks = r.head.motors['head_tilt'].params['range_t'] head_tilt_range_rad = (r.head.motors['head_tilt'].ticks_to_world_rad( head_tilt_range_ticks[1]), r.head.motors['head_tilt'].ticks_to_world_rad( head_tilt_range_ticks[0])) wrist_yaw_range_ticks = r.end_of_arm.motors['wrist_yaw'].params[ 'range_t'] wrist_yaw_range_rad = ( r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad( wrist_yaw_range_ticks[1]), r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad( wrist_yaw_range_ticks[0])) gripper_range_ticks = r.end_of_arm.motors['stretch_gripper'].params[ 'range_t'] gripper_range_rad = ( r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad( gripper_range_ticks[0]), r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad( gripper_range_ticks[1])) gripper_range_robotis = ( r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct( gripper_range_rad[0]), r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct( gripper_range_rad[1])) self.head_pan_cg = HeadPanCommandGroup( head_pan_range_rad, self.node.head_pan_calibrated_offset_rad, self.node.head_pan_calibrated_looked_left_offset_rad) self.head_tilt_cg = HeadTiltCommandGroup( head_tilt_range_rad, self.node.head_tilt_calibrated_offset_rad, self.node.head_tilt_calibrated_looking_up_offset_rad, self.node.head_tilt_backlash_transition_angle_rad) self.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad) self.gripper_cg = GripperCommandGroup(gripper_range_robotis) self.telescoping_cg = TelescopingCommandGroup( tuple(r.arm.params['range_m']), self.node.wrist_extension_calibrated_retracted_offset_m) self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m'])) self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5))
def execute_cb_arm(self, goal): # create feedback msg self.arm_feedback = FollowJointTrajectoryFeedback() self.arm_feedback.joint_names = goal.trajectory.joint_names ## notice that the order in goal.trajectory.joint_names is different ## from the default order in self.joint_names!!!! #~ print goal.trajectory.joint_names print "arm" #~ print len(goal.trajectory.points) print goal.trajectory.points # we execute the trajectory at 5Hz, which means we execute 5 # points per second action_rate = rospy.Rate(5) ## for each point in the planned trajectory, we excute it through ## SPI and send feedback for trajectory_point in goal.trajectory.points: # the actual jointstate is same as the required jointstate self.arm_feedback.actual = trajectory_point # we send the feedback infomation back to the client self._as_arm.publish_feedback(self.arm_feedback) # send the jointstates through SPI in the correct order !!!! for i in range(len(self.jointStates) - 1): self.jointStates[i] = trajectory_point.positions[ goal.trajectory.joint_names.index(self.joint_names[i])] print self.jointStates ########################## IMPORTANT ####################### ### add your own linear mapping between self.jointStates and self.jointPWMs ### self.jointPWWs = f(self.jointStates) self.jointPWMs = np.array(self.k) * np.array( self.jointStates) + np.array(self.b) print self.jointPWMs ############################################################ # we publish the updated joint states, with the correct time stamps self.joint_state_msg.header.stamp = rospy.Time.now() self.joint_state_msg.position = self.jointStates self.pub_state.publish(self.joint_state_msg) self.joint_PWM_msg.position = self.jointPWMs self.pub_PWM.publish(self.joint_PWM_msg) # send PWMs to SPI # uncomment this if you are using real hardware and sending # data to Arduino through SPI self.spi_send_PWM() action_rate.sleep() # After executing every trajectory point sent by the client # we notify the client that the action is being completed. self.arm_result = FollowJointTrajectoryResult() self.arm_result.error_code = 0 # error code 0 means successful self._as_arm.set_succeeded(self.arm_result)
def tr2_arm_follow_joint_trajectory(goal): global tr2_arm_action_server, tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state, tr2_a4_state, tr2_a0_pub, tr2_a1_pub, tr2_a2_pub, tr2_a3_pub, tr2_a4_pub, tr2_mode_servo_pub, tr2_stop_pub tr2_mode_servo_pub.publish(1) tr2_stop_pub.publish(0) time.sleep(0.050) success = True feedback = FollowJointTrajectoryFeedback() result = FollowJointTrajectoryResult() joint_names = goal.trajectory.joint_names feedback.joint_names = joint_names print len(goal.trajectory.points) t_start = datetime.datetime.now() for point in goal.trajectory.points: tr2_a0_pub.publish(point.positions[0]) tr2_a1_pub.publish(point.positions[1]) tr2_a2_pub.publish(point.positions[2]) tr2_a3_pub.publish(point.positions[3]) tr2_a4_pub.publish(point.positions[4]) '''if tr2_arm_action_server.is_preempt_requested(): tr2_arm_action_server.set_preempted() success = False break''' while (datetime.datetime.now() - t_start ).total_seconds() < point.time_from_start.nsecs / 1000000000.0: feedback.desired = point feedback.actual.positions = [ tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state, tr2_a4_state ] #feedback.error.positions tr2_arm_action_server.publish_feedback(feedback) tr2_arm_action_server.set_succeeded(result) time.sleep(5) tr2_stop_pub.publish(1)
def execute_cb(self, goal): rospy.loginfo("Received new goal:\n%s"%goal) success = True start_time = rospy.Time.now() i = 1 previouspoint = None for point in goal.trajectory.points: time_to_wait = start_time + point.time_from_start - rospy.Time.now() if time_to_wait > rospy.Duration(0): rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec()) rospy.sleep(time_to_wait) # only publish feedback if we have received some position recently if previouspoint and self.latestpositions: fb = FollowJointTrajectoryFeedback() fb.desired = previouspoint fb.actual = JointTrajectoryPoint() fb.actual.positions = self.latestpositions fb.error = JointTrajectoryPoint() fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions) self.action_server.publish_feedback(fb) # only use positions and velocities (velocities are always positive) point.velocities = map(abs, point.velocities) point.accelerations = [] point.effort = [] rospy.loginfo("Go to point %d:\n%s"%(i, point)) self.latestpositions = None self.pub_trajpoint.publish(point) previouspoint = point i += 1 if success: rospy.loginfo('%s: Succeeded' % self.action_name) self.result = FollowJointTrajectoryResult() self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL self.action_server.set_succeeded(self.result)
def __init__(self, controller_name): self._action_ns = controller_name + '/follow_joint_trajectory' self._as = actionlib.SimpleActionServer(self._action_ns, FollowJointTrajectoryAction, execute_cb=self._execute_cb, auto_start=False) self._action_name = rospy.get_name() self._as.start() self._feedback = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() self._rate = rospy.Rate(20) rospy.loginfo('Successful init')
def __init__(self, reconfig_server, rate=100.0): self._dyn = reconfig_server self.continuous = self._dyn.config['continuous'] self._fjt = '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() # Current joint states self._positions = {} self._velocities = {} # Actual robot control self.states = EdoStates() rospy.Subscriber("joint_states", JointState, self._js_callback) # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic reconfigure self._control_rate = rate # Hz self._update_rate_spinner = rospy.Rate(self._control_rate) self._control_joints = [] self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() # Start the action server rospy.sleep(0.5) while True: if self.states.edo_current_state in [ self.states.CS_CALIBRATED, self.states.CS_BRAKED ]: self._server.start() self._alive = True rospy.loginfo("Trayectory action server started") break else: rospy.logerr( "Joint Trajectory Action Server cannot be started when robot is in state %s" % self.states.get_current_code_string()) rospy.logerr( "Make sure your robot is started and calibrated properly with calibrate.launch" ) rospy.logerr("Trying again in 1 second... ") rospy.sleep(1)
def __init__(self, limb, reconfig_server, rate=100.0, mode='joint_impedance', sim = False, interpolation='minjerk'): self._mode = mode self._dyn = reconfig_server self._ns = limb + '/joint_trajectory_controller' self._fjt_ns = self._ns + '/follow_joint_trajectory2' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._limb = BorisRobot() self._name = limb self._interpolation = interpolation self._server.start() self._alive = True # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic # reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() self._limb.set_control_mode(mode=self._mode) if sim: rospy.loginfo("Setting gains for simulated robot") self._limb.set_joint_impedance(np.array([2000,2000,2000,2000,2000,2000,2000]),np.array([100,100,100,100,100,100,100])) # Create our spline coefficients self._coeff = [None] * len(self._limb.joint_names(self._name)) # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16, queue_size=10) self._pub_rate.publish(self._control_rate)
def _set_current_joint_angles(self, joint_angles): self._current_joint_angles = joint_angles if self._trajectory_valid: #If the specified trajectory is too far away from current joint angles, abort it trajectory_angles = self._get_trajectory_joint_angles( self._trajectory_t) #TODO: improve trajectory error tracking if (np.any( np.abs(trajectory_angles - joint_angles) > np.deg2rad(45))): rospy.logerr("Trajectory aborted due to tracking error: %s", str(np.rad2deg(trajectory_angles - joint_angles))) self._abort_trajectory() else: fb = FollowJointTrajectoryFeedback() fb.header.stamp = rospy.Time.now() fb.desired.positions = self._get_trajectory_joint_angles( self._trajectory_max_t) fb.desired.time_from_start = rospy.Duration( secs=self._trajectory_t) fb.actual = fb.desired gh = self._trajectory_gh gh.publish_feedback(fb)
def __init__(self, rate=100.0): self._alive = False self.init_success = False self._action_name = rospy.get_name() # Action Feedback/Result """ Define the joint names """ self._joint_names = ['linear_joint'] """ Controller parameters from arguments, messages, and dynamic reconfigure """ self._trajectory_control_rate = rate # Hz self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() self._estop_delay = 0 self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() #self._dyn = reconfig_server self._ns = '/movo/torso_controller' self._fjt_ns = self._ns + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._alive = True self.estop = False self._movo_status_sub = rospy.Subscriber("/movo/feedback/status", Status, self._update_movo_status) self._js = rospy.wait_for_message("/movo/linear_actuator/joint_states", JointState) self.pos_targets = self._get_current_position(self._joint_names) self._sub = rospy.Subscriber("/movo/linear_actuator/joint_states", JointState, self._update_movo_joint_states) self._cmd_pub = rospy.Publisher("/movo/linear_actuator_cmd", LinearActuatorCmd, queue_size=10) self._server.start() self.init_success = True
def __init__(self, arm, serial_number, rate=100.0): self._alive = False self.init_success = True self._action_name = rospy.get_name() self._name = arm # Action Feedback/Result """ Define the joint names """ self._joint_names = ['%s_shoulder_pan_joint'%arm, '%s_shoulder_lift_joint'%arm, '%s_elbow_joint'%arm, '%s_wrist_1_joint'%arm, '%s_wrist_2_joint'%arm, '%s_wrist_3_joint'%arm] """ Controller parameters from arguments, messages, and dynamic reconfigure """ self._trajectory_control_rate = rate # Hz self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() self._traj_smoother = TrajectorySmoother(rospy.get_name(),arm) self._ctl = SIArmController(self._joint_names,arm,serial_number) if not self._ctl.init_success: rospy.logerr("Failed to initialize controller, make sure the serial number exists") self.clean_shutdown() self.init_success = False return self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() #self._dyn = reconfig_server self._ns = '/vector/%s_arm'%arm self._fjt_ns = self._ns + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._alive = True self._server.start()
def initialize(self): ns = self.controller_namespace + '/joint_trajectory_action_node/constraints' # rospy.get_param will try to fetch the information from the parameter server, # but if the parameter is not available, it will use the default value. # let motors roll for specified amount of time self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0) self.goal_constraints = [] self.trajectory_constraints = [] # The controller will look for goals / trajectory contrains on the parameter server # The values should be set on top of the controller namespace + '/joint_trajectory_action_node/constraints' for joint in self.joint_names: self.goal_constraints.append( rospy.get_param(ns + '/' + joint + '/goal', 0.1)) #0.04 self.trajectory_constraints.append( rospy.get_param(ns + '/' + joint + '/trajectory', 1.5)) rospy.loginfo("Goal constrains: " + ",".join([ ji + "=" + str(ci) for ji, ci in zip(self.joint_names, self.goal_constraints) ])) rospy.loginfo("Trajectory constrains: " + ",".join([ ji + "=" + str(ci) for ji, ci in zip(self.joint_names, self.trajectory_constraints) ])) # Message containing current state for all controlled joints # control_msgs/FollowJointTrajectoryFeedback self.feedback = FollowJointTrajectoryFeedback() self.feedback.joint_names = self.joint_names self.feedback.desired.positions = [0.0] * self.num_joints self.feedback.desired.velocities = [0.0] * self.num_joints self.feedback.desired.accelerations = [0.0] * self.num_joints self.feedback.actual.positions = [0.0] * self.num_joints self.feedback.actual.velocities = [0.0] * self.num_joints self.feedback.error.positions = [0.0] * self.num_joints self.feedback.error.velocities = [0.0] * self.num_joints self.running = dict() return 0
def send_goal(self, goal): # send goals to all controllers for controller in self.controllers: controller.send_goal(goal) # publish feedback until they are done done_goals = [False] * len(self.controllers) while not rospy.is_shutdown() and not all(done_goals): # construct feedback message curr_fed = FollowJointTrajectoryFeedback() for cidx, controller in enumerate(self.controllers): if controller.is_goal_done(): done_goals[cidx] = True # add to feedback message this controller stuff fed = controller.get_feedback() # curr_fed add stuff # compose global result and publish for controller in self.controllers: res = controller.get_result()
def __init__(self, name): ''' \brief Constructor for class moveit_action \param[in] name Name of subscribed topic. ''' # Action variables ## Action service result self.ac_result = FollowJointTrajectoryResult() ## Action service feedback self.ac_feedback = FollowJointTrajectoryFeedback() # Action Initialization ## Topic name self.action_name = name ## Action server self.action_server = actionlib.SimpleActionServer( self.action_name, FollowJointTrajectoryAction, execute_cb=self.action_callback, auto_start=False) self.action_server.start()
def __init__(self, name): self._feedback = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() # Getting publishers for all joints self._poppy = rospy.get_param("/poppy") self._joints_pub = {} for k, d in self._poppy.iteritems(): if k != 'joint_state_controller': #just ignore this one self._joints_pub[k.replace('_position_controller', '')] = rospy.Publisher( "/poppy/" + k + "/command", Float64) rospy.loginfo('%s: is ready' % self._action_name)
class MoveItAction(object): _feedback = FollowJointTrajectoryFeedback() _result = FollowJointTrajectoryResult() def __init__(self, name): self.publisher = rospy.Publisher("joint_path_command", JointTrajectory) self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() def execute_cb(self, goal): self.publisher.publish(goal.trajectory) try: rospy.sleep(goal.trajectory.points[-1].time_from_start) self._result.error_code = 0 except: self._result.error_code = self._result.OLD_HEADER_TIMESTAMP self._as.set_succeeded(self._result)
def do_trajectory(goal): joint_names = goal.trajectory.joint_names trajectory_points = goal.trajectory.points print("GOAL TRAJECTORY:") print(goal.trajectory) # publish normally??? left_arm_pub = rospy.Publisher('r2d2_left_arm_controller/command', JointTrajectory, queue_size=10) left_arm_pub.publish(goal.trajectory) # print("Published") start_time = time.time() print(time.time() - start_time) while (time.time() - start_time < 10): print(time.time() - start_time) feedback = FollowJointTrajectoryFeedback() feedback.desired = JointTrajectoryPoint() feedback.desired = goal.trajectory.points[0] # get actual positions from joint state or robot publisher ... feedback.actual = JointTrajectoryPoint() feedback.actual.positions = [0, 0, 0] feedback.error = JointTrajectoryPoint() feedback.error.positions = numpy.subtract(feedback.desired.positions, feedback.actual.positions) server.publish_feedback(feedback) time.sleep(1.0) result = FollowJointTrajectoryResult() server.set_succeeded(result, "Trajectory completed successfully") print(result.SUCCESSFUL)
class JointContoller(): _feedback = FollowJointTrajectoryFeedback() _result = FollowJointTrajectoryResult() def __init__(self): self.name = rospy.get_name() self.publisher = rospy.Publisher('joint_states', JointState, queue_size=10) # Set up action client self.action_server = actionlib.SimpleActionServer( '%s/follow_joint_trajectory' % self.name, FollowJointTrajectoryAction, self.do_action_callback, False) self.action_server.start() self.rate = rospy.Rate(10) # 10hz self.joint_state = JointState() self.joint_state.header = Header() self.joint_state.name = ['hip', 'shoulder', 'elbow', 'wrist'] self.joint_state.position = [ 1.57, 1.57, 1.57, 1.57 ] # will be updated in home() before publication in the loop self.joint_state.velocity = [] self.joint_state.effort = [] # Initialise the PCA9685 using the default address (0x40) and the bus self.pwm = Adafruit_PWM_Servo_Driver.PWM(address=0x40, busnum=1) self.pwm.setPWMFreq(cal.PWM_FREQUENCY) self.home() def do_action_callback(self, goal): print 'Received goal %d' % len(goal.trajectory.points[0].positions) timePassed = rospy.Duration(0) self.joint_state.name = goal.trajectory.joint_names for point in goal.trajectory.points: self.joint_state.header.stamp = rospy.Time.now() self.move_arm(goal.trajectory.joint_names, point.positions) self.publisher.publish(self.joint_state) rospy.sleep(point.time_from_start - timePassed) timePassed = point.time_from_start print('step') self._result.error_code = 0 self.action_server.set_succeeded(self._result) print('move complete') def get_joint_goal(self, joint_name, joint_list, goal_list): if joint_name in joint_list: index = joint_list.index(joint_name) return goal_list[index] print 'ERROR!! Could not find %s goal' % joint_name def set_joint_state(self, joint_name, pulse): index = self.joint_state.name.index(joint_name) print '%s at %d is %d (%0.2f)' % (joint_name, index, pulse, math.radians(pulse)) self.joint_state.position[index] = math.radians(pulse) def move_arm(self, names, goal): base = self.get_joint_goal('hip', names, goal) shoulder = self.get_joint_goal('shoulder', names, goal) elbow = self.get_joint_goal('elbow', names, goal) claw = self.get_joint_goal('wrist', names, goal) self.setJointAngles(base, shoulder, elbow, claw) def check_min_max(self, minVal, maxVal, test): if (test < minVal): test = minVal print "min %d" % (minVal) if (test > maxVal): test = maxVal print "min %d" % (maxVal) return test # Helper function to make setting a servo pulse width simpler. def set_servo_pulse(self, channel, pulse): pwm_val = 150 + int( pulse * 450.0 / 180.0) # magic incantation to make the timing work self.pwm.setPWM(channel, 0, pwm_val) def b(self, pulse): pulse = self.check_min_max(cal.BASE_MIN_PWM, cal.BASE_MAX_PWM, pulse) self.set_joint_state('hip', pulse) self.set_servo_pulse(cal.BASE_SERVO_CHANNEL, pulse) def s(self, pulse): pulse = self.check_min_max(cal.SHOULDER_MIN_PWM, cal.SHOULDER_MAX_PWM, pulse) self.set_joint_state('shoulder', pulse) self.set_servo_pulse(cal.SHOULDER_SERVO_CHANNEL, pulse) def e(self, pulse): pulse = self.check_min_max(cal.ELBOW_MIN_PWM, cal.ELBOW_MAX_PWM, pulse) self.set_joint_state('elbow', pulse) self.set_servo_pulse(cal.ELBOW_SERVO_CHANNEL, pulse) def c(self, pulse): pulse = self.check_min_max(cal.CLAW_MIN_PWM, cal.CLAW_MAX_PWM, pulse) self.set_joint_state('wrist', pulse) self.set_servo_pulse(cal.CLAW_SERVO_CHANNEL, pulse) def setJointAngles(self, base, shoulder, elbow, claw): self.b(int(math.degrees(base))) self.s(int(math.degrees(shoulder))) self.e(int(math.degrees(elbow))) self.c(int(math.degrees(claw))) def home(self): self.b(cal.BASE_HOME_PWM) self.s(cal.SHOULDER_HOME_PWM) self.e(cal.ELBOW_HOME_PWM) self.c(cal.CLAW_HOME_PWM) def off(self): self.pwm.setPWM(cal.BASE_SERVO_CHANNEL, 0, 0) self.pwm.setPWM(cal.SHOULDER_SERVO_CHANNEL, 0, 0) self.pwm.setPWM(cal.ELBOW_SERVO_CHANNEL, 0, 0) self.pwm.setPWM(cal.CLAW_SERVO_CHANNEL, 0, 0) def loop(self): # publish states regularly though this is also done on commands while not rospy.is_shutdown(): self.joint_state.header.stamp = rospy.Time.now() self.publisher.publish(self.joint_state) self.rate.sleep() self.off()