예제 #1
0
    def command_joint_position(self, desired_pose):
        """
        Command a specific desired hand pose.

        The desired pose must be the correct dimensionality (self._num_joints).
        Only the pose is commanded, and **no bound-checking happens here**:
        any commanded pose must be valid or Bad Things May Happen. (Generally,
        values between 0.0 and 1.5 are fine, but use this at your own risk.)

        :param desired_pose: The desired joint configurations.
        :return: True if pose is published, False otherwise.
        """

        # Check that the desired pose can have len() applied to it, and that
        # the number of dimensions is the same as the number of hand joints.
        if (not hasattr(desired_pose, '__len__') or
                len(desired_pose) != self._num_joints):
            rospy.logwarn('Desired pose must be a {}-d array: got {}.'
                          .format(self._num_joints, desired_pose))
            return False

        msg = JointState()  # Create and publish
        try:
            msg.position = desired_pose
            self.pub_joint.publish(msg)
            rospy.logdebug('Published desired pose.')
            return True
        except rospy.exceptions.ROSSerializationException:
            rospy.logwarn('Incorrect type for desired pose: {}.'.format(
                desired_pose))
            return False
예제 #2
0
 def gripper_cb(self, data):
     js = JointState()
     js.header = data.header
     js.name = ["Gripper"]
     js.position = [data.current_pos]
     js.velocity = [data.velocity]
     self.gripper_pub.publish(js)
예제 #3
0
def handstate_callback(handstate_msg):
    assert len(handstate_msg.positions) == 4
    assert len(handstate_msg.inner_links) == 3
    assert len(handstate_msg.outer_links) == 3

    jointstate_msg = JointState()
    jointstate_msg.header.stamp = handstate_msg.header.stamp
    jointstate_msg.name = [
        joint_prefix + '00',
        # Joint j01 is a mimic joint that is published by robot_state_publisher.
        joint_prefix + '01',
        joint_prefix + '11',
        joint_prefix + '21',
        # TODO: These are not currently being published. See below.
        #joint_prefix + '02',
        #joint_prefix + '12',
        #joint_prefix + '22',
    ]
    jointstate_msg.position += [ handstate_msg.positions[3] ]
    jointstate_msg.position += handstate_msg.inner_links

    # TODO: These positions look very wrong, so we'll let robot_state_publisher
    # use the mimic ratios in HERB's URDF to compute the distal joint angles. 
    #jointstate_msg.position += handstate_msg.outer_links

    jointstate_pub.publish(jointstate_msg)
예제 #4
0
    def command_joint_torques(self, desired_torques):
        """
        Command a desired torque for each joint.

        The desired torque must be the correct dimensionality
        (self._num_joints). Similarly to poses, we do not sanity-check
        the inputs. As a rule of thumb, values between +- 0.5 are fine.

        :param desired_torques: The desired joint torques.
        :return: True if message is published, False otherwise.
        """

        # Check that the desired torque vector can have len() applied to it,
        # and that the number of dimensions is the same as the number of
        # joints. This prevents passing singletons or incorrectly-shaped lists
        # to the message creation (which does no checking).
        if (not hasattr(desired_torques, '__len__') or
                len(desired_torques) != self._num_joints):
            rospy.logwarn('Desired torques must be a {}-d array: got {}.'
                          .format(self._num_joints, desired_torques))
            return False

        msg = JointState()  # Create and publish
        try:
            msg.effort = desired_torques
            self.pub_joint.publish(msg)
            rospy.logdebug('Published desired torques.')
            return True
        except rospy.exceptions.ROSSerializationException:
            rospy.logwarn('Incorrect type for desired torques: {}.'.format(
                desired_torques))
            return False
예제 #5
0
 def joint_trajectory_to_joint_state(self, joint_trajectory_msg, index):
     joint_states_msg = JointState()
     joint_states_msg.name = joint_trajectory_msg.joint_names
     joint_states_msg.position = joint_trajectory_msg.points[index].positions
     joint_states_msg.velocity = joint_trajectory_msg.points[index].velocities
     joint_states_msg.effort = joint_trajectory_msg.points[index].effort
     return joint_states_msg
예제 #6
0
파일: utils.py 프로젝트: crigroup/criros
def sorted_joint_state_msg(msg, joint_names):
  """
  Returns a sorted C{sensor_msgs/JointState} for the given joint names
  @type  msg: sensor_msgs/JointState
  @param msg: The input message
  @type  joint_names: list
  @param joint_names: The sorted joint names
  @rtype: sensor_msgs/JointState
  @return: The C{JointState} message with the fields in the order given by joint names
  """
  valid_names = set(joint_names).intersection(set(msg.name))
  valid_position = len(msg.name) == len(msg.position)
  valid_velocity = len(msg.name) == len(msg.velocity)
  valid_effort = len(msg.name) == len(msg.effort)
  num_joints = len(valid_names)
  retmsg = JointState()
  retmsg.header = copy.deepcopy(msg.header)
  for name in joint_names:
    if name not in valid_names:
      continue
    idx = msg.name.index(name)
    retmsg.name.append(name)
    if valid_position:
      retmsg.position.append(msg.position[idx])
    if valid_velocity:
      retmsg.velocity.append(msg.velocity[idx])
    if valid_effort:
      retmsg.effort.append(msg.effort[idx])
  return retmsg
    def execute(self, goal):
        #pickle.dump(goal, open("trajecoryGoalMsg3.data", "wb"))
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        num_points = len(trajectory_points)
        rospy.loginfo("Received %i trajectory points" % num_points)
        pprint(goal.trajectory)

        end_time = trajectory_points[-1].time_from_start.to_sec()
        control_rate = rospy.Rate(self._control_rate)
        i=num_points
        rospy.loginfo("the num of point is %i" % i )
        self.jointTrajectoryPublisher.publish(goal.trajectory)

        for trajectory_point in trajectory_points:
            js = JointState()
            #rospy.loginfo("the length of js.name is %i" %len(js.name))
            js.name=copy.deepcopy(joint_names)
            for j in range(len(joint_names)):
                #rospy.loginfo("j is %i" % j)
                js.name[j] = js.name[j]+'_'+str(i)
            i=i-1
            #js.position = trajectory_point.positions
            #js.velocity=trajectory_point.velocities
            #self.jointStatePublisher.publish(js)
            control_rate.sleep()
            
        start_time = rospy.get_time()
        now_from_start = rospy.get_time() - start_time
        
        last_idx = -1
        self._result.error_code = self._result.SUCCESSFUL
        self.server.set_succeeded(self._result)
예제 #8
0
def publish_position():
    '''joint position publisher'''    
    msg = JointState()
    msg.header.stamp = rospy.Time.now()
    msg.name = JOINT_NAME_ARRAY
    msg.position = joints_pos
    pub_js.publish(msg)
예제 #9
0
    def eyelidServosPut(self,params):
        msg = JointState()
        msg.name = list()
        msg.position = list()
        msg.velocity = list()
        msg.effort = list()
        if params.has_key('leftEyelid_position'):
            msg.name.append('left_eyelid_joint') 
            msg.position.append(math.radians(float(params['leftEyelid_position'])-float(90)))
        if params.has_key('leftEyelid_speed'):
            #Anadir velocuidad
            msg.velocity.append(params['leftEyelid_speed'])
        else:
            msg.velocity.append(3.0)
        if params.has_key('rightEyelid_position'):
            msg.name.append('right_eyelid_joint')
            msg.position.append(math.radians(float(params['rightEyelid_position'])-float(90)))
        if params.has_key('rightEyelid_speed'):
            #Anadir velocuidad
            msg.velocity.append(params['rightEyelid_speed'])
        else:
            msg.velocity.append(3.0)

        msg.header.stamp = rospy.Time.now()
        self.cmd_joints_pub.publish(msg)
예제 #10
0
 def test_fk_easy4(self):
     chain_state = JointState()
     chain_state.position = [0, 0, 0]
     eef = self.chain.fk(chain_state, -1)
     print eef
     eef_expected = numpy.matrix([[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 2], [0, 0, 0, 1]])
     self.assertAlmostEqual(numpy.linalg.norm(eef - eef_expected), 0.0, 6)
def generateJointStatesFromRegisterStateAndPublish():
    rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, updateLocalJointState)

    global currentJointState 
    currentJointState = JointState()
    currentJointState.header.frame_id = "robotiq_s_model from real-time state data"
    currentJointState.name = ["finger_1_joint_1", 
                                "finger_1_joint_2", 
                                "finger_1_joint_3", 
                                "finger_2_joint_1", 
                                "finger_2_joint_2", 
                                "finger_2_joint_3", 
                                "finger_middle_joint_1", 
                                "finger_middle_joint_2", 
                                "finger_middle_joint_3", 
                                "palm_finger_1_joint", 
                                "palm_finger_2_joint"]

    currentJointState.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    currentJointState.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    currentJointState.effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    pub = rospy.Publisher('joint_states', JointState, queue_size=1) #no buffering?
    rospy.init_node('robotiq_s_model_joint_state_publisher', anonymous=False)
    rate = rospy.Rate(100) # publishes at 100Hz

    while not rospy.is_shutdown():
        currentJointState.header.stamp = rospy.Time.now()
        pub.publish(currentJointState)
        rate.sleep()

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
 def js_cb(self, a):
     rospy.loginfo('Received array')
     js = JointState()
     js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
     jList = a.data
     jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
     i = 0
     for pos in jMatrix:
         rospy.loginfo(pos)
         js.position = pos
         gsvr = GetStateValidityRequest()
         rs = RobotState()
         rs.joint_state = js
         gsvr.robot_state = rs
         gsvr.group_name = "both_arms"
         resp = self.coll_client(gsvr)
         if (resp.valid == False):
             rospy.loginfo('false')
             rospy.loginfo(i)
             self.js_pub.publish(0)
             return
         i = i + 1
     self.js_pub.publish(1)
     rospy.loginfo('true')
     return   
예제 #13
0
 def set_stiffness(self):
     stiffness_message = JointState()
     stiffness_message.name = ["HeadYaw", "HeadPitch"]
     stiffness_message.position = [0,0]
     stiffness_message.velocity = [0,0]
     stiffness_message.effort   = [1,1]
     self.stiffness_publisher.publish(stiffness_message)
예제 #14
0
 def loop(self):
     hz = 20
     r = rospy.Rate(hz)
     msg = JointState()
     for i in range(8):
         msg.name.append("q"+str(i+1))
         
     while not rospy.is_shutdown() and not self.endthread:
         br = tf.TransformBroadcaster()
         msg.header.stamp=rospy.Time.now()
         msg.position = 8 * [0.0]
         msg.effort = 8 * [0.0]
         self.torque[6]=0.1
         for i in range(8):
             msg.position[i]=self.q_desired[i]
             msg.effort[i]=self.torque[i]
         if(self.publishInfo):
             self.publisher.publish(msg)     
             
         if(self.publishDesiredInfo):
             br.sendTransform((self.wTp_desired[0],
                              self.wTp_desired[1],
                              self.wTp_desired[2]),
                              tf.transformations.quaternion_from_euler(self.wTp_desired[3],
                                                                       self.wTp_desired[4],
                                                                       self.wTp_desired[5]),
                              rospy.Time.now(),
                              self.child_frame,
                              self.parent_frame)
             # Add in the tf transform now
         
         self.master.update_idletasks()    
         r.sleep()            
예제 #15
0
파일: robot2.py 프로젝트: brianpage/vspgrid
        def publish(eventStop):

            # for arbotix
            pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
            pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
            pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
            pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
            pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)

            # for visualization
            jointPub = rospy.Publisher(
                "/joint_states", JointState, queue_size=5)
            jmsg = JointState()
            jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']

            while not rospy.is_shutdown() and not eventStop.is_set():

                jmsg.header.stamp = rospy.Time.now()
                jmsg.position = self.q
                if self.sim:
                    jointPub.publish(jmsg)

                pub1.publish(self.q[0])
                pub2.publish(self.q[1])
                pub3.publish(self.q[2])
                pub4.publish(self.q[3])
                pub5.publish(self.q[4])

                eventStop.wait(self.dt)
예제 #16
0
def resethead(): # rotate the head(not PTU)
    pub = rospy.Publisher('/head/commanded_state', JointState)       
    head_command = JointState() 
    head_command.name=["HeadPan"]
    head_command.position=[0.0] #forwards  
    pub.publish(head_command)
    eyeplay(0)# reset eyes
예제 #17
0
def head_wander():
    rospy.init_node('head_wander')
    joints_pub = rospy.Publisher('/cmd_joints', JointState)
    say = rospy.ServiceProxy('/qbo_talk/festival_say', Text2Speach)

    seq = 0
    while not rospy.is_shutdown():
        seq += 1

        msg = JointState()
        msg.header.seq = seq
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = 'base_footprint'

        msg.name = ['head_pan_joint','head_tilt_joint']
        msg.position = [random.uniform(-0.9,0.9), random.uniform(-0.8,0.3)]

        joints_pub.publish(msg)

        if random.random() < 0.3:
            try:
                say(random.choice(UTTERANCES))
            except rospy.ServiceException:
                # Ignore errors
                pass

        rospy.sleep(2.0)
예제 #18
0
파일: gummi.py 프로젝트: vbansal1/GummiArm
 def publishJointState(self):
     msg = JointState()
     msg.header.stamp = rospy.Time.now()
     msg.name = self.jointNames
     msg.position = self.getJointAngles()
     msg.effort = self.getLoads()
     self.jointStatePub.publish(msg)
def talker():
    joint1 = radians(90)
    pub = rospy.Publisher('/kukaAllegroHand/robot_cmd', JointState)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10000) # 10hz
    msg = JointState()
    msg.position = [0,joint1,0,0,0,0,0]
    pub.publish(msg)
    r.sleep()
    time.sleep(.4)
    
    while True:
        for acc in range(600,601):
            print '************************* acceleratioin: %d *************************' % acc
            # acc = 200
            trjFile = '../trj/acc_new_' + str(acc) + '.dat'
            trj = np.genfromtxt(trjFile, delimiter="\t")
            done = True
            i = 1
            while not rospy.is_shutdown() and i < len(trj):
                print i
                msg = JointState()
                msg.position = [0,joint1,0,0,0,0,0]
                msg.position[0] = radians(trj[i][0])
                msg.position[2] = radians(trj[i][0])
                msg.position[3] = radians(trj[i][0])
                msg.position[4] = radians(trj[i][0])
                msg.position[5] = radians(trj[i][0])
                msg.position[6] = radians(trj[i][0])
                pub.publish(msg)
                r.sleep()
                time.sleep((trj[i][2] - trj[i-1][2])/10)
                i += 1
예제 #20
0
 def generate_views(self):        
     rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose)
     try:
         resp = self.nav_goals(10, self.inflation_radius, self.roi)
         
         if not resp.goals.poses:
             self.views = []
             return
         for j in range(len(resp.goals.poses)):
             for i in range(0,self.views_at_pose):
                 pose = resp.goals.poses[j]
                 pan = random.uniform(self.min_pan, self.max_pan)
                 tilt = random.uniform(self.min_tilt, self.max_tilt)
                 jointstate = JointState()
                 jointstate.name = ['pan', 'tilt']
                 jointstate.position = [pan, tilt]
                 jointstate.velocity = [self.velocity, self.velocity]
                 jointstate.effort = [float(1.0), float(1.0)]
                 resp_ptu_pose = self.ptu_fk(pan,tilt,pose)
                 p = Pose()
                 view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose)
                 self.views.append(view)
             
     except rospy.ServiceException, e:
         rospy.logerr("Service call failed: %s" % e)
예제 #21
0
 def update(self):
     if not self.driver_status == 'DISCONNECTED':
         # Get Joint Positions
         self.current_joint_positions = self.rob.getj()
         msg = JointState()
         msg.header.stamp = rospy.get_rostime()
         msg.header.frame_id = "robot_secondary_interface_data"
         msg.name = self.JOINT_NAMES
         msg.position = self.current_joint_positions
         msg.velocity = [0]*6
         msg.effort = [0]*6
         self.joint_state_publisher.publish(msg)
         
         # Get TCP Position
         tcp_angle_axis = self.rob.getl()
         # Create Frame from XYZ and Angle Axis
         T = PyKDL.Frame()   
         axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5])
         # Get norm and normalized axis
         angle = axis.Normalize()
         # Make frame
         T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2])
         T.M = PyKDL.Rotation.Rot(axis,angle)
         # Create Pose
         self.current_tcp_pose = tf_c.toMsg(T)
         self.current_tcp_frame = T
         self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
예제 #22
0
    def default(self, ci="unused"):
        """ Publish the data of the posture sensor as a ROS JointState message """
        js = JointState()
        js.header = self.get_ros_header()

        js.name = [
            "kuka_arm_0_joint",
            "kuka_arm_1_joint",
            "kuka_arm_2_joint",
            "kuka_arm_3_joint",
            "kuka_arm_4_joint",
            "kuka_arm_5_joint",
            "kuka_arm_6_joint",
            "head_pan_joint",
            "head_tilt_joint",
        ]
        js.position = [
            self.data["seg0"],
            self.data["seg1"],
            self.data["seg2"],
            self.data["seg3"],
            self.data["seg4"],
            self.data["seg5"],
            self.data["seg6"],
            self.data["pan"],
            self.data["tilt"],
        ]
        # js.velocity = [1, 1, 1, 1, 1, 1, 1]
        # js.effort = [50, 50, 50, 50, 50, 50, 50]

        self.publish(js)
    def _move_to(self, point, dur):
            
        rospy.sleep(dur)
        msg = JointState()
        
        with self.lock:
            if not self.sig_stop:
                self.joint_positions = point.positions[:]
		self.joint_velocities = point.velocities[:]

                msg.name = ['joint_1',
                            'joint_2',
                            'joint_3',
                            'joint_4',
                            'joint_5',
                            'joint_6',
                            'joint_7']
		#msg.name = self.joint_names
		msg.position = self.joint_positions
		msg.velocity = self.joint_velocities #[0.2]*7
                msg.header.stamp = rospy.Time.now()

                self.joint_command_pub.publish(msg)

                #rospy.loginfo('Moved to position: %s in %s', str(self.joint_positions), str(dur))
            else:
                rospy.loginfo('Stoping motion immediately, clearing stop signal')
                self.sig_stop = False
 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)
예제 #25
0
    def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
        '''
        Returns the joint state of the arm in the current planning scene state or the passed in state.

        **Args:**

            *robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.  
            If None, will use the current robot state in the planning scene interface
            
            *fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state

        **Returns:**
            A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state

        **Raises:**

            **ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
        '''
        if not robot_state:
            robot_state = self._psi.get_robot_state()
        joint_state = JointState()
        joint_state.header = robot_state.joint_state.header
        for n in self.joint_names:
            found = False
            for i in range(len(robot_state.joint_state.name)):
                if robot_state.joint_state.name[i] == n:
                    joint_state.name.append(n)
                    joint_state.position.append(robot_state.joint_state.position[i])
                    found = True
                    break
            if not found and fail_if_joint_not_found:
                raise ValueError('Joint '+n+' is missing from robot state')
        return joint_state
예제 #26
0
파일: rc.py 프로젝트: paulyang1990/toy_code
def rc_talker():
    '''init pyxhook'''
    #Create hookmanager
    hookman = pyxhook.HookManager()
    #Define our callback to fire when a key is pressed down
    hookman.KeyDown = key_down_event
    hookman.KeyUp   = key_up_event
    #Hook the keyboard
    hookman.HookKeyboard()
    #Start our listener
    hookman.start()

    pub = rospy.Publisher('rc_sender', String, queue_size=10)
    # 2016-10-26 test urdf joint control
    # controls a file in ~/toy_code/toy_ros_space/src/rc_sim_oub/test.launch
    #roslaunch test.launch model:='$(find urdf_tutorial)/urdf/07-physics.urdf' gui:=True
    pub_2 = rospy.Publisher('test_joint', JointState, queue_size=10)
    rospy.init_node('rc_talker')
    rate = rospy.Rate(50)
    while not rospy.is_shutdown():
        #hello_str = "hello world %s" % rospy.get_time()
        pub.publish(''.join(rc_keys))

        # 2016-10-26 test urdf joint control
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.name = ['base_to_left_arm', 'base_to_right_arm']
        msg.position = [-0.3*int(rc_keys[0]),0.3*int(rc_keys[1])]
        pub_2.publish(msg)
        #print rc_keys
        rate.sleep()
        if running == False:
            #Close the listener when we are done
            hookman.cancel()
            break
    def computePositionsForNextJoint(self, currPositions, jointsToSweep, maxs, mins, joint_list):
        if len(jointsToSweep) != 0:    
            for kk in range(0, int(1.0/self.resolutionOfSweeps)): # The actual sweeping
                currPositions[joint_list.index(jointsToSweep[0])] 
                currPositions[joint_list.index(jointsToSweep[0])] = mins[joint_list.index(jointsToSweep[0])]+(self.resolutionOfSweeps*kk)*(maxs[joint_list.index(jointsToSweep[0])]-mins[joint_list.index(jointsToSweep[0])])
                if (len(jointsToSweep) > 1):
                    self.computePositionsForNextJoint(currPositions, jointsToSweep[1:len(jointsToSweep)], maxs, mins, joint_list)

                # Combine message and publish
                msg = JointState()
                msg.header.stamp = rospy.Time.now()
                msg.name = self.joint_list
                # print self.free_joints
                msg.position = currPositions
                msg.velocity = [0.0]*len(maxs)
                msg.effort = [0.0]*len(maxs)
                time.sleep(0.0001)
                self.pub.publish(msg)
                # Get End Effector Positions
                rate = rospy.Rate(20.0)
                try:
                    (trans,rot) = self.listener.lookupTransform(self.baseLink, self.finalLink, rospy.Time(0))
                    if (self.currX != trans[0]) or (self.currY != trans[1] or self.currZ != trans[2]):
                        self.csvWriter.writerow(trans)
                    self.currX = trans[0]
                    self.currY = trans[1]
                    self.currZ = trans[2]
                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                    print "ERROR: looking up tf"
                    time.sleep(0.5) # Sleep for a bit to let system catch up
                    continue
예제 #28
0
    def build_joint_state_msg(self):
        """
        @brief JointState message builder.
        
        Builds a JointState message with the current position of the finger 
        joints and its names.
        """
        
        js_msg = JointState()
        js_msg.header.stamp = rospy.Time.now()
        
        if self.joint_names == []:
            self.joint_names = ["{}.{}".format('hand', attr) 
                    for attr in ORI_ATTRIBUTES] + \
            ["{}.{}.{}".format(finger, bone, ori) 
                for finger in FINGER_NAMES 
                    for bone in FINGER_BONES
                        for ori in ORI_ATTRIBUTES]
            LOG.v("Publishing JointState for the following joints: {}".format(self.joint_names), "start_transmit")
        
        js_msg.position = [0.0] * len(self.joint_names)

        pos = 0
        # Build JointState. First the hand...        
        for i, attr in enumerate(ORI_ATTRIBUTES):
            js_msg.name.append('hand.' + str(attr))
            
            # Roll precision hack
            if attr == 'roll':
                vector = self.hand.palm_normal
            else:
                vector = self.hand.direction
                
            js_msg.position[pos] = getattr(vector, attr)
            pos += 1

        # ...then the fingers
        for i, finger_name, finger in \
            [(i, finger_name, self.fingers[finger_name]) \
                for i, finger_name in enumerate(FINGER_NAMES)]:
                        
            # LEAP API v2.0: Skeletal model
            # Get bones
            for j, bone_name, bone in \
                [(j, bone_name, finger.bone(j)) \
                    for j, bone_name in enumerate(FINGER_BONES)]:

                # Fill the joint values one by one
                for k, attr in enumerate(ORI_ATTRIBUTES):

                    joint_name = "{}.{}.{}".format(finger_name, bone_name, attr)
                    joint_value = getattr(bone.direction, attr)
                    
                    js_msg.name.append(joint_name)
                    js_msg.position[pos] = joint_value
                    pos += 1
                    
        # return the JointState message
        return js_msg
def get_new_JointState(pan, tilt):
	j = JointState()
	j.header.stamp = rospy.Time.now()
	j.name = ["panservo", "tiltservo"]
	j.position = [pan, tilt]
	j.velocity = []
	j.effort = []
	return j
예제 #30
0
    def __init__(self):
        global jspr2msg

        jspr2msg = JointState()
        jspr2msg.name = [ "left_shoulder_tilt_joint", "left_lin_act_cyl_joint", "left_upper_arm_hinge_joint","left_linear_actuator_joint", "right_shoulder_tilt_joint", "right_lin_act_cyl_joint", "right_upper_arm_hinge_joint","right_linear_actuator_joint", "torso_lift_joint", "wheel_linear_actuator_joint", "fl_anchor_rod_joint", "fl_push_rod_joint", "fl_caster_rotation_joint", "fr_anchor_rod_joint", "fr_push_rod_joint", "fr_caster_rotation_joint", "bl_anchor_rod_joint", "bl_push_rod_joint", "bl_caster_rotation_joint", "br_anchor_rod_joint", "br_push_rod_joint", "br_caster_rotation_joint"]
        jspr2msg.position = [0.0 for name in jspr2msg.name]
        jspr2msg.velocity = [0.0 for name in jspr2msg.name]
        rospy.loginfo("done init")
    def _ros_init(self):
        # Can check log msgs according to log_level {rospy.DEBUG, rospy.INFO, rospy.WARN, rospy.ERROR}
        rospy.init_node('RLkitUR', anonymous=True, log_level=rospy.DEBUG)
        rospy.logdebug("Starting RLkitUR Class object...")

        # Init GAZEBO Objects
        self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state',
                                                SetModelState)
        self.get_world_state = rospy.ServiceProxy(
            '/gazebo/get_world_properties', GetWorldProperties)

        # Subscribe joint state and target pose
        rospy.Subscriber("/joint_states", JointState,
                         self.joints_state_callback)
        rospy.Subscriber("/target_blocks_pose", Point,
                         self.target_point_callback)
        rospy.Subscriber("/gazebo/link_states", LinkStates,
                         self.link_state_callback)
        rospy.Subscriber("/collision_status", Bool, self.collision_status)

        # Gets training parameters from param server
        self.desired_pose = Pose()
        self.running_step = rospy.get_param("/running_step")
        self.max_height = rospy.get_param("/max_height")
        self.min_height = rospy.get_param("/min_height")
        self.observations = rospy.get_param("/observations")
        self.joint_names = rospy.get_param("/joint_names")

        # Joint limitation
        shp_max = rospy.get_param("/joint_limits_array/shp_max")
        shp_min = rospy.get_param("/joint_limits_array/shp_min")
        shl_max = rospy.get_param("/joint_limits_array/shl_max")
        shl_min = rospy.get_param("/joint_limits_array/shl_min")
        elb_max = rospy.get_param("/joint_limits_array/elb_max")
        elb_min = rospy.get_param("/joint_limits_array/elb_min")
        wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
        wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
        wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
        wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
        wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
        wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
        self.joint_limits = {
            "shp_max": shp_max,
            "shp_min": shp_min,
            "shl_max": shl_max,
            "shl_min": shl_min,
            "elb_max": elb_max,
            "elb_min": elb_min,
            "wr1_max": wr1_max,
            "wr1_min": wr1_min,
            "wr2_max": wr2_max,
            "wr2_min": wr2_min,
            "wr3_max": wr3_max,
            "wr3_min": wr3_min
        }

        # Joint Velocity limitation
        shp_vel_max = rospy.get_param("/joint_velocity_limits_array/shp_max")
        shp_vel_min = rospy.get_param("/joint_velocity_limits_array/shp_min")
        shl_vel_max = rospy.get_param("/joint_velocity_limits_array/shl_max")
        shl_vel_min = rospy.get_param("/joint_velocity_limits_array/shl_min")
        elb_vel_max = rospy.get_param("/joint_velocity_limits_array/elb_max")
        elb_vel_min = rospy.get_param("/joint_velocity_limits_array/elb_min")
        wr1_vel_max = rospy.get_param("/joint_velocity_limits_array/wr1_max")
        wr1_vel_min = rospy.get_param("/joint_velocity_limits_array/wr1_min")
        wr2_vel_max = rospy.get_param("/joint_velocity_limits_array/wr2_max")
        wr2_vel_min = rospy.get_param("/joint_velocity_limits_array/wr2_min")
        wr3_vel_max = rospy.get_param("/joint_velocity_limits_array/wr3_max")
        wr3_vel_min = rospy.get_param("/joint_velocity_limits_array/wr3_min")
        self.joint_velocty_limits = {
            "shp_vel_max": shp_vel_max,
            "shp_vel_min": shp_vel_min,
            "shl_vel_max": shl_vel_max,
            "shl_vel_min": shl_vel_min,
            "elb_vel_max": elb_vel_max,
            "elb_vel_min": elb_vel_min,
            "wr1_vel_max": wr1_vel_max,
            "wr1_vel_min": wr1_vel_min,
            "wr2_vel_max": wr2_vel_max,
            "wr2_vel_min": wr2_vel_min,
            "wr3_vel_max": wr3_vel_max,
            "wr3_vel_min": wr3_vel_min
        }

        #  Init joint pose
        shp_init_value = rospy.get_param("/init_joint_pose/shp")
        shl_init_value = rospy.get_param("/init_joint_pose/shl")
        elb_init_value = rospy.get_param("/init_joint_pose/elb")
        wr1_init_value = rospy.get_param("/init_joint_pose/wr1")
        wr2_init_value = rospy.get_param("/init_joint_pose/wr2")
        wr3_init_value = rospy.get_param("/init_joint_pose/wr3")
        self.init_joint_angles = [
            shp_init_value, shl_init_value, elb_init_value, wr1_init_value,
            wr2_init_value, wr3_init_value
        ]

        # 3D coordinate limits
        x_max = rospy.get_param("/cartesian_limits/x_max")
        x_min = rospy.get_param("/cartesian_limits/x_min")
        y_max = rospy.get_param("/cartesian_limits/y_max")
        y_min = rospy.get_param("/cartesian_limits/y_min")
        z_max = rospy.get_param("/cartesian_limits/z_max")
        z_min = rospy.get_param("/cartesian_limits/z_min")
        self.xyz_limits = {
            "x_max": x_max,
            "x_min": shp_vel_min,
            "y_max": y_max,
            "y_min": y_min,
            "z_max": z_max,
            "z_min": z_min
        }

        # Fill in the Done Episode Criteria list
        self.episode_done_criteria = rospy.get_param("/episode_done_criteria")

        # Stablishes connection with simulator
        self._gz_conn = GazeboConnection()
        self._ctrl_conn = ControllersConnection(namespace="")

        # Controller type for ros_control
        self.current_controller_type = rospy.get_param("/control_type")
        self.pre_controller_type = self.current_controller_type

        # Init the observations, target, ...
        self.base_orientation = Quaternion()
        self.target_point = Point()
        self.link_state = LinkStates()
        self.joints_state = JointState()
        self.end_effector = Point()
        self.distance = 0.

        # Arm/Control parameters
        self._ik_params = setups['UR5_6dof']['ik_params']

        # ROS msg type
        self._joint_pubisher = JointPub()
        self._joint_traj_pubisher = JointTrajPub()
        self.reset_publisher = rospy.Publisher("/ur/reset",
                                               String,
                                               queue_size=1)
        self.target_point = Point()
        self._target_point_pubisher = rospy.Publisher("/target_goal",
                                                      Point,
                                                      queue_size=1)

        # Controller list
        self.vel_traj_controller = [
            'joint_state_controller', 'gripper_controller',
            'vel_traj_controller'
        ]
        self.pos_traj_controller = [
            'joint_state_controller', 'gripper_controller',
            'pos_traj_controller'
        ]
        self.vel_controller = [
            "joint_state_controller", "gripper_controller",
            "ur_shoulder_pan_vel_controller",
            "ur_shoulder_lift_vel_controller", "ur_elbow_vel_controller",
            "ur_wrist_1_vel_controller", "ur_wrist_2_vel_controller",
            "ur_wrist_3_vel_controller"
        ]
        self.pos_controller = [
            "joint_state_controller", "gripper_controller",
            "ur_shoulder_pan_pos_controller",
            "ur_shoulder_lift_pos_controller", "ur_elbow_pos_controller",
            "ur_wrist_1_pos_controller", "ur_wrist_2_pos_controller",
            "ur_wrist_3_pos_controller"
        ]

        # Stop flag durning training
        self.stop_flag = False
	global currentState
	currentState = msg.position

def desiredStateCB(msg):
	global desiredState
	desiredState = msg.position
	

if __name__ == '__main__':

	rospy.init_node('velocity_control')
    rate = rospy.Rate(60)
	joint_pub = rospy.Publisher('/jaco/joint_control', JointState, queue_size=1000)
	current_sub = rospy.Subscriber('/jaco/joint_state', JointState, currentStateCB)
	desired_sub = rospy.Subscriber('desired_position', JointState, desiredStateCB)
	msg = JointState()
    msg.name = ['jaco_arm_0_joint', 'jaco_arm_1_joint', 'jaco_arm_2_joint', 'jaco_arm_3_joint', 'jaco_arm_4_joint', 'jaco_arm_5_joint', 'jaco_finger_joint_0', 'jaco_finger_joint_2', 'jaco_finger_joint_4']


	while not rospy.is_shutdown():
		error = desiredState - currentState
		for e in error:
			if e < 0.01:
				e = 0
		
		vel = P*error
		msg.velocity = vel

		joint_pub.publish(msg)

		rate.sleep()
예제 #33
0
    server.insert(int_marker, processFeedback)
    menu_handler.apply(server, int_marker.name)


if __name__ == "__main__":

    rospy.init_node("visual_control")
    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
    pub2 = rospy.Publisher('read_coords', Int32, queue_size=10)
    pub3 = rospy.Publisher('move_to_time', CoordsWithTime, queue_size=100)
    time.sleep(1)
    pub2.publish(1)
    time.sleep(1)

    joint_state_send = JointState()
    joint_state_send.header = Header()
    joint_state_send.name = [
        'base_to_base_rot', 'base_rot_to_link_1', 'link_1_to_link_2',
        'link_2_to_link_3', 'link_3_to_link_end'
    ]
    joint_state_send.name = joint_state_send.name + [
        'link_1_to_add_1', 'link_2_to_add_4', 'link_3_to_add_2',
        'base_rot_to_add_3', 'link_2_to_add_5'
    ]

    br = TransformBroadcaster()

    # create a timer to update the published transforms

    server = InteractiveMarkerServer("uarm_controller")
def main():
    print "INITIALIZING HEAD NODE IN SIMULATION MODE BY MARCOSOFT..."
    ###Connection with ROS
    rospy.init_node("head")
    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = ["pan_connect", "tilt_connect"]
    jointStates.position = [0, 0]

    #Stablishing suscribers & plublishers
    subPosition = rospy.Subscriber(
        "head/goal_pose", Float32MultiArray, callbackPosHead
    )  #Se corre esta linea para ajustar el angulo del kinect
    pubHeadPose = rospy.Publisher("head/current_pose",
                                  Float32MultiArray,
                                  queue_size=1)
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
    pubHeadBattery = rospy.Publisher("/hardware/robot_state/head_battery",
                                     Float32,
                                     queue_size=1)

    #Rate at which ROS will loop (in Hz)
    loop = rospy.Rate(30)

    global goalPan
    global goalTilt

    goalPan = 0
    goalTilt = 0
    pan = 0
    tilt = 0
    speedPan = 0.1  #These values should represent the Dynamixel's moving_speed
    speedTilt = 0.1
    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0, 0]

    while not rospy.is_shutdown():
        deltaPan = goalPan - pan
        deltaTilt = goalTilt - tilt
        if deltaPan > speedPan:
            deltaPan = speedPan
        if deltaPan < -speedPan:
            deltaPan = -speedPan
        if deltaTilt > speedTilt:
            deltaTilt = speedTilt
        if deltaTilt < -speedTilt:
            deltaTilt = -speedTilt
        pan += deltaPan
        tilt += deltaTilt
        jointStates.header.stamp = rospy.Time.now()
        jointStates.position[0] = pan
        jointStates.position[
            1] = -tilt  #A tilt > 0 goes upwards, but to keep a dextereous system, positive tilt should go downwards
        pubJointStates.publish(jointStates)
        #print "Poses: " + str(panPose) + "   " + str(tiltPose)
        msgCurrentPose.data = [pan, tilt]
        pubHeadPose.publish(msgCurrentPose)
        msgBattery = Float32()
        msgBattery.data = 12.0
        pubHeadBattery.publish(msgBattery)
        loop.sleep()
def cb(msg):
    state_msg = JointState()
    state_msg.name = msg.joint_names
    state_msg.position = msg.points[0].positions
    state_msg.header = msg.header
    joint_state_pub.publish(state_msg)
예제 #36
0
    if start_flag == True:
        return

    for i in range(0, len(data.position)):
        if "joint" in data.name[i]:
            ctrl_joint.position.append(data.position[i])

    start_flag = True


if __name__ == "__main__":
    rospy.init_node("joint_control_test")

    start_flag = False
    joint_state = JointState()
    joint_size = 0

    duration = rospy.get_param("~duration", 0.05)  #20Hz

    joint_control_topic_name = rospy.get_param("~joint_control_topic_name",
                                               "/dragon/joints_ctrl")
    pub = rospy.Publisher(joint_control_topic_name, JointState, queue_size=10)
    joint_state_topic_name = rospy.get_param("~joint_state_topic_name",
                                             "/dragon/joint_states")
    rospy.Subscriber(joint_state_topic_name, JointState, joint_state_cb)

    start_time = 0

    target_max_pitch = -0.34
    ctrl_joint = JointState()
    def _feedback_cb(self, msg):
        for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity,
                                       msg.effort):
            if name not in self.hebi_mapping:
                print("WARNING: arm_callback - unrecognized name!!!")
            else:
                self.current_jt_pos[name] = pos
                self.current_jt_vel[name] = vel
                self.current_jt_eff[name] = eff
                if not rospy.is_shutdown(
                ) and self._joint_state_pub is not None:  # Publish JointState() for RViz
                    jointstate = JointState()
                    jointstate.header.stamp = rospy.Time.now()
                    jointstate.name = self._active_joints
                    jointstate.position = self._get_joint_angles()
                    jointstate.velocity = [0.0] * len(jointstate.name)
                    jointstate.effort = [0.0] * len(jointstate.name)
                    self._joint_state_pub.publish(jointstate)

        # TODO: Make this less hacky
        if not rospy.is_shutdown() and self._joint_state_pub is not None:
            if not self._executing_trajectory:
                joint_grav_torques = self.kdl_kin.get_joint_torques_from_gravity(
                    self._get_joint_angles(),
                    grav=[0, 0, -9.81])  # TODO: FIXME: Hardcoded
                jointstate = JointState()
                jointstate.header.stamp = rospy.Time.now()
                jointstate.name = self.hebi_mapping
                if self._hold_position:
                    # jointstate.position = self.waypoints[-1].positions  # jerky
                    jointstate.position = self._hold_joint_states
                else:
                    jointstate.position = []
                jointstate.velocity = []
                jointstate.effort = joint_grav_torques
                self.cmd_pub.publish(jointstate)
예제 #38
0
positions.append(0.0)
joints.append('head_j2')
positions.append(0.0)
joints.append('head_j3')
positions.append(0.0)
joints.append('head_j4')
positions.append(0.0)
joints.append('head_j5')
positions.append(0.0)
joints.append('head_j6')
positions.append(0.0)





header = Header(0,rospy.Time.now(),'0')
pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions)))

try:
    while not rospy.is_shutdown():
	time.sleep(1.0)	
	header = Header(0,rospy.Time.now(),'0')
	pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions)))	    
except (KeyboardInterrupt,EOFError,rospy.ROSInterruptException):
    pass




예제 #39
0
#!/usr/bin/python3

import rospy
import time
from std_msgs.msg import Int32
from sensor_msgs.msg import JointState

pub = rospy.Publisher('motor_data', JointState, queue_size=10)
rospy.init_node('initialize_position')

joint = JointState()
joint.header.stamp = rospy.Time.now()
joint.position = [10, 10, 90, 0, 0]
joint.velocity = [10, 10, 10, 10, 10]
joint.name = ["1", "2", "3", "4", "5"]

pub.publish(joint)
예제 #40
0
    def __init__(self):
        rospy.init_node('thymio')

        # load script on the Thymio

        rospy.wait_for_service('aseba/get_node_list')
        get_aseba_nodes = rospy.ServiceProxy('aseba/get_node_list',
                                             GetNodeList)

        while True:
            if 'thymio-II' in get_aseba_nodes().nodeList:
                break
            rospy.loginfo('Waiting for thymio node ...')
            rospy.sleep(1)

        rospy.wait_for_service('aseba/load_script')
        load_script = rospy.ServiceProxy('aseba/load_script', LoadScripts)
        default_script_path = os.path.join(
            roslib.packages.get_pkg_dir('thymio_driver'),
            'aseba/thymio_ros.aesl')

        script_path = rospy.get_param('~script', default_script_path)

        rospy.loginfo("Load aseba script %s", script_path)

        load_script(script_path)

        # initialize parameters

        self.tf_prefix = rospy.get_param('tf_prefix', '')

        self.odom_frame = self.frame_name(
            rospy.get_param('~odom_frame', 'odom'))
        self.robot_frame = self.frame_name(
            rospy.get_param('~robot_frame', 'base_link'))

        self.x = 0
        self.y = 0
        self.th = 0
        self.then = rospy.Time.now()
        odom_rate = rospy.get_param('~odom_max_rate', -1)
        if odom_rate == 0:
            self.odom_min_period = -1
        else:
            self.odom_min_period = 1.0 / odom_rate
        self.odom_msg = Odometry(header=rospy.Header(frame_id=self.odom_frame),
                                 child_frame_id=self.robot_frame)

        self.odom_msg.pose.pose.position.z = 0
        self.odom_msg.pose.covariance[0] = -1
        self.odom_msg.header.stamp = rospy.Time.now()

        # subscribe to topics

        self.aseba_pub = rospy.Publisher('aseba/events/set_speed',
                                         AsebaEvent,
                                         queue_size=1)

        self.left_wheel_angle = 0
        self.right_wheel_angle = 0

        self.axis = rospy.get_param('~axis_length', BASE_WIDTH / 1000.0)
        self.motor_speed_deadband = rospy.get_param('~motor_speed_deadband',
                                                    10)
        # self.ticks2mm = rospy.get_param('~ticks2mm', 1.0 / SPEED_COEF)
        # self.diff_factor = rospy.get_param('~diff_factor', 1.0)

        def_cal = [0.001 / SPEED_COEF, 0]

        left_wheel_calibration = rospy.get_param('~left_wheel_calibration/q',
                                                 def_cal)

        self.left_wheel_speed, self.left_wheel_motor_speed = motor_speed_conversion(
            *left_wheel_calibration)
        rospy.loginfo('Init left wheel with calibration %s',
                      left_wheel_calibration)

        right_wheel_calibration = rospy.get_param('~right_wheel_calibration/q',
                                                  def_cal)
        self.right_wheel_speed, self.right_wheel_motor_speed = motor_speed_conversion(
            *right_wheel_calibration)

        rospy.loginfo('Init right wheel with calibration %s',
                      right_wheel_calibration)

        left_wheel_joint = rospy.get_param('~left_wheel_joint',
                                           'left_wheel_joint')
        right_wheel_joint = rospy.get_param('~right_wheel_joint',
                                            'right_wheel_joint')

        self.wheel_state_msg = JointState()
        self.wheel_state_msg.name = [left_wheel_joint, right_wheel_joint]

        self.wheel_state_pub = rospy.Publisher('joint_states',
                                               JointState,
                                               queue_size=1)
        self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
        self.odom_broadcaster = TransformBroadcaster()

        rospy.Subscriber('aseba/events/odometry', AsebaEvent,
                         self.on_aseba_odometry_event)
        rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)

        self.buttons = Joy()
        self.buttons_pub = rospy.Publisher('buttons', Joy, queue_size=1)
        rospy.Subscriber("aseba/events/buttons", AsebaEvent,
                         self.on_aseba_buttons_event)

        for button in BUTTONS:
            rospy.Subscriber('aseba/events/button_' + button, AsebaEvent,
                             self.on_aseba_button_event(button))

        proximity_range_min = rospy.get_param('~proximity/range_min', 0.0215)
        proximity_range_max = rospy.get_param('~proximity/range_min', 0.14)
        proximity_field_of_view = rospy.get_param('~proximity/field_of_view',
                                                  0.3)

        self.proximity_sensors = [{
            'publisher':
            rospy.Publisher('proximity/' + name, Range, queue_size=1),
            'msg':
            Range(header=rospy.Header(
                frame_id=self.frame_name('proximity_{name}_link'.format(
                    name=name))),
                  radiation_type=Range.INFRARED,
                  field_of_view=proximity_field_of_view,
                  min_range=proximity_range_min,
                  max_range=proximity_range_max)
        } for name in PROXIMITY_NAMES]

        self.proximityToLaserPublisher = rospy.Publisher('proximity/laser',
                                                         LaserScan,
                                                         queue_size=1)
        self.proximityToLaser = LaserScan(
            header=rospy.Header(frame_id=self.frame_name('laser_link')),
            angle_min=-0.64,
            angle_max=0.64,
            angle_increment=0.32,
            time_increment=0,
            scan_time=0,
            range_min=proximity_range_min + 0.08,
            range_max=proximity_range_max + 0.08)
        rospy.Subscriber('aseba/events/proximity', AsebaEvent,
                         self.on_aseba_proximity_event)

        self.ground_sensors = [{
            'publisher':
            rospy.Publisher('ground/' + name, Range, queue_size=1),
            'msg':
            Range(header=rospy.Header(
                frame_id=self.frame_name('ground_{name}_link'.format(
                    name=name))),
                  radiation_type=Range.INFRARED,
                  field_of_view=proximity_field_of_view,
                  min_range=(GROUND_MIN_RANGE / 1000.0),
                  max_range=(GROUND_MAX_RANGE / 1000.0))
        } for name in GROUND_NAMES]

        ground_threshold = rospy.get_param('~ground/threshold', 200)

        rospy.Subscriber('aseba/events/ground', AsebaEvent,
                         self.on_aseba_ground_event)
        rospy.set_param("~ground_threshold", ground_threshold)

        self.imu = Imu(header=rospy.Header(frame_id=self.robot_frame))
        # no orientation or angular velocity information
        self.imu.orientation_covariance[0] = -1
        self.imu.angular_velocity_covariance[0] = -1
        # just an accelerometer
        self.imu.linear_acceleration_covariance[0] = 0.07
        self.imu.linear_acceleration_covariance[4] = 0.07
        self.imu.linear_acceleration_covariance[8] = 0.07

        self.imu_publisher = rospy.Publisher('imu', Imu, queue_size=1)
        rospy.Subscriber('aseba/events/accelerometer', AsebaEvent,
                         self.on_aseba_accelerometer_event)

        self.tap_publisher = rospy.Publisher('tap', Empty, queue_size=1)
        rospy.Subscriber('aseba/events/tap', AsebaEvent,
                         self.on_aseba_tap_event)

        self.temperature = Temperature(header=rospy.Header(
            frame_id=self.robot_frame))
        self.temperature.variance = 0.01
        self.temperature_publisher = rospy.Publisher('temperature',
                                                     Temperature,
                                                     queue_size=1)
        rospy.Subscriber('aseba/events/temperature', AsebaEvent,
                         self.on_aseba_temperature_event)

        self.sound_publisher = rospy.Publisher('sound', Float32, queue_size=1)
        self.sound_threshold_publisher = rospy.Publisher(
            'aseba/events/set_sound_threshold', AsebaEvent, queue_size=1)
        rospy.Subscriber('aseba/events/sound', AsebaEvent,
                         self.on_aseba_sound_event)
        rospy.Subscriber('sound_threshold', Float32, self.on_sound_threshold)

        self.remote_publisher = rospy.Publisher('remote', Int8, queue_size=1)
        rospy.Subscriber('aseba/events/remote', AsebaEvent,
                         self.on_aseba_remote_event)

        rospy.Subscriber('comm/transmit', Int16, self.on_sound_threshold)
        self.comm_publisher = rospy.Publisher('comm/receive',
                                              Int16,
                                              queue_size=1)
        self.aseba_set_comm_publisher = rospy.Publisher(
            'aseba/events/set_comm', AsebaEvent, queue_size=1)
        rospy.Subscriber('aseba/events/comm', AsebaEvent,
                         self.on_aseba_comm_event)

        # actuators

        for name in BODY_LEDS:
            rospy.Subscriber('led/body/' + name, ColorRGBA,
                             self.on_body_led(name))

        rospy.Subscriber('led', Led, self.on_led)
        self.aseba_led_publisher = rospy.Publisher('aseba/events/set_led',
                                                   AsebaEvent,
                                                   queue_size=6)

        rospy.Subscriber('led/off', Empty, self.on_led_off)

        rospy.Subscriber('led/gesture', LedGesture, self.on_led_gesture)
        self.aseba_led_gesture_publisher = rospy.Publisher(
            'aseba/events/set_led_gesture', AsebaEvent, queue_size=6)
        rospy.Subscriber('led/gesture/circle', Float32,
                         self.on_led_gesture_circle)
        rospy.Subscriber('led/gesture/off', Empty, self.on_led_gesture_off)
        rospy.Subscriber('led/gesture/blink', Float32,
                         self.on_led_gesture_blink)
        rospy.Subscriber('led/gesture/kit', Float32, self.on_led_gesture_kit)
        rospy.Subscriber('led/gesture/alive', Empty, self.on_led_gesture_alive)

        rospy.Subscriber('sound/play', Sound, self.on_sound_play)
        self.aseba_led_gesture_publisher = rospy.Publisher(
            'aseba/events/set_led_gesture', AsebaEvent, queue_size=6)
        rospy.Subscriber('sound/play/system', SystemSound,
                         self.on_system_sound_play)
        self.aseba_play_sound_publisher = rospy.Publisher(
            'aseba/events/play_sound', AsebaEvent, queue_size=1)
        self.aseba_play_system_sound_publisher = rospy.Publisher(
            'aseba/events/play_system_sound', AsebaEvent, queue_size=1)

        rospy.Subscriber('alarm', Bool, self.on_alarm)
        self.alarm_timer = None

        rospy.Subscriber('shutdown', Empty, self.on_shutdown_msg)
        self.aseba_shutdown_publisher = rospy.Publisher(
            'aseba/events/shutdown', AsebaEvent, queue_size=1)

        rospy.on_shutdown(self.shutdown)

        Server(ThymioConfig, self.change_config)
        # tell ros that we are ready
        rospy.Service('thymio_is_ready', std_srvs.srv.Empty, self.ready)
예제 #41
0
#!/usr/bin/env python3
#include libraries
import rospy
import tf
from math import cos, sin, pi
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from nav_msgs.msg import Odometry

jointstates = JointState()

jointstates.name = ['wheel0', 'wheel1', 'wheel2', 'wheel3']

pubodom = rospy.Publisher('odom', Odometry, queue_size=10)

broadcaster = tf.TransformBroadcaster()
map_broadcaster = tf.TransformBroadcaster()

timeold = 0

vel = Twist()

x = y = theta = 0
counter = 0


def jstatecb(jointstatesraw):
    global jointstates
    global timeold
    global x, y, theta, counter
예제 #42
0
            noise = np.random.normal(0, 0.25, 7)
            '''print right.joint_angles().values()
            print right.joint_angles().keys()
            print right.joint_angles()
            print "\r\n"
            '''
            '''seeds_random={
                'right': JointState(
                 header=hdr,
                 name= right.joint_angles().keys(),
                 position= (right.joint_angles().values() + noise).tolist() 
                 )
             }
             '''

            js = JointState()
            js.header = hdr
            i = 0
            for key, val in right.joint_angles().iteritems():
                js.name.append(key)
                js.position.append(val + noise[i])
                i += 1

            ikreq.seed_angles = [js]

            resp = iksvc(ikreq)
    try:
        return limb_joints
    except:
        mess = 1
        smile.publish(mess)
예제 #43
0
def handleUpdateResponse(r):
    """Handles the response for an update request

      Parses the message received from the robot, then publish the result

    Arguments:
        r {bytearray} -- response from the robot

    Returns:
        RobotState -- state object
    """

    global last_state
    global update_response
    global stored_pose_count
    global name
    global do_update
    global p_joint_states
    global p_robot_state

    try:

        i = 8

        state = rosweld_drivers.msg.RobotState()

        state.speed = unpack('>f', r[i:i + 4])[0]
        i = i + 4
        state.pose.position.x = unpack('>f', r[i:i + 4])[0] / 1000
        i = i + 4
        state.pose.position.y = unpack('>f', r[i:i + 4])[0] / 1000
        i = i + 4
        state.pose.position.z = unpack('>f', r[i:i + 4])[0] / 1000
        i = i + 4
        state.euler_angles.rx = unpack('>f', r[i:i + 4])[0]
        i = i + 4
        state.euler_angles.ry = unpack('>f', r[i:i + 4])[0]
        i = i + 4
        state.euler_angles.rz = unpack('>f', r[i:i + 4])[0]
        i = i + 4
        orientation = tf.transformations.quaternion_from_euler(
            radians(state.euler_angles.rz), radians(state.euler_angles.ry),
            radians(state.euler_angles.rx), axes)
        state.pose.orientation.x = orientation[0]
        state.pose.orientation.y = orientation[1]
        state.pose.orientation.z = orientation[2]
        state.pose.orientation.w = orientation[3]
        # -1 from the step index, because NACHI's list indexing is from 1, not zero
        state.step = unpack('>i', r[i:i + 4])[0] - 1
        i = i + 4
        state.storedPoses = unpack('>i', r[i:i + 4])[0]
        i = i + 4
        stateId = unpack('>i', r[i:i + 4])[0]
        state.robotProgramState = RobotStates.toString(stateId)
        i = i + 4
        state.mode = ["Teach", "Playback",
                      "High-speed Teach"][unpack('>i', r[i:i + 4])[0]]
        for _ in range(7):
            i = i + 4
            state.joints.append(unpack('>f', r[i:i + 4])[0])

        #print "Update received"
        #print seq_back, fingerY, currentMove, speed, RobotStates.toString(robotProgramState)

        update_response = True
        state.isMoving = last_state is not None and last_state.pose != state.pose

        if last_state != state or do_update:
            p_robot_state.publish(state)
            js = JointState()
            js.name = [
                "joint_1", "joint_2", "joint_7", "joint_3", "joint_4",
                "joint_5", "joint_6", "joint_tip"
            ]
            js.position = [
                state.joints[0], -1 * (state.joints[1] - pi / 2),
                state.joints[6], -1 * state.joints[2], state.joints[3],
                -1 * state.joints[4], state.joints[5], 0.0
            ]
            p_joint_states.publish(js)

            do_update = False

        last_state = state
        stored_pose_count = state.storedPoses

    except Exception as e:
        status(name, "Robot update failed: %s" % (str(e)), STATE.ERROR)

    return state
예제 #44
0
        if mapped >= jointRange[1]:
            return jointRange[1]
        elif mapped <= jointRange[0]:
            return jointRange[0]
        else:
            return mapped


if __name__ == '__main__':

    gp = Grasper()
    gp.readGloveCalibration()

    rospy.init_node('contact_publisher')
    rate = rospy.Rate(60)

    while not rospy.is_shutdown():
        msg = JointState()
        msg.name = [
            'jaco_arm_0_joint', 'jaco_arm_1_joint', 'jaco_arm_2_joint',
            'jaco_arm_3_joint', 'jaco_arm_4_joint', 'jaco_arm_5_joint',
            'jaco_finger_joint_0', 'jaco_finger_joint_2', 'jaco_finger_joint_4'
        ]

        msg.position[0:6] = gp.jointsPos
        msg.position[6:9] = gp.fingersPos

        print(msg.position)
        gp.pub.publish(msg)
        rate.sleep()
예제 #45
0
 def publish_joints(self, joints):
     joint_state = JointState()
     joint_state.header.stamp = rospy.Time.now()
     joint_state.name = self.group.get_joints()[:-1]
     joint_state.position = joints
     self.joint_state_publisher.publish(joint_state)
예제 #46
0
    def IK(self, T):

        # getting the desired base to end effector pose from the Transform message of geometry_msgs message
        x_desired = T

        # a list to store the final joint position guess
        final_joint_pos = []
        current_joint_state = JointState()

        # index the right joint names with the correct joint state
        current_joint_state.name = self.active_joint_names

        # initialize a zeros list for the guess
        q_c_rand = numpy.zeros((self.active_num_joints))

        # iteration variables
        l = 0
        m = 0
        n = 0

        # boolean condition
        found = False

        # main loop for number of tries
        while not found and n < 50:
            m = 0
            n = n + 1

            # loop for getting random position values for performing inverse kinematics
            for k in range(self.active_num_joints):
                N = random.uniform(-1 * math.pi, 1 * math.pi)
                q_c_rand[k] = N
            q = True

            # loop for performing convergence of delta x which is the displacement between the desired end effector pose and the current end effector pose
            while q == True and m < 250:

                # updates the position in the joint state with every new guess
                current_joint_state.position = q_c_rand

                # gets new base to end effector and joint transforms from the forward kinematics using the joint state of inverse kinematics
                x_cur, joint_trans = self.forward_kinematics(
                    current_joint_state)

                L = len(joint_trans)

                # calculation of the end effector to base for calculation purposes
                x_cur_inv = tf.transformations.inverse_matrix(x_cur)

                # getting the difference in the target end effector pose to the current end effector pose
                ee_ee_ik = tf.transformations.concatenate_matrices(
                    x_cur_inv, x_desired)

                # getting the rotational component of the transform
                r_ee_ik = ee_ee_ik[:3, :3]
                A, B = rotation_from_matrix(r_ee_ik)

                # getting the rotational part of delta x for a gain of 1
                r_v_ee_ik = numpy.dot(A, B)

                # getting the translational part of delta x for a gain of 1
                t_v_ee_ik = tf.transformations.translation_from_matrix(
                    ee_ee_ik)

                # list of both the translational and rotational parts of delta x
                del_x_ik = numpy.append(t_v_ee_ik, r_v_ee_ik)

                # Jacobian calculation and assembly loop
                Jacobian = numpy.empty((6, 0))
                for i in range(L):
                    b_j = joint_trans[i]
                    j_b = tf.transformations.inverse_matrix(b_j)
                    j_ee = numpy.dot(j_b, x_cur)
                    ee_j = tf.transformations.inverse_matrix(j_ee)
                    R_ee_j = ee_j[:3, :3]
                    t_j_ee = tf.transformations.translation_from_matrix(j_ee)
                    s = skew_mat(t_j_ee)
                    S = numpy.dot(-1 * R_ee_j, s)
                    A = numpy.append(R_ee_j, S, axis=1)
                    B = numpy.append(numpy.zeros([3, 3]), R_ee_j, axis=1)
                    # velocity transformation matrix
                    vj = numpy.append(A, B, axis=0)
                    # x axis alignment
                    if (self.joint_axes[i] == ([1, 0, 0])):
                        Jacobian = numpy.column_stack((Jacobian, vj[:, 3]))
                    # y axis alignment
                    elif (self.joint_axes[i] == ([0, 1, 0])):
                        Jacobian = numpy.column_stack((Jacobian, vj[:, 4]))
                    # z axis alignment
                    elif (self.joint_axes[i] == ([0, 0, 1])):
                        Jacobian = numpy.column_stack((Jacobian, vj[:, 5]))
                    # negative x axis alignment
                    elif (self.joint_axes[i] == ([-1, 0, 0])):
                        Jacobian = numpy.column_stack(
                            (Jacobian, -1 * vj[:, 3]))
                    # negative y axis alignment
                    elif (self.joint_axes[i] == ([0, -1, 0])):
                        Jacobian = numpy.column_stack(
                            (Jacobian, -1 * vj[:, 4]))
                    # negative z axis alignment
                    elif (self.joint_axes[i] == ([0, 0, -1])):
                        Jacobian = numpy.column_stack(
                            (Jacobian, -1 * vj[:, 5]))
                    # fixed axis ignorance
                    elif (self.joint_axes[i] == 'None'):
                        continue

                # Calculation of pseudo Jacobian inverse
                J_pseudo_inv_ik = numpy.linalg.pinv(Jacobian)

                # calculation of joint displacement
                del_q = numpy.dot(J_pseudo_inv_ik, del_x_ik)
                # print del_q
                # calculation of new increment in joint value
                q_c_rand = q_c_rand + del_q

                # to check if delta x is within allowable tolerance ; tolerance = 0.01 (can be changed)
                for j in range(len(del_x_ik)):
                    if (numpy.linalg.norm(del_x_ik) < 0.0001):
                        l = l + 1
                if l == len(del_x_ik):
                    final_joint_pos = q_c_rand
                    if self.is_state_valid(final_joint_pos) == True:
                        found = True
                        for i in range(7):
                            if final_joint_pos[i] > 2 * math.pi:
                                final_joint_pos[i] = numpy.mod(
                                    final_joint_pos[i], 2 * math.pi)
                            elif final_joint_pos[i] < -2 * math.pi:
                                final_joint_pos[i] = numpy.mod(
                                    final_joint_pos[i], -2 * math.pi)
                        if final_joint_pos[i] > math.pi:
                            final_joint_pos[i] = final_joint_pos[i] - (2 *
                                                                       math.pi)
                        elif final_joint_pos[i] < -1 * math.pi:
                            final_joint_pos[i] = final_joint_pos[i] + (2 *
                                                                       math.pi)
                        return final_joint_pos
                else:
                    # iteration count
                    m = m + 1
                    if m == 250:
                        if n < 20:
                            print 'taking new guess'
                        q = False
            # allowing maximum tries of 20
            if n == 20:
                print 'reached maximum number of tries please check if the end effector is in the workspace of the robot and try again'
                return 0
예제 #47
0
    def __init__(self):
        rospy.logdebug("Starting URSimReaching Class object...")

        # Init GAZEBO Objects
        self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state',
                                                SetModelState)
        self.get_world_state = rospy.ServiceProxy(
            '/gazebo/get_world_properties', GetWorldProperties)

        # Subscribe joint state and target pose
        rospy.Subscriber("/joint_states", JointState,
                         self.joints_state_callback)
        rospy.Subscriber("/target_blocks_pose", Point,
                         self.target_point_callback)
        rospy.Subscriber("/gazebo/link_states", LinkStates,
                         self.link_state_callback)

        # Gets training parameters from param server
        self.desired_pose = Pose()
        self.running_step = rospy.get_param("/running_step")
        self.max_height = rospy.get_param("/max_height")
        self.min_height = rospy.get_param("/min_height")
        self.observations = rospy.get_param("/observations")

        # Joint limitation
        shp_max = rospy.get_param("/joint_limits_array/shp_max")
        shp_min = rospy.get_param("/joint_limits_array/shp_min")
        shl_max = rospy.get_param("/joint_limits_array/shl_max")
        shl_min = rospy.get_param("/joint_limits_array/shl_min")
        elb_max = rospy.get_param("/joint_limits_array/elb_max")
        elb_min = rospy.get_param("/joint_limits_array/elb_min")
        wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
        wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
        wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
        wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
        wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
        wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
        self.joint_limits = {
            "shp_max": shp_max,
            "shp_min": shp_min,
            "shl_max": shl_max,
            "shl_min": shl_min,
            "elb_max": elb_max,
            "elb_min": elb_min,
            "wr1_max": wr1_max,
            "wr1_min": wr1_min,
            "wr2_max": wr2_max,
            "wr2_min": wr2_min,
            "wr3_max": wr3_max,
            "wr3_min": wr3_min
        }

        shp_init_value = rospy.get_param("/init_joint_pose/shp")
        shl_init_value = rospy.get_param("/init_joint_pose/shl")
        elb_init_value = rospy.get_param("/init_joint_pose/elb")
        wr1_init_value = rospy.get_param("/init_joint_pose/wr1")
        wr2_init_value = rospy.get_param("/init_joint_pose/wr2")
        wr3_init_value = rospy.get_param("/init_joint_pose/wr3")
        self.init_joint_pose = [
            shp_init_value, shl_init_value, elb_init_value, wr1_init_value,
            wr2_init_value, wr3_init_value
        ]

        # Fill in the Done Episode Criteria list
        self.episode_done_criteria = rospy.get_param("/episode_done_criteria")

        # stablishes connection with simulator
        self._gz_conn = GazeboConnection()
        self._ctrl_conn = ControllersConnection(namespace="")

        # Controller type for ros_control
        self._ctrl_type = rospy.get_param("/control_type")
        self.pre_ctrl_type = self._ctrl_type

        # We init the observations
        self.base_orientation = Quaternion()
        self.target_point = Point()
        self.link_state = LinkStates()
        self.joints_state = JointState()
        self.end_effector = Point()
        self.distance = 0.

        # Arm/Control parameters
        self._ik_params = setups['UR5_6dof']['ik_params']

        # ROS msg type
        self._joint_pubisher = JointPub()
        self._joint_traj_pubisher = JointTrajPub()

        # Gym interface and action
        self.action_space = spaces.Discrete(6)
        self.observation_space = 15  #np.arange(self.get_observations().shape[0])
        self.reward_range = (-np.inf, np.inf)
        self._seed()

        # Change the controller type
        set_joint_vel_server = rospy.Service('/set_velocity_controller',
                                             SetBool, self._set_vel_ctrl)
        set_joint_traj_vel_server = rospy.Service(
            '/set_trajectory_velocity_controller', SetBool,
            self._set_traj_vel_ctrl)

        self.vel_traj_controller = [
            'joint_state_controller', 'gripper_controller',
            'vel_traj_controller'
        ]
        self.vel_controller = [
            "joint_state_controller", "gripper_controller",
            "ur_shoulder_pan_vel_controller",
            "ur_shoulder_lift_vel_controller", "ur_elbow_vel_controller",
            "ur_wrist_1_vel_controller", "ur_wrist_2_vel_controller",
            "ur_wrist_3_vel_controller"
        ]

        # Helpful False
        self.stop_flag = False
        stop_trainning_server = rospy.Service('/stop_training', SetBool,
                                              self._stop_trainnig)
        start_trainning_server = rospy.Service('/start_training', SetBool,
                                               self._start_trainnig)
예제 #48
0
#!/usr/bin/env python

# Original code https://github.com/corot/turtlebot_arm.git

import rospy
from sensor_msgs.msg import JointState

rospy.init_node("fake_joint_pub")
p = rospy.Publisher('joint_states', JointState, queue_size=5)

msg = JointState()
msg.name = ["gripper_link_joint"]
msg.position = [0.0 for name in msg.name]
msg.velocity = [0.0 for name in msg.name]

while not rospy.is_shutdown():
    msg.header.stamp = rospy.Time.now()
    p.publish(msg)
    rospy.sleep(0.1)

예제 #49
0
def ik_service_client(limb = "right", use_advanced_options = False):
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    poses = {
        'right': PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point(
                    x=0.450628752997,
                    y=0.161615832271,
                    z=0.217447307078,
                ),
                orientation=Quaternion(
                    x=0.704020578925,
                    y=0.710172716916,
                    z=0.00244101361829,
                    w=0.00194372088834,
                ),
            ),
        ),
    }
    # Add desired pose for inverse kinematics
    ikreq.pose_stamp.append(poses[limb])
    # Request inverse kinematics from base to "right_hand" link
    ikreq.tip_names.append('right_hand')

    if (use_advanced_options):
        # Optional Advanced IK parameters
        rospy.loginfo("Running Advanced IK Service Client example.")
        # The joint seed is where the IK position solver starts its optimization
        ikreq.seed_mode = ikreq.SEED_USER
        seed = JointState()
        seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
                     'right_j4', 'right_j5', 'right_j6']
        seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
        ikreq.seed_angles.append(seed)

        # Once the primary IK task is solved, the solver will then try to bias the
        # the joint angles toward the goal joint configuration. The null space is 
        # the extra degrees of freedom the joints can move without affecting the
        # primary IK task.
        ikreq.use_nullspace_goal.append(True)
        # The nullspace goal can either be the full set or subset of joint angles
        goal = JointState()
        goal.name = ['right_j1', 'right_j2', 'right_j3']
        goal.position = [0.1, -0.3, 0.5]
        ikreq.nullspace_goal.append(goal)
        # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
        # If empty, the default gain of 0.4 will be used
        ikreq.nullspace_gain.append(0.4)
    else:
        rospy.loginfo("Running Simple IK Service Client example.")

    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException) as e:
        rospy.logerr("Service call failed: %s" % (e,))
        return False

    # Check if result valid, and type of seed ultimately used to get solution
    if (resp.result_type[0] > 0):
        seed_str = {
                    ikreq.SEED_USER: '******',
                    ikreq.SEED_CURRENT: 'Current Joint Angles',
                    ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
                   }.get(resp.result_type[0], 'None')
        rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
              (seed_str,))
        # Format solution into Limb API-compatible dictionary
        limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position)))
        rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints)
        rospy.loginfo("------------------")
        rospy.loginfo("Response Message:\n%s", resp)
    else:
        rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
        rospy.logerr("Result Error %d", resp.result_type[0])
        return False

    return True
    def initializeHeadLiftJoint(self):
        lift_goal = JointState()
        head_goal = JointState()

        lift_goal.name = ["torso_lift_joint"]
        head_goal.name = ["head_2_joint", "head_1_joint"]

        lift_goal.position = [0.3]

        head_goal.position = [-0.9, 0.0]

        head_goal.effort = [0.0, 0.0]
        lift_goal.effort = [0.0]

        head_goal.velocity = [0.0, 0.0]
        lift_goal.velocity = [0.0]

        self.lift_goal_pub.publish(lift_goal)
        self.head_goal_pub.publish(head_goal)
예제 #51
0
def main():
    print "INITIALIZING ARM NODE IN SIMULATION BY [EDD-II]"
    ###Connection with ROS
    rospy.init_node("head_simul_node")
    br = tf.TransformBroadcaster()
    jointStates = JointState()
    jointStates.name = [
        "arm_flex_joint", "arm_roll_joint", "wrist_flex_joint",
        "wrist_roll_joint"
    ]
    jointStates.position = [0, 0, 0, 0]

    subPosition = rospy.Subscriber("/hardware/arm/goal_pose",
                                   Float32MultiArray, callbackPos)
    pubArmPose = rospy.Publisher("/hardware/arm/current_pose",
                                 Float32MultiArray,
                                 queue_size=1)

    #For test, these lines can be unneeded
    pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
    pubArmBattery = rospy.Publisher("/hardware/robot_state/right_arm_battery",
                                    Float32,
                                    queue_size=1)

    loop = rospy.Rate(10)

    global goalAngles
    global goalGripper
    global speeds
    goalAngles = [0, 0, 0, 0]
    angles = [0, 0, 0, 0]
    speeds = [0.01, 0.01, 0.01, 0.01]
    goalGripper = 0
    gripper = 0
    gripperSpeed = 0.1

    msgCurrentPose = Float32MultiArray()
    msgCurrentPose.data = [0, 0, 0, 0]
    msgCurrentGripper = Float32()
    msgCurrentGripper.data = 0
    deltaAngles = [0, 0, 0, 0]
    deltaGripper = 0
    while not rospy.is_shutdown():
        for i in range(len(deltaAngles)):
            deltaAngles[i] = goalAngles[i] - angles[i]
            if deltaAngles[i] > speeds[i]:
                deltaAngles[i] = speeds[i]
            if deltaAngles[i] < -speeds[i]:
                deltaAngles[i] = -speeds[i]
            angles[i] += deltaAngles[i]
            jointStates.position[i] = angles[i]
            msgCurrentPose.data[i] = angles[i]

        deltaGripper = goalGripper - gripper
        if deltaGripper > gripperSpeed:
            deltaGripper = gripperSpeed
        if deltaGripper < -gripperSpeed:
            deltaGripper = -gripperSpeed
        gripper += deltaGripper

        jointStates.header.stamp = rospy.Time.now()
        pubJointStates.publish(jointStates)
        pubArmPose.publish(msgCurrentPose)
        msgBattery = Float32()
        msgBattery.data = 11.6
        pubArmBattery.publish(msgBattery)
        loop.sleep()
예제 #52
0
#!/usr/bin/env python 
import rospy
from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState
from tocabi_controller.msg import model

import pinocchio
import numpy as np
from sys import argv
from os.path import dirname, join, abspath

qmsg = JointState()
modelmsg = model()
start = False

def callback(data):
      global qmsg
      global start 
      qmsg = data
      start = True

def talker():
      rospy.init_node("tocabi_pinocchio", anonymous = False)
      rospy.Subscriber("/tocabi/pinocchio/jointstates", JointState, callback)
      global model
      pub = rospy.Publisher("/tocabi/pinocchio", model, queue_size=1)
      modelmsg.CMM = [0 for i in range(33*6)]
      modelmsg.COR = [0 for i in range(33*33)]
      modelmsg.M = [0 for i in range(33*33)]
      modelmsg.g = [0 for i in range(33)]
예제 #53
0
    def __init__(self, sock=None, ip="192.168.1.108"):
        '''
        constructor
        :param socket: can optionally pass in a socket if this class is used in a larger framework
        :param ip: ip address of robot
        :return:
        '''
        if not sock == None:
            self.sock = sock
        else:
            self.HOST = ip
            self.PORT = 30003
            self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.sock.connect((self.HOST, self.PORT))
            self.sock.settimeout(2)  # very important!
            # print self.sock.recv(1024)

        #  variables
        self.line = ''
        self.messageSize = 1060
        self.time = 0.0  # time elapsed since the controller was started
        self.q_target = 6 * [0.0]  # target joint positions
        self.qd_target = 6 * [0.0]  # target joint velocities
        self.qdd_target = 6 * [0.0]  # target joint accelerations
        self.i_target = 6 * [0.0]  # target joint currents
        self.m_target = 6 * [0.0]  # target torques
        self.q_actual = 6 * [0.0]  # actual joint positions
        self.qd_actual = 6 * [0.0]  # actual joint velocities
        self.i_actual = 6 * [0.0]  # actual joint currents
        self.i_control = 6 * [0.0]  # joint control currents
        self.tool_vec_actual = 6 * [
            0.0
        ]  # actual cartesian coordinates of the tool (x,y,z,rx,ry,rz)
        self.tcp_speed_actual = 6 * [
            0.0
        ]  # actual speed of the tool given in cartesian coords
        self.tcp_force = 6 * [0.0]  # genearlized forces in the tcp
        self.tool_vec_target = 6 * [
            0.0
        ]  # target cartesian coordinates of the tool (x,y,z,rx,ry,rz)
        self.tcp_speed_target = 6 * [
            0.0
        ]  # target speed of the tool given in cartesian coords
        self.digital_input_bits = 0.0  # crrent state of the digital inputs
        self.motor_temps = 6 * [0.0]  # motor temperatures (celsius)
        self.controller_timer = 0.0  # controller realtime thread execution time
        self.test_value = 0.0  # a value used by UR software only
        self.robot_mode = 0.0  # robot mode
        self.joint_modes = 6 * [0.0]  # joint control modes
        self.safety_mode = 0.0
        self.tool_accel_values = 3 * [0.0]  # tool accelerometer values
        self.speed_scaling = 0.0  # speed scaling of the trajectory limiter
        self.lin_momentum_norm = 0.0  # norm of cartesian linear momentum
        self.v_main = 0.0  # masterboard: main voltage
        self.v_robot = 0.0  #masterboard:robot voltage (48v)
        self.i_robot = 0.0  # materboard: robot current
        self.v_actual = 6 * [0.0]  # actual joint voltages
        self.digital_outputs = 0.0  # digitial outputs
        self.program_state = 0.0  # program state
        self.joint_state = JointState()
        self.joint_state.name = [
            'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
            'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
        ]
예제 #54
0
 def set_joint_state(self,joint_target):
     self.joint_target = joint_target
     joint_state = JointState()
     joint_state.position = tuple(joint_target)
     self.pub.publish(joint_state)
예제 #55
0
robot = RobotCommander()
scene = PlanningSceneInterface()
group = MoveGroupCommander("manipulator")

directory = "/home/abd-alrahman/catkin_ws/src/attrobot/plotting/1_55_23_pos/itr_0.csv"
time_algo, time, thetas, dthetas, thetas_sp, dthetas_sp = Read_Loggings(
    directory)

rospy.wait_for_service('/compute_fk')
fk_solution = rospy.ServiceProxy('/compute_fk', GetPositionFK)
ref_frame = Header()
ref_frame.frame_id = "base_link"
ref_frame.stamp = rospy.Time.now()
# fk_link_names
fk_link_names = ["racket"]
joint_state = JointState()
joint_state.header = Header()
joint_state.header.frame_id = "racket"
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
    "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
]
x_sp_list = []
y_sp_list = []
z_sp_list = []
x_list = []
y_list = []
z_list = []

for i in range(len(thetas_sp)):
    joint_state.position = [
예제 #56
0
    def __init__(self):
        super().__init__("ros2_dorna_utilities")

        self.namespace = ""

        if len(sys.argv) != 2:
            self.namespace = sys.argv[1]
        else:
            pass

        self.utils_to_gui_msg = DornaUtilsToGui()
        self.joint_state = JointState()

        self.utils_to_gui_timer_period = 0.5

        # self.utils_to_esd_msg.actual_pose = ""
        self.utils_to_gui_msg.saved_poses = []
        # self.utils_to__msg.echo_utility_action = ""
        # self.utils_to_esd_msg.echo_utility_pose_name = ""

        self.act_pos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        self.prev_action = ""
        self.prev_pose_name = ""

        if self.namespace != "":
            self.joint_names = [
                self.namespace + "/" + "dorna_axis_1_joint",
                self.namespace + "/" + "dorna_axis_2_joint",
                self.namespace + "/" + "dorna_axis_3_joint",
                self.namespace + "/" + "dorna_axis_4_joint",
                self.namespace + "/" + "dorna_axis_5_joint", "nonexistent"
            ]
        else:
            self.joint_names = [
                "dorna_axis_1_joint", "dorna_axis_2_joint",
                "dorna_axis_3_joint", "dorna_axis_4_joint",
                "dorna_axis_5_joint", "nonexistent"
            ]

        self.joints_input = os.path.join(
            get_package_share_directory('ros2_dorna_utilities'), 'poses',
            'joint_poses.csv')
        self.joints_oldposes = os.path.join(
            get_package_share_directory('ros2_dorna_utilities'), 'poses',
            'joint_oldposes.csv')
        self.joints_newposes = os.path.join(
            get_package_share_directory('ros2_dorna_utilities'), 'poses',
            'joint_newposes.csv')

        self.joint_state_subscriber = self.create_subscription(
            JointState, "/dorna_joint_states", self.joint_callback, 10)

        self.gui_to_utils_subscriber = self.create_subscription(
            DornaGuiToUtils, "/dorna_gui_to_utils", self.gui_to_utils_callback,
            10)

        time.sleep(2)

        self.utils_to_gui_publisher_ = self.create_publisher(
            DornaUtilsToGui, "/dorna_utils_to_gui", 10)

        self.utils_to_gui_timer = self.create_timer(
            self.utils_to_gui_timer_period, self.utils_to_gui_callback)
예제 #57
0
def sign(value):
	if value >= 0:
		return 1
	else:
		return -1
if __name__ == '__main__':
	ser = serial.Serial('/dev/ttyUSB0', timeout=1.0)
	dif = DifControl(ser)
	dif.init_motors(10, 20, 20)
	v_X_targ = 0
	w_Z_targ = 0
	rospy.init_node("differential_control")
	sub_cmd_vel = rospy.Subscriber("/cmd_vel", Twist, cb_cmd_vel, queue_size = 4)
	joints_states_pub = rospy.Publisher("/joint_states_wheels", JointState, queue_size = 4)
	msg = JointState()
	msg.name.append("right")
	msg.name.append("left")
	msg.velocity.append(0)
	msg.velocity.append(0)
	while not rospy.is_shutdown():
		try:
			dif.set_speed(v_X_targ, w_Z_targ)
			v_r, v_l = dif.get_motors_speed()
			msg.velocity[0] = v_r
			msg.velocity[1] = v_l
			joints_states_pub.publish(msg)
			sleep(0.1)
		except KeyboardInterrupt:
			break
예제 #58
0
            rospy.sleep(1)
            #mk_fig.mk_figures()
            break


if __name__ == '__main__':
    try:

        rospy.init_node('gripper_node')
        #spin_rate = rospy.Rate(1)
        # 各クラスのインスタンス化
        force_sensor_capture = ForceSensorCapture()
        joint_controller = JointController()

        #sub_flag = Sub_handing_end_flag()
        initial_position = JointState()
        initial_position.name.extend([
            'arm_lift_joint', 'arm_flex_joint', 'arm_roll_joint',
            'wrist_flex_joint', 'wrist_roll_joint', 'hand_motor_joint'
        ])
        initial_position.position.extend([0.0, 0.0, 0.0, -1.57, 0.0, 1.2])

        release = JointState()
        release.name.extend(['hand_motor_joint'])
        release.position.extend([1.1])

        #csvファイルの準備
        path = '/home/naoyamada/catkin_ws3/src/pre_experiment/scripts/csv/f/raw_data.csv'
        path2 = '/home/naoyamada/catkin_ws3/src/pre_experiment/scripts/csv/f/ave_data.csv'
        os.remove(path)
        f = open(path, 'w')
예제 #59
0
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import JointState

rospy.init_node("sim_velocity")
pub = rospy.Publisher('/joint_state',JointState,queue_size=10)

step = 0.1
pose = [0,0,0,0]
vel = [step,-step,step,-step]
js_msg = JointState()
rate  = rospy.Rate(10)

while not rospy.is_shutdown():
    js_msg.header.stamp = rospy.Time.now()
    js_msg.position = pose
    js_msg.velocity = vel
    pose = [el+step for el in pose]
    pub.publish(js_msg)
    rate.sleep()
예제 #60
0
def np_array_to_joint_state_effort(array):
    data = JointState()
    for i in range(0, array.size):
        data.effort.append(array[i])
    return data