Пример #1
4
 def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
     """
     Interpolate a trajectory from a start state (or current state) to a goal in joint space
     :param goal: A RobotState to be used as the goal of the trajectory
     param total_time: The time to execute the trajectory
     :param nb_points: Number of joint-space points in the final trajectory
     :param start: A RobotState to be used as the start state, joint order must be the same as the goal
     :return: The corresponding RobotTrajectory
     """
     dt = total_time*(1.0/nb_points)
     # create the joint trajectory message
     traj_msg = JointTrajectory()
     rt = RobotTrajectory()
     if start == None:
         start = self.get_current_state()
     joints = []
     start_state = start.joint_state.position
     goal_state = goal.joint_state.position
     for j in range(len(goal_state)):
         pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1)
         joints.append(pose_lin[1:].tolist())
     for i in range(nb_points):
         point = JointTrajectoryPoint()
         for j in range(len(goal_state)):
             point.positions.append(joints[j][i])
         # append the time from start of the position
         point.time_from_start = rospy.Duration.from_sec((i+1)*dt)
         # append the position to the message
         traj_msg.points.append(point)
     # put name of joints to be moved
     traj_msg.joint_names = self.joint_names()
     # send the message
     rt.joint_trajectory = traj_msg
     return rt
Пример #2
0
 def send_command(self, cmd):
     rospy.logdebug("  Sending cmd to controller [%s]"%self._controller_id)
     cmd_msg = JointTrajectory()
     cmd_msg.header.stamp = rospy.Time().now()
     cmd_msg.joint_names = self._config["joint_names"]
     cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]]
     self._pub.publish(cmd_msg)
def main():

    pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
    rospy.init_node('traj_test')

    p1 = JointTrajectoryPoint()
    p1.positions = [0,0,0,0,0,0,0]
    p1.velocities = [0,0,0,0,0,0,0]
    p1.accelerations = [0,0,0,0,0,0,0]

    p2 = JointTrajectoryPoint()
    p2.positions = [0,0,1.0,0,-1.0,0,0]
    p2.velocities = [0,0,0,0,0,0,0]
    p2.accelerations = [0,0,0,0,0,0,0]
    p2.time_from_start = rospy.Time(4.0)

    p3 = JointTrajectoryPoint()
    p3.positions = [0,0,-1.0,0,1.0,0,0]
    p3.velocities = [0,0,0,0,0,0,0]
    p3.accelerations = [0,0,0,0,0,0,0]
    p3.time_from_start = rospy.Time(20.0)


    traj = JointTrajectory()
    traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
    traj.points = [p1,p2,p3]


    rospy.loginfo(traj)

    r = rospy.Rate(1) # 10hz
    pub.publish(traj)
    r.sleep()
    pub.publish(traj)
    def convert_trajectory(self, traj):
        """ Converts a trajectory into a joint trajectory
        Args:
            traj: Trajectory to convert
        Returns:
            joint trajectory
        """
        new_traj = JointTrajectory()

        new_traj.header = traj.header
        # Take each joint in the trajectory and add it to the joint trajectory
        for joint_name in traj.joint_names:
            new_traj.joint_names.append(self.joint_map[joint_name].joint_name)
        # For each poiint in the trajectory
        for point in traj.points:
            new_point = JointTrajectoryPoint()
            for i, pos in enumerate(point.positions):
                new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos))
            for i, vel in enumerate(point.velocities):
                new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel))

            new_point.time_from_start = point.time_from_start
            new_traj.points.append(new_point)

        return new_traj
Пример #5
0
    def create_placings_from_object_pose(self, posestamped):
        """ Create a list of PlaceLocation of the object rotated every 15deg"""
        place_locs = []
        pre_grasp_posture = JointTrajectory()
        # Actually ignored....
        pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id
        pre_grasp_posture.joint_names = [
            name for name in self._gripper_joint_names.split()]
        jtpoint = JointTrajectoryPoint()
        jtpoint.positions = [
            float(pos) for pos in self._gripper_pre_grasp_positions.split()]
        jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
        pre_grasp_posture.points.append(jtpoint)
        # Generate all the orientations every step_degrees_yaw deg
        for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)):
            pl = PlaceLocation()
            pl.place_pose = posestamped
            newquat = quaternion_from_euler(0.0, 0.0, yaw_angle)
            pl.place_pose.pose.orientation = Quaternion(
                newquat[0], newquat[1], newquat[2], newquat[3])
            # TODO: the frame is ignored, this will always be the frame of the gripper
            # so arm_tool_link
            pl.pre_place_approach = self.createGripperTranslation(
                Vector3(1.0, 0.0, 0.0))
            pl.post_place_retreat = self.createGripperTranslation(
                Vector3(-1.0, 0.0, 0.0))

            pl.post_place_posture = pre_grasp_posture
            place_locs.append(pl)

        return place_locs
Пример #6
0
    def __create_spin_command(self, arm, theta=math.pi):
        if arm == 'l':
            jnts = self.robot_state.left_arm_positions 
        if arm == 'r':
            jnts = self.robot_state.right_arm_positions 
        
        command = JointTrajectory()
        command.joint_names = ['%s_shoulder_pan_joint' % arm[0], 
                               '%s_shoulder_lift_joint' % arm[0],
                               '%s_upper_arm_roll_joint' % arm[0],
                               '%s_elbow_flex_joint' % arm[0],
                               '%s_forearm_roll_joint' % arm[0],
                               '%s_wrist_flex_joint' % arm[0],
                               '%s_wrist_roll_joint' % arm[0]]
        
        jnts[-1] += theta
        command.points.append(JointTrajectoryPoint(
            positions=jnts,
            velocities = [0.0] * (len(command.joint_names)),
            accelerations = [],
            time_from_start =  rospy.Duration(0.1)))

        goal = JointTrajectoryGoal()
        goal.trajectory = command
        return goal
Пример #7
0
    def check_collision(self):
        if not self.collsion_srv:
            return False

        # Create a new trajectory by combining both the trajectories, assumption is that the timing is same
        # The order of joining should be [left + right]
        self.adjust_traj_length()
        traj = JointTrajectory()
        traj.joint_names = self.left._goal.trajectory.joint_names + self.right._goal.trajectory.joint_names

        no_points = len(self.left._goal.trajectory.points)
        for index in xrange(no_points):
            pt = JointTrajectoryPoint()
            left_pt = self.left._goal.trajectory.points[index]
            right_pt = self.right._goal.trajectory.points[index]

            pt.positions = left_pt.positions + right_pt.positions
            pt.velocities = left_pt.velocities + right_pt.velocities
            pt.time_from_start = left_pt.time_from_start
            traj.points.append(pt)

        msg = CollisionCheckRequest()
        msg.trajectory = traj

        try:
            collision_service = rospy.ServiceProxy("collision_check", CollisionCheck)
            resp = collision_service(msg)
            if resp.collision_found:
                rospy.logwarn("Collision Found")
            else:
                rospy.loginfo("Collision not found, good to go :)")
            return resp.collision_found
        except rospy.ServiceException as e:
            rospy.logwarn("Exception on collision check {}".format(e))
            return True
    def send_arm_goal(self, arm_goal):
        arm_joint_names = ['joint_lift',
                           'joint_waist',
                           'joint_big_arm',
                           'joint_forearm',
                           'joint_wrist_pitching',
                           'joint_wrist_rotation']

        rospy.loginfo("Waiting for follow_joint_trajectory server")
        self.arm_client.wait_for_server()
        rospy.loginfo("Connected to follow_joint_trajectory server")
        rospy.sleep(1)
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joint_names
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].time_from_start = rospy.Duration(10)
        rospy.loginfo("Preparing for moving the arm to goal position!")
        rospy.sleep(1)
        arm_goal_pos = FollowJointTrajectoryGoal()
        arm_goal_pos.trajectory = arm_trajectory

        arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
        self.arm_client.send_goal(arm_goal_pos)
        rospy.loginfo("Send goal to the trajectory server successfully!")
        self.arm_client.wait_for_result()
Пример #9
0
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"):
    """
    This function will convert the plan to a joint trajectory compatible with the 
    /arm_N/arm_controller/command topic
    """
    theplan = plan

    # create joint trajectory message
    jt = JointTrajectory()

    # fill the header
    jt.header.seq = seq
    jt.header.stamp.secs = 0  # secs
    jt.header.stamp.nsecs = 0  # nsecs
    jt.header.frame_id = frame_id

    # specify the joint names
    jt.joint_names = theplan.joint_trajectory.joint_names

    # fill the trajectory points and computer constraint times
    njtp = len(theplan.joint_trajectory.points)
    for ii in range(0, njtp):
        jtp = JointTrajectoryPoint()
        jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
        jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1))
        jt.points.append(jtp)
    return jt
    def __init__(self):
        rospy.init_node('arm_simple_trajectory')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Which joints define the arm?
        arm_joints = ['joint_lift',
                      'joint_waist',
                      'joint_big_arm', 
                      'joint_forearm',
                      'joint_wrist_pitching',
                      'joint_wrist_rotation']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        else:
            # Set a goal configuration for the arm
            arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0]
    
        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for xm arm trajectory controller...')
        
        arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory',
                                                  FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        rospy.sleep(1)
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        rospy.sleep(1)
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        rospy.loginfo('...done')
        rospy.sleep(1)
Пример #11
0
    def joint_trajectory(self, joint_trajectory):
        '''
        Returns just the part of the trajectory corresponding to the arm.

        **Args:**
        
            **joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert

        **Returns:**
            A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory

        **Raises:**
            
            **ValueError:** If some arm joint is not found in joint_trajectory
        '''
        arm_trajectory = JointTrajectory()
        arm_trajectory.header = copy.deepcopy(joint_trajectory.header)
        arm_trajectory.joint_names = copy.copy(self.joint_names)
        indexes = [-1]*len(self.joint_names)
        for (i, an) in enumerate(self.joint_names):
            for (j, n) in enumerate(joint_trajectory.joint_names):
                if an == n:
                    indexes[i] = j
                    break
            if indexes[i] < 0:
                raise ValueError('Joint '+n+' is missing from joint trajectory')
        
        for joint_point in joint_trajectory.points:
            arm_point = JointTrajectoryPoint()
            arm_point.time_from_start = joint_point.time_from_start
            for i in indexes:
                arm_point.positions.append(joint_point.positions[i])
            arm_trajectory.points.append(arm_point)
        return arm_trajectory
Пример #12
0
    def fill_ik_traj(self, poses, dist=None, duration=None, tot_points=None):
        if dist is None:
            dist = self.calc_dist(poses[0], poses[-1])
        if duration is None:
            duration = dist*30
        if tot_points is None:
           tot_points = 500*dist
            
        ik_fracs = np.linspace(0, 1, len(poses))   #A list of fractional positions along course to evaluate ik

        req = self.form_ik_request(poses[0])
        ik_goal = self.ik_pose_proxy(req)
        seed = ik_goal.solution.joint_state.position

        ik_points = [[0]*7 for i in range(len(poses))]
        for i, p in enumerate(poses):
            request = self.form_ik_request(p)
            request.ik_request.ik_seed_state.joint_state.position = seed
            ik_goal = self.ik_pose_proxy(request)
            ik_points[i] = ik_goal.solution.joint_state.position
            seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
        try:
            ik_points = np.array(ik_points)
        except:
            print "Value error"
            print 'ik_points: ', ik_points
        print 'ik_points_array: ', ik_points
        ang_fracs = np.linspace(0,1, tot_points)  
        
        diff = np.max(np.abs(np.diff(ik_points,1,0)),0)
        print 'diff: ', diff
        for i in xrange(len(ik_points)):
            if ik_points[i,4]<0.:
                ik_points[i,4] += 2*math.pi
        if any(ik_points[:,4]>3.8):
            print "OVER JOINT LIMITS!"
            ik_points[:,4] -= 2*math.pi

        # Linearly interpolate angles between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path.  Used to maintain large number of trajectory points without running IK on every one.    
        angle_points = np.zeros((7, tot_points))
        for i in xrange(7):
            angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])

        #Build Joint Trajectory from angle positions
        traj = JointTrajectory()
        traj.header.frame_id = poses[0].header.frame_id
        traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
        #print 'angle_points', len(angle_points[0]), angle_points
        for i in range(len(angle_points[0])):
            traj.points.append(JointTrajectoryPoint())
            traj.points[i].positions = angle_points[:,i]
            traj.points[i].velocities = [0]*7
            traj.points[i].time_from_start = rospy.Duration(ang_fracs[i]*duration)
        angles_safe = self.check_trajectory(traj)
        if angles_safe:
            print "Check Passed: Angles Safe"
            return traj
        else:
            print "Check Failed: Unsafe Angles"
            return None
Пример #13
0
 def goal_cb(self, pt_msg):
     rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name()))
     if (pt_msg.header.frame_id != self.camera_ik.base_frame):
         try:
             now = pt_msg.header.stamp
             self.tfl.waitForTransform(pt_msg.header.frame_id,
                                       self.camera_ik.base_frame,
                                       now, rospy.Duration(1.0))
             pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg)
         except (LookupException, ConnectivityException, ExtrapolationException):
             rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(),
                                                                                     pt_msg.header.frame_id,
                                                                                     self.camera_ik.base_frame))
     target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z])
     # Get IK Solution
     current_angles = list(copy.copy(self.joint_angles_act))
     iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)])
     # Start with current angles, then replace angles in camera IK with IK solution
     # Use desired joint angles to avoid sagging over time
     jtp = JointTrajectoryPoint()
     jtp.positions = list(copy.copy(self.joint_angles_des))
     for i, joint_name in enumerate(self.camera_ik.arm_joint_names):
         jtp.positions[self.joint_names.index(joint_name)] = iksol[i]
     deltas = np.abs(np.subtract(jtp.positions, current_angles))
     duration = np.max(np.divide(deltas, self.max_vel_rot))
     jtp.time_from_start = rospy.Duration(duration)
     jt = JointTrajectory()
     jt.joint_names = self.joint_names
     jt.points.append(jtp)
     rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name()))
     self.joint_traj_pub.publish(jt)
Пример #14
0
 def move_head(self, pan , tilt):   
     # Which joints define the head?
     head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
     # Create a single-point head trajectory with the head_goal as the end-point
     head_trajectory = JointTrajectory()
     head_trajectory.joint_names = head_joints
     head_trajectory.points.append(JointTrajectoryPoint())
     head_trajectory.points[0].positions = pan , tilt
     head_trajectory.points[0].velocities = [0.0 for i in head_joints]
     head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
     head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
 
     # Send the trajectory to the head action server
     rospy.loginfo('Moving the head to goal position...')
     
     head_goal = FollowJointTrajectoryGoal()
     head_goal.trajectory = head_trajectory
     head_goal.goal_time_tolerance = rospy.Duration(0.0)
 
     # Send the goal
     self.head_client.send_goal(head_goal)
     
     # Wait for up to 5 seconds for the motion to complete 
     self.head_client.wait_for_result(rospy.Duration(2.0))
     
     rospy.loginfo('...done')
Пример #15
0
    def tuck(self):
        # prepare a joint trajectory
        msg = JointTrajectory()
        msg.joint_names = servos
        msg.points = list()
    
        point = JointTrajectoryPoint()
        point.positions = forward
        point.velocities = [ 0.0 for servo in msg.joint_names ]
        point.time_from_start = rospy.Duration(5.0)
        msg.points.append(point)
       # point = JointTrajectoryPoint()
        #point.positions = to_side
        #point.velocities = [ 0.0 for servo in msg.joint_names ]
        #point.time_from_start = rospy.Duration(8.0)
        #msg.points.append(point)
        #point = JointTrajectoryPoint()
        #point.positions = tucked
        #point.velocities = [ 0.0 for servo in msg.joint_names ]
        #point.time_from_start = rospy.Duration(11.0)
        #msg.points.append(point)

        # call action
        msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = msg
        self._client.send_goal(goal)
Пример #16
0
def dual_arm_test():
    pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
    cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory)
    rospy.init_node('dual_arm_test')
    rospy.sleep(1.0)
    while not rospy.is_shutdown():
        traj = JointTrajectory()
        traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint']
        ptn = JointTrajectoryPoint()
        ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
        ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
        ptn.time_from_start = rospy.Duration(2.0)
        traj.header.stamp = rospy.Time.now()
        traj.points.append(ptn)
        pub.publish(traj)
        cob_pub.publish(traj)
        rospy.sleep(3.0)
        ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
        ptn.time_from_start = rospy.Duration(2.0)
        traj.points = []
        traj.header.stamp = rospy.Time.now()
        traj.points.append(ptn)
        pub.publish(traj)
        cob_pub.publish(traj)
        rospy.sleep(3.0)
Пример #17
0
    def exectute_joint_traj(self, joint_trajectory, timing):
        '''Moves the arm through the joint sequence'''

        # First, do filtering on the trajectory to fix the velocities
        trajectory = JointTrajectory()

        # Initialize the server
        # When to start the trajectory: 0.1 seconds from now
        trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
        trajectory.joint_names = self.joint_names

        ## Add all frames of the trajectory as way points
        for i in range(len(timing)):
            positions = joint_trajectory[i].joint_pose
            velocities = [0] * len(positions)
            # Add frames to the trajectory
            trajectory.points.append(JointTrajectoryPoint(positions=positions,
                                     velocities=velocities,
                                     time_from_start=timing[i]))

        output = self.filter_service(trajectory=trajectory,
                                     allowed_time=rospy.Duration.from_sec(20))
        rospy.loginfo('Trajectory for arm ' + str(self.arm_index) +
                      ' has been filtered.')
        traj_goal = JointTrajectoryGoal()

        # TO-DO: check output.error_code
        traj_goal.trajectory = output.trajectory
        traj_goal.trajectory.header.stamp = (rospy.Time.now() +
                                            rospy.Duration(0.1))
        traj_goal.trajectory.joint_names = self.joint_names

        # Sends the goal to the trajectory server
        self.traj_action_client.send_goal(traj_goal)
Пример #18
0
 def make_gripper_posture(self, pose):
     t = JointTrajectory()
     t.joint_names = GRIPPER_JOINT_NAMES
     tp = JointTrajectoryPoint()
     tp.positions = [pose for j in t.joint_names]
     tp.effort = GRIPPER_EFFORT
     t.points.append(tp)
     return t
Пример #19
0
 def head_up_msg(self):
     jtm = JointTrajectory()
     jtm.joint_names = self.joint_names
     jtp = JointTrajectoryPoint()
     jtp.positions = [0]*len(self.joint_names)
     jtp.time_from_start = rospy.Duration(1.0)
     jtm.points = [jtp]
     return jtm
Пример #20
0
def talker():
    if 1 == 0:
        pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
        jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"]
        jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"]
    else:
        pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
        jn = [
            "LeftShoulderPitch",
            "LeftShoulderRoll",
            "LeftShoulderYaw",
            "LeftElbowPitch",
            "LeftForearmYaw",
            "LeftWristRoll",
            "LeftWristPitch",
        ]
        jn_r = [
            "RightShoulderPitch",
            "RightShoulderRoll",
            "RightShoulderYaw",
            "RightElbowPitch",
            "RightForearmYaw",
            "RightWristRoll",
            "RightWristPitch",
        ]

    # this doesnt work:
    # jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"]
    # value = 0

    rospy.init_node("send_arm_test", anonymous=True)
    rate = rospy.Rate(1)  # 10hz
    while not rospy.is_shutdown():

        msg = JointTrajectory()
        value = 0.1
        value_r = 0.1

        msg.joint_names = jn
        pt = JointTrajectoryPoint()
        pt.positions = [value] * len(jn)
        pt.velocities = [0] * len(jn)
        pt.accelerations = [0] * len(jn)
        pt.effort = [0] * len(jn)
        pt.time_from_start = rospy.Duration.from_sec(3)

        msg.points.append(pt)
        print msg.joint_names, rospy.get_time()
        pub.publish(msg)

        # TODO: add a sleep here between left and right

        msg.joint_names = jn_r
        msg.points[0].positions = [value_r] * len(jn)
        print msg.joint_names, rospy.get_time()
        pub.publish(msg)

        rate.sleep()
 def up_msg(self):
     jtm = JointTrajectory()
     jtm.joint_names = self.joint_names
     jtp = JointTrajectoryPoint()
     jtp.positions = [1.]*len(self.joint_names)
     jtp.velocities = [0.]*len(self.joint_names)
     jtp.time_from_start = rospy.Duration(5.0)
     jtm.points = [jtp]
     return jtm
 def up_msg(self):
     jtm = JointTrajectory()
     jtm.joint_names = self.joint_names
     jtp = JointTrajectoryPoint()
     jtp.positions = [self.filtered_angle]*len(self.joint_names)
     print self.controller, self.filtered_angle
     jtp.time_from_start = rospy.Duration(1.0)
     jtm.points = [jtp]
     return jtm
 def random_msg(self):
     jtm = JointTrajectory()
     jtm.joint_names = self.joint_names
     jtp = JointTrajectoryPoint()
     jtp.positions = 2*np.random.random(len(self.joint_names)) - 1
     jtp.velocities = [0.]*len(self.joint_names)
     jtp.time_from_start = rospy.Duration(5.)
     jtm.points = [jtp]
     return jtm
Пример #24
0
def get_post_place_posture():
    pre_grasp_posture = JointTrajectory()
    pre_grasp_posture.header.frame_id = END_EFFECTOR
    pre_grasp_posture.header.stamp = rospy.Time.now()
    pre_grasp_posture.joint_names = GRIPPER_JOINTS
    pos = JointTrajectoryPoint()  # open gripper
    pos.positions = [0.0]
    pre_grasp_posture.points.append(pos)
    return pre_grasp_posture
Пример #25
0
 def get_post_place_posture():
     post_place_posture = JointTrajectory()
     post_place_posture.header.frame_id = EEF_LINK
     post_place_posture.header.stamp = rospy.Time.now()
     post_place_posture.joint_names = GRIPPER_JOINTS
     pos = JointTrajectoryPoint()  # gripper open after place
     pos.positions = [0.0]
     post_place_posture.points.append(pos)
     return post_place_posture
Пример #26
0
    def __init__(self):
        rospy.init_node('trajectory_demo')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Which joints define the arm?
        arm_joints = ['link_1_joint',
                      'servo_2_joint',
                      'link_3_joint', 
                      'link_4_joint',
                      'link_5_joint']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal  = [0, 0, 0, 0, 0]
  
        else:
            # Set a goal configuration for the arm
            arm_goal  = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413]

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')
        
        arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)
                
        rospy.loginfo('...done')
Пример #27
0
    def set_position(self,goal):
	traj = JointTrajectory()
	traj_p = JointTrajectoryPoint()
	traj.joint_names = ["torso_lift_joint"]
	traj_p.positions = [goal]
	traj_p.velocities = [0.]
	traj_p.accelerations = [0.]
	traj_p.time_from_start = rospy.Duration(2.)
	traj.points.append(traj_p)
	self.pub.publish(traj)
    def move_torso_topic(self, joint_1, joint_2, time=1.0):
        jt = JointTrajectory()
        jt.joint_names = ['torso_1_joint', 'torso_2_joint']
        p = JointTrajectoryPoint()
        p.positions = [joint_1, joint_2]
        rospy.loginfo("Moving torso to: " + str(p.positions))
        p.time_from_start = rospy.Duration(time)
        jt.points.append(p)

        self.torso_pub.publish(jt)
Пример #29
0
def make_joint_trajectory(names, points):
    jt = JointTrajectory()
    jt.joint_names = names
    pt = JointTrajectoryPoint()
    pt.positions = points
    pt.effort = [0]*len(points)
    pt.velocities = [0]*len(points)
    pt.accelerations = [0]*len(points)
    jt.points = [pt]
    return jt
    def __init__(self):
        rospy.init_node('joint_trajectory_command_handler')
        self.real_robot =  rospy.get_param("~real_robot")
        ac_rate = rospy.get_param("~action_cycle_rate")
        self.rate = rospy.Rate(ac_rate)

        # Publisher to JointTrajectory robot controller
        if self.real_robot:
            # self.jt_pub = rospy.Publisher('/scaled_pos_traj_controller/command', JointTrajectory, queue_size=10)
            self.jt_pub = rospy.Publisher('/pos_traj_controller/command', JointTrajectory, queue_size=10)
        else:
            self.jt_pub = rospy.Publisher('/eff_joint_traj_controller/command', JointTrajectory, queue_size=10)

        # Subscriber to JointTrajectory Command coming from Environment
        rospy.Subscriber('env_arm_command', JointTrajectory, self.callback_env_joint_trajectory, queue_size=1)
        self.msg = JointTrajectory()
        # Queue with maximum size 1
        self.queue = Queue(maxsize=1)
        # Flag used to publish empty JointTrajectory message only once when interrupting execution
        self.stop_flag = False 
Пример #31
0
def move1():
    global joints_pos
    g = FollowJointTrajectoryGoal()
    g.trajectory = JointTrajectory()
    g.trajectory.joint_names = JOINT_NAMES
    try:
        joint_states = rospy.wait_for_message("joint_states", JointState)
        joints_pos = joint_states.position
        g.trajectory.points = [
            JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
            JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
            JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
            JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
        client.send_goal(g)
        client.wait_for_result()
    except KeyboardInterrupt:
        client.cancel_goal()
        raise
    except:
        raise
Пример #32
0
def moveJoint (jointcmds,prefix,nbJoints):
#  topic_name = '/' + prefix + '/effort_joint_trajectory_controller/command'
  topic_name = '/' + prefix + '/position_joint_trajectory_controller/command'
  pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
  jointCmd = JointTrajectory()  
  point = JointTrajectoryPoint()
  jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);  
  point.time_from_start = rospy.Duration.from_sec(2.0)
  for i in range(0, nbJoints):
#    jointCmd.joint_names.append(prefix +'_joint_'+str(i+1))
    jointCmd.joint_names.append('j2s7s300_joint_'+str(i+1))
    point.positions.append(jointcmds[i])
#    point.velocities.append(0.01)
#    point.accelerations.append(0)
#    point.effort.append(0)
  jointCmd.points.append(point)
  rate = rospy.Rate(100)
  while (not pub.get_num_connections()):
    rate.sleep()
  pub.publish(jointCmd)
Пример #33
0
 def moveJoint(self, jointcmds, nbJoints):
     topic_name = '/j2n6s300/effort_joint_trajectory_controller/command'
     pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
     jointCmd = JointTrajectory()
     point = JointTrajectoryPoint()
     jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
     point.time_from_start = rospy.Duration.from_sec(5.0)
     for i in range(0, nbJoints):
         jointCmd.joint_names.append('j2n6s300_joint_' + str(i + 1))
         point.positions.append(jointcmds[i])
         point.velocities.append(0)
         point.accelerations.append(0)
         point.effort.append(0)
     jointCmd.points.append(point)
     rate = rospy.Rate(100)
     count = 0
     while count < 500:
         pub.publish(jointCmd)
         count = count + 1
         rate.sleep()
Пример #34
0
def get_trajectory_goal(joint_names, q1, q2, q3):
    joint_num = len(joint_names)
    g = FollowJointTrajectoryGoal()
    g.trajectory = JointTrajectory()
    g.trajectory.joint_names = joint_names
    g.trajectory.points = [
        JointTrajectoryPoint(positions=q1,
                             velocities=[0] * joint_num,
                             time_from_start=rospy.Duration(0.0)),
        JointTrajectoryPoint(positions=q2,
                             velocities=[0] * joint_num,
                             time_from_start=rospy.Duration(2.0)),
        JointTrajectoryPoint(positions=q3,
                             velocities=[0] * joint_num,
                             time_from_start=rospy.Duration(4.0)),
        JointTrajectoryPoint(positions=q1,
                             velocities=[0] * joint_num,
                             time_from_start=rospy.Duration(6.0))
    ]
    return g
Пример #35
0
Файл: load.py Проект: Aand1/cart
 def set_head_state(self, jvals):
     """ Sets goal for head using provided joint values"""
     # Build trajectory message
     head_goal = JointTrajectory()
     head_goal.joint_names.append('head_pan_joint')
     head_goal.joint_names.append('head_tilt_joint')
     head_goal.points.append(JointTrajectoryPoint())
     head_goal.points[0].time_from_start = rospy.Duration(0.25)
     #self.head_goal.header.frame_id = 'base_link'
     for i in range(len(head_goal.joint_names)):
         head_goal.points[0].positions.append(jvals[i])
         head_goal.points[0].velocities.append(1)
     head_goal.header.stamp = rospy.get_rostime() + rospy.Duration(0.01)
     try:
         #print head_goal
         self.publishers["head"].publish(head_goal)
         if self.verbose:
             print "published [%s] to head_traj_controller/command topic" % jvals
     except:
         print "failed to publish head position!"
    def gripper_send_position_goal(self,
                                   position=0.3,
                                   velocity=0.4,
                                   action='move'):
        """Send position goal to the gripper
		
		Keyword Arguments:
			position {float} -- Gripper angle (default: {0.3})
			velocity {float} -- Gripper velocity profile (default: {0.4})
			action {str} -- Gripper movement (default: {'move'})
		"""

        self.turn_gripper_position_controller_on()
        duration = 0.2
        if action == 'pre_grasp_angle':
            gripper_angle_grasp = deepcopy(self.d[-2])
            max_distance = 0.085
            angular_coeff = 0.11
            K = 1  # lower numbers = higher angles
            angle = (max_distance - gripper_angle_grasp) / angular_coeff * K
            position = angle
            velocity = 0.4
        elif action == 'pick':
            position = 0.75
            velocity = 0.05
            duration = 8.0

        goal = FollowJointTrajectoryGoal()
        goal.trajectory = JointTrajectory()
        goal.trajectory.joint_names = self.robotiq_joint_name
        goal.trajectory.points.append(
            JointTrajectoryPoint(positions=[position],
                                 velocities=[velocity],
                                 accelerations=[0.0],
                                 time_from_start=rospy.Duration(duration)))
        self.client_gripper.send_goal(goal)
        if action == 'pick':
            while not rospy.is_shutdown(
            ) and not self.left_collision and not self.right_collision:
                pass
            self.client_gripper.cancel_goal()
def send_joint_position_actionlib(settings):

    # rosnode の初期化
    rospy.init_node('send_joint_position')
    # トピック名,メッセージ型を使って ActionLib client を定義
    client = actionlib.SimpleActionClient(
        '/fullbody_controller/follow_joint_trajectory',
        FollowJointTrajectoryAction)
    client.wait_for_server()  # ActionLib のサーバと通信が接続されることを確認
    # ActionLib client の goal を指定
    # http://wiki.ros.org/actionlib_tutorials/Tutorials の
    # Writing a Simple Action Client (Python) を参照
    # __TOPIC_PREFIX__Action で actionlib.SimpleActionClient を初期化
    # ゴールオブジェクトは __TOPIC_PREFIX__Goal を使って生成
    goal = FollowJointTrajectoryGoal()
    goal.trajectory = JointTrajectory()
    goal.trajectory.header.stamp = rospy.Time.now()
    goal.trajectory.joint_names = [
        'arm_joint1', 'arm_joint2', 'arm_joint3', 'arm_joint4', 'arm_joint5',
        'arm_joint6'
    ]
    for i in range(5):
        point = JointTrajectoryPoint()
        point.positions = [
            math.pi / 2, 0, math.pi / 4 * (i % 2), 0, math.pi / 2, math.pi / 2
        ]
        point.time_from_start = rospy.Duration(1.0 + i)
        goal.trajectory.points.append(point)
        client.send_goal(goal)
        rospy.loginfo("wait for goal ...")
        while (1):
            key = getKey()
            if (client.get_state() == 3):
                rospy.loginfo("done")
                break
            else:
                if (key == 'c'):
                    client.cancel_goal()
                    rospy.loginfo("canceling")
                    break
        client.wait_for_result()  # ロボットの動作が終わるまで待つ.
Пример #38
0
def OARbot_nav():
    global velocity, bot, goal,x,y
    x=0
    y=0
    rospy.init_node('OARbot_control', anonymous=True)
    rate=rospy.Rate(100)
    goal=location(x,y)
    bot=rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    arm=rospy.Publisher('/j2n6s300/effort_joint_trajectory_controller/command',JointTrajectory, queue_size=1)
    rospy.Subscriber('/odom',Odometry, pose_callback)
    velocity=Twist()
    jointcmds=[0.0,2.9,1.3,4.2,1.4,0.0]
    jointCmd=JointTrajectory()
    point=JointTrajectoryPoint()
    jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);  
    point.time_from_start = rospy.Duration.from_sec(5.0)

    for i in range(0, 6):
        jointCmd.joint_names.append('j2n6s300_joint_'+str(i+1))
        point.positions.append(jointcmds[i])
        point.velocities.append(5)
        point.accelerations.append(0)
        point.effort.append(0)
    jointCmd.points.append(point)
    rospy.sleep(2)
    key=raw_input("ready?")
    x=float(raw_input("Where to?"))
    y=float(raw_input())
    goal=location(x,y)
    walk_thread=threading.Thread(target=walk, args=())
    walk_thread.start()
    talk_thread=threading.Thread(target=talk, args=())
    talk_thread.start()
    rospy.sleep(1)

    while not rospy.is_shutdown():                   
        x=float(raw_input("Where to?"))
        y=float(raw_input())
        goal=location(x,y)    
  
        rospy.sleep(1)
Пример #39
0
def arms_down():
    pubL = rospy.Publisher('/left_arm_controller/command',
                           JointTrajectory,
                           queue_size=1)
    rospy.init_node('arms_down', anonymous=True)

    rate = rospy.Rate(2)
    pubR = rospy.Publisher('/right_arm_controller/command',
                           JointTrajectory,
                           queue_size=1)

    left_traj = JointTrajectory()
    left_traj.joint_names = [
        "left_arm_elbow_pitch_joint", 'left_arm_elbow_yaw_joint',
        'left_arm_shoulder_pitch_joint', 'left_arm_shoulder_roll_joint',
        'left_arm_shoulder_yaw_joint', 'left_arm_wrist_pitch_joint',
        'left_arm_wrist_roll_joint'
    ]

    left_traj_points = JointTrajectoryPoint()
    left_traj_points.positions = [0.0, 0.0, -1.57, -1.5, 0.0, 0.0, 0.0]
    left_traj_points.time_from_start = rospy.Duration.from_sec(0.5)
    left_traj.points.append(left_traj_points)

    right_traj = JointTrajectory()
    right_traj.joint_names = [
        "right_arm_elbow_pitch_joint", 'right_arm_elbow_yaw_joint',
        'right_arm_shoulder_pitch_joint', 'right_arm_shoulder_roll_joint',
        'right_arm_shoulder_yaw_joint', 'right_arm_wrist_pitch_joint',
        'right_arm_wrist_roll_joint'
    ]

    right_traj_points = JointTrajectoryPoint()
    right_traj_points.positions = [0.0, 0.0, -1.57, 1.5, 0.0, 0.0, 0.0]
    right_traj_points.time_from_start = rospy.Duration.from_sec(1)
    right_traj.points.append(right_traj_points)
    rospy.sleep(8)

    i = 0
    while i < 10:
        left_traj_points.time_from_start = rospy.Duration.from_sec(1 + i * 0.1)
        pubL.publish(left_traj)
        rate.sleep()
        right_traj_points.time_from_start = rospy.Duration.from_sec(1 +
                                                                    i * 0.15)
        pubR.publish(right_traj)
        i += 1
        rospy.loginfo("sent goal: %d", i)
 def init_trajectory(self):
     """Initialize a new target trajectory."""
     state = self.jointStatePublisher.last_joint_states
     self.trajectory_t0 = self.robot.getTime()
     self.trajectory = self.gripper_trajectory = JointTrajectory()
     self.trajectory.joint_names = self.prefixedJointNames
     self.gripper_trajectory_names = self.gripper
     self.trajectory.points = [
         JointTrajectoryPoint(positions=state.position if state else [0] *
                              7,
                              velocities=[0] * 7,
                              accelerations=[0] * 7,
                              time_from_start=rospy.Duration(0.0))
     ]
     self.gripper_trajectory.points = [
         JointTrajectoryPoint(positions=state.position if state else [0] *
                              2,
                              velocities=[0] * 2,
                              accelerations=[0] * 2,
                              time_from_start=rospy.Duration(0.0))
     ]
Пример #41
0
def MoveToStartPosition():
    global joints_pos
    g = FollowJointTrajectoryGoal()
    g.trajectory = JointTrajectory()
    g.trajectory.joint_names = JOINT_NAMES
    try:
        joint_states = rospy.wait_for_message("joint_states", JointState)
        joints_pos = joint_states.position
        g.trajectory.points = [
            JointTrajectoryPoint(positions=joints_pos,
                                 velocities=[0] * 6,
                                 time_from_start=rospy.Duration(3.0)),
            JointTrajectoryPoint(positions=STARTING_POSITION,
                                 velocities=[0] * 6,
                                 time_from_start=rospy.Duration(6.0))
        ]

        client.send_goal(g)
        client.wait_for_result()
    except:
        raise
Пример #42
0
def moveJoint(jointcmds, prefix, nbJoints):
    topic_name = '/' + prefix + '/arm_controller/command'
    print("topic_name: ", topic_name)
    pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
    jointCmd = JointTrajectory()
    point = JointTrajectoryPoint()
    jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
    point.time_from_start = rospy.Duration.from_sec(0.)
    for i in range(0, nbJoints):
        jointCmd.joint_names.append('drive' + str(i + 1) + '_joint')
        point.positions.append(jointcmds[i])
        point.velocities.append(0)
        point.accelerations.append(0)
        point.effort.append(0)
    jointCmd.points.append(point)
    rate = rospy.Rate(1)
    count = 0
    while (count < 2):
        pub.publish(jointCmd)
        count += 1
        rate.sleep()
Пример #43
0
    def __init__(self, arm_prefix):
        rospy.init_node('dmp_fitter', anonymous=True)
        rospy.Subscriber('joint_states', JointState, self.callback)

        self.joint_names = [
            arm_prefix + joint for joint in [
                "_shoulder_pan_joint", "_shoulder_lift_joint",
                "_upper_arm_roll_joint", "_elbow_flex_joint",
                "_forearm_roll_joint", "_wrist_flex_joint", "_wrist_roll_joint"
            ]
        ]

        self.js_0 = None
        self.js_1 = None
        self.jt = JointTrajectory()
        self.traj_pub = rospy.Publisher('motion_segment',
                                        JointTrajectory,
                                        queue_size=1)
        self.moving = False
        self.traj = []
        self.dmp_count = 0
Пример #44
0
    def readInPoses(self):

        xaplibrary = rospy.get_param('~xap', None)
        if xaplibrary:
            self.parseXapPoses(xaplibrary)

        poses = rospy.get_param('~poses', None)
        if poses:
            for key,value in poses.iteritems():
                try:
                # TODO: handle multiple points in trajectory
                    trajectory = JointTrajectory()
                    trajectory.joint_names = value["joint_names"]
                    point = JointTrajectoryPoint()
                    point.time_from_start = Duration(value["time_from_start"])
                    point.positions = value["positions"]
                    trajectory.points = [point]
                    self.poseLibrary[key] = trajectory
                except:
                    rospy.logwarn("Could not parse pose \"%s\" from the param server:", key);
                    rospy.logwarn(sys.exc_info())

        # add a default crouching pose:
        if not "crouch" in self.poseLibrary:
            trajectory = JointTrajectory()
            trajectory.joint_names = ["Body"]
            point = JointTrajectoryPoint()
            point.time_from_start = Duration(1.5)
            point.positions = [0.0,0.0,                     # head
                1.545, 0.33, -1.57, -0.486, 0.0, 0.0,        # left arm
                -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,     # left leg
                -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,    # right leg
                1.545, -0.33, 1.57, 0.486, 0.0, 0.0]        # right arm
            trajectory.points = [point]
            self.poseLibrary["crouch"] = trajectory;


        rospy.loginfo("Loaded %d poses: %s", len(self.poseLibrary), self.poseLibrary.keys())
Пример #45
0
def moveFingers (jointcmds,prefix,nbJoints):
#  topic_name = '/' + prefix + '/effort_finger_trajectory_controller/command'
  topic_name = '/' + prefix + '/position_finger_trajectory_controller/command'
  pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)  
  jointCmd = JointTrajectory()  
  point = JointTrajectoryPoint()
  jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);  
  point.time_from_start = rospy.Duration.from_sec(5.0)
  for i in range(0, nbJoints):
    jointCmd.joint_names.append(prefix +'_joint_finger_'+str(i+1))
    point.positions.append(jointcmds[i])
    point.velocities.append(0)
    point.accelerations.append(0)
    point.effort.append(0)
  jointCmd.points.append(point)
  rate = rospy.Rate(100)
  count = 0
  while (count < 50):
    pub.publish(jointCmd)
    count = count + 1
    rate.sleep()
Пример #46
0
    def TOPPRA2JointTrajectory(self, jnt_traj, f):
        # Sampling frequency is required to get the time samples correctly.
        # The number of points in ts_sample is duration*frequency.
        ts_sample = np.linspace(0, jnt_traj.get_duration(),
                                int(jnt_traj.get_duration() * f))
        # Sampling. This returns a matrix for all DOFs. Accessing specific one is
        # simple: qs_sample[:, 0]
        qs_sample = jnt_traj.eval(ts_sample)
        qds_sample = jnt_traj.evald(ts_sample)
        qdds_sample = jnt_traj.evaldd(ts_sample)

        n = qs_sample.shape[0]
        dof = qs_sample.shape[1]

        # Transform into JointTrajectory
        joint_trajectory = JointTrajectory()
        for i in range(0, n):
            temp_point = JointTrajectoryPoint()

            for j in range(0, dof):
                temp_point.positions.append(qs_sample[i, j])
                temp_point.velocities.append(qds_sample[i, j])
                temp_point.accelerations.append(qdds_sample[i, j])

            temp_point.time_from_start = rospy.Duration.from_sec(i / f)
            joint_trajectory.points.append(temp_point)

        # Add last point with zero velocity and acceleration
        last_point = JointTrajectoryPoint()
        for i in range(0, dof):
            last_point.positions.append(qs_sample[n - 1, i])
            last_point.velocities.append(0.0)
            last_point.accelerations.append(0.0)
        last_point.time_from_start = rospy.Duration.from_sec((n) / f)
        joint_trajectory.points.append(last_point)

        for i in range(0, dof):
            joint_trajectory.joint_names.append("joint" + str(i + 1))

        return joint_trajectory
Пример #47
0
    def action_server_cb(self, goal):
        """
        :param goal:
        :type goal: MoveGoal
        """
        rospy.loginfo(u'goal received')
        self.execute = goal.type == MoveGoal.PLAN_AND_EXECUTE
        if goal.type == MoveGoal.UNDEFINED:
            result = MoveResult()
            # TODO new error code
            result.error_code = MoveResult.INSOLVABLE
            self._as.set_aborted(result)
        else:
            result = MoveResult()
            for i, move_cmd in enumerate(goal.cmd_seq):  # type: (int, MoveCmd)
                # TODO handle empty controller case
                intermediate_result = self.send_to_process_manager_and_wait(
                    move_cmd)
                result.error_code = intermediate_result.error_code
                if result.error_code != MoveResult.SUCCESS:
                    # clear traj from prev cmds
                    result.trajectory = JointTrajectory()
                    break
                result.trajectory = self.append_trajectory(
                    result.trajectory, intermediate_result.trajectory)
                if i < len(goal.cmd_seq) - 1:
                    self.let_process_manager_continue()
            else:  # if not break
                rospy.loginfo(u'found solution')
                if result.error_code == MoveResult.SUCCESS and self.execute:
                    result.error_code = self.send_to_robot(result)

            self.start_js = None
            if result.error_code != MoveResult.SUCCESS:
                self._as.set_aborted(result)
            else:
                self._as.set_succeeded(result)
            self.let_process_manager_continue()
        rospy.loginfo(u'goal result: {}'.format(
            ERROR_CODE_TO_NAME[result.error_code]))
Пример #48
0
    def pan_tilt(self, pan, tilt):
        """Moves the head by setting pan/tilt angles.

              Args:
            pan: The pan angle, in radians. A positive value is clockwise.
            tilt: The tilt angle, in radians. A positive value is downwards.
        """
        # TODO: Check that the pan/tilt angles are within joint limits
        if not ((pan >= Head.MIN_PAN and pan <= Head.MAX_PAN) and
                (tilt >= Head.MIN_TILT and tilt <= Head.MAX_TILT)):
            # Not within join limits
            return

        # TODO: Create a trajectory point
        point = JointTrajectoryPoint()

        # TODO: Set positions of the two joints in the trajectory point
        point.positions.append(pan)
        point.positions.append(tilt)

        # TODO: Set time of the trajectory point
        point.time_from_start = rospy.Duration(secs=2.5, nsecs=0)

        # TODO: Create goal
        goal = FollowJointTrajectoryGoal()

        traj = JointTrajectory()
        # TODO: Add joint names to the list
        traj.joint_names.append(PAN_JOINT)
        traj.joint_names.append(TILT_JOINT)

        # TODO: Add trajectory point created above to trajectory
        traj.points.append(point)

        goal.trajectory = traj
        # TODO: Send the goal
        self._pan_tilt_client.send_goal(goal)

        # TODO: Wait for result
        self._pan_tilt_client.wait_for_result()
Пример #49
0
def youbot_traj():

    rospy.init_node('lab7youbot_traj')

    rospack = rospkg.RosPack()

    path = rospack.get_path('lab07_example01')
    bag = rosbag.Bag(path + '/../lab7data.bag')

    traj_pub = rospy.Publisher('/EffortJointInterface_trajectory_controller/command', JointTrajectory, queue_size=5)

    my_traj = JointTrajectory()


    tfs = 4
    my_traj.header.stamp = rospy.Time.now()
    my_traj.joint_names.append('arm_joint_1')
    my_traj.joint_names.append('arm_joint_2')
    my_traj.joint_names.append('arm_joint_3')
    my_traj.joint_names.append('arm_joint_4')
    my_traj.joint_names.append('arm_joint_5')

    for topic, msg, t in bag.read_messages(topics=['my_joint_states']):
        my_pt = JointTrajectoryPoint()

        if len(msg.position) != 0:
            print len(msg.position)
            for i in range(0, 5):
                my_pt.positions.append(msg.position[i])
                my_pt.velocities.append(msg.velocity[i])

        my_pt.time_from_start.secs = tfs
        tfs = tfs + 4
        my_traj.points.append(my_pt)

    bag.close

    rospy.sleep(5)

    traj_pub.publish(my_traj)
Пример #50
0
    def defaultPos(self):
        dur = rospy.Duration(1)
        #CalcPos = invkin([0,0,0.563])
        #CalcPos.append(self.GripperPos)
        #print(CalcPos)

        jtp = JointTrajectoryPoint(positions=[0, 0, 0, 0, 1],
                                   velocities=[1.0] * self.N_JOINTS,
                                   time_from_start=dur)
        dur += rospy.Duration(8)
        newJTP = []
        newJTP.append(jtp)
        self.joint_positions = newJTP

        self.jt = JointTrajectory(joint_names=self.names,
                                  points=self.joint_positions)
        self.goal = FollowJointTrajectoryGoal(trajectory=self.jt,
                                              goal_time_tolerance=dur +
                                              rospy.Duration(8))
        self.send_command()
        #self.xyzPosition = [0,0,0.563]
        return
Пример #51
0
def build_traj(start, end, duration):

  if sorted(start.name) <> sorted(end.name):
    rospy.logerr('Start and End position joint_names mismatch')
    raise

  # assume the start-position joint-ordering
  joint_names = start.name
 
  start_pt = JointTrajectoryPoint()
  start_pt.positions = start.position
  start_pt.velocities = [0]*len(start.position)
  start_pt.time_from_start = rospy.Duration(0.0)

  end_pt = JointTrajectoryPoint()
  for j in joint_names:
    idx = end.name.index(j)
    end_pt.positions.append(end.position[idx])  # reorder to match start-pos joint ordering
    end_pt.velocities.append(0)
  end_pt.time_from_start = rospy.Duration(duration)

  return JointTrajectory(joint_names=joint_names, points=[start_pt, end_pt])
Пример #52
0
    def __init__(self, pub_cmd):
        super(Rosbag, self).__init__()

        self.pub_cmd = pub_cmd
        self.rosbag_dir = "/root/ros/install/share/servo_robot/data"
        self.rosbag_done = True
        self.rosbag_run = False
        self.rosbag_bag = None
        self.rosbag_sub = None

        self.msg_bag = JointTrajectory()
        self.msg_bag.joint_names = [
            'a1_joint', 'a2_joint', 'a3_joint', 'a4_joint', 'a5_joint',
            'a6_joint'
        ]
        self.msg_bag.points.append(JointTrajectoryPoint())
        self.msg_bag.points[0].time_from_start.secs = 0.0
        self.msg_bag.points[0].time_from_start.nsecs = 100000000.0

        self.pub_status = rospy.Publisher(SMART_STATUS,
                                          DataList,
                                          queue_size=10)
 def __init__(self):
     # load metadata
     config_metadata = self.load_metadata()
     self.t_scale = float(config_metadata['t_scale'])
     # APC Net Key Parameter
     self.lyap_switch_threshold = 0.45
     self.lyap_old = 10
     self.switch_control = False
     # Config Local Net
     self.apcNet_local = DirectSiamese()
     self.apcNet_local.model_fld = 'direct_siamese_e2e_zoom_01'
     self.apcNet_local.load_ckpt(load_best=False)
     # Config Global Net
     self.apcNet_global = DirectSiamese()
     self.apcNet_global.model_fld = 'direct_siamese_e2e_01'
     self.apcNet_global.load_ckpt(load_best=False)
     # Init msg for publishing
     self.joint_traj = JointTrajectory()
     self.joint_traj.joint_names = [
         'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
         'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
     ]
     self.joint_traj.points = [
         JointTrajectoryPoint(positions=[0] * 6,
                              velocities=[0] * 6,
                              time_from_start=rospy.Duration(0.1))
     ]
     self.vel_ctrl_old = np.zeros([6, 1])
     # Approximated Synchronised Topics
     self.image_sub = message_filters.Subscriber("/camera/image_color",
                                                 Image)
     self.joint_sub = message_filters.Subscriber("/joint_states",
                                                 JointState)
     self.synced_msgs = message_filters.ApproximateTimeSynchronizer(
         [self.image_sub, self.joint_sub], 10, 0.1)
     self.synced_msgs.registerCallback(self.image_callback)
     self.joint_command_pub = rospy.Publisher("/ur_driver/joint_speed",
                                              JointTrajectory,
                                              queue_size=10)
Пример #54
0
def move_arm():
    rospy.init_node('arm_controller_custom', anonymous=False)

    topic_name = 'custom/arm_controller/command'
    pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)

    rate = rospy.Rate(60)

    while not rospy.is_shutdown():
        trajectory = JointTrajectory()
        point = JointTrajectoryPoint()

        trajectory.header.frame_id = "/base_link"

        trajectory.joint_names.append("waist")
        trajectory.joint_names.append("shoulder")
        trajectory.joint_names.append("elbow")
        trajectory.joint_names.append("wrist_angle")
        trajectory.joint_names.append("wrist_rotate")

        print('Enter Value for angles:')
        a = float(input('  Waist:\t'))
        b = float(input('  Shoulder:\t'))
        c = float(input('  Elbow:\t'))
        d = float(input('  Wrist Angle:\t'))

        point.positions.append(a)
        point.positions.append(b)
        point.positions.append(c)
        point.positions.append(d)
        point.positions.append(0.0)
        point.time_from_start = rospy.Duration(0, 1e8)

        trajectory.points.append(point)

        trajectory.header.stamp = rospy.Time.now()

        pub.publish(trajectory)
        rate.sleep()
Пример #55
0
def close_gripper(group):
    rospy.loginfo('-------------------------------------------------')
    rospy.loginfo('Closing gripper')
    rospy.loginfo('-------------------------------------------------')
    pub = rospy.Publisher('/gripper/command', JointTrajectory, queue_size=10)

    # traj = JointTrajectory()
    # h = Header()
    # h.stamp = rospy.Time.now()
    # h.frame_id = 'gripper_base_link'
    # traj.header = h

    # traj.joint_names.append('gripper_finger1_joint')

    # traj_point = JointTrajectoryPoint()
    # traj_point.positions.append(0.5)
    # traj.points.append(traj_point)

    # traj.points[0].time_from_start = rospy.Duration.from_sec(1.0)

    # pub.publish(traj)

    jt = JointTrajectory()
    jt.joint_names.append("gripper_finger1_joint")
    point = JointTrajectoryPoint()
    point.positions.append(0.37)
    point.time_from_start = rospy.Duration.from_sec(0.8)
    jt.points.append(point)

    # pub.publish(jt)
    cnt = 0
    rate = rospy.Rate(10)  # 10hz
    while not rospy.is_shutdown() and cnt < 30:
        cnt += 1
        pub.publish(jt)
        rate.sleep()
    rospy.loginfo('-------------------------------------------------')
    rospy.loginfo('DONE')
    rospy.loginfo('-------------------------------------------------')
Пример #56
0
 def on_cancel(self, goal_handle):
     log("on_cancel")
     if goal_handle == self.goal_handle:
         with self.following_lock:
             # Uses the next little bit of trajectory to slow to a stop
             STOP_DURATION = 0.5
             now = time.time()
             point0 = sample_traj(self.traj, now - self.traj_t0)
             point0.time_from_start = rospy.Duration(0.0)
             point1 = sample_traj(self.traj, now - self.traj_t0 + STOP_DURATION)
             point1.velocities = [0] * 6
             point1.accelerations = [0] * 6
             point1.time_from_start = rospy.Duration(STOP_DURATION)
             self.traj_t0 = now
             self.traj = JointTrajectory()
             self.traj.joint_names = joint_names
             self.traj.points = [point0, point1]
             
             self.goal_handle.set_canceled()
             self.goal_handle = None
     else:
         goal_handle.set_canceled()
 def move(self,
          shoulder_lift_cmd_in=-pi / 4.0,
          elbow_cmd_in=pi / 2.0,
          duration_in=5.0):
     wrist_1_cmd = (-1.0) * (shoulder_lift_cmd_in + elbow_cmd_in
                             )  # keep it level
     wrist_2_cmd = 0.5 * pi  # straight along arm
     g = FollowJointTrajectoryGoal()
     g.trajectory = JointTrajectory()
     g.trajectory.joint_names = self.joint_names
     p1 = JointTrajectoryPoint()
     p1.positions = [
         0.0, shoulder_lift_cmd_in, elbow_cmd_in, wrist_1_cmd, wrist_2_cmd,
         0.0
     ]
     p1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     p1.time_from_start = rospy.Duration(duration_in)
     g.trajectory.points.append(p1)
     self.client.send_goal(g)
     rospy.loginfo("Sent goal and waiting for arm")
     self.client.wait_for_result()
     rospy.loginfo("Arm move complete")
Пример #58
0
    def publish_solution(self):

        traj_msg = JointTrajectory()

        # Build message
        n_pts = self.RRT.solution[0][0].size

        # For all times
        for i in range(n_pts):

            traj_pt = JointTrajectoryPoint()

            t = self.RRT.solution[2][i]  # Time
            traj_pt.time_from_start.secs = int(t)
            traj_pt.time_from_start.nsecs = (t - int(t)) * 1000

            # For all DOF
            for j in range(self.n_DOF):
                traj_pt.positions.append(self.RRT.solution[0][j][i])
                traj_pt.velocities.append(self.RRT.solution[0][j +
                                                               self.n_DOF][i])
                traj_pt.accelerations.append(
                    self.RRT.solution[3][j + self.n_DOF][i])

            # For all actuators
            for j in range(self.RRT.DS.m):
                traj_pt.effort.append(self.RRT.solution[1][j][i])

            traj_msg.points.append(traj_pt)

        traj_msg.joint_names.append('1')
        traj_msg.joint_names.append('2')

        self.pub_traj_sol.publish(traj_msg)

        if self.verbose:
            rospy.loginfo("Planner: Trajectory Solution Published ")

        self.RRT.plot_2D_Tree()
 def callback(self, anymsg, topic_name):
     if self.pause_button.isChecked():
         return
     # In case of control_msgs/FollowJointTrajectoryActionGoal
     # set trajectory_msgs/JointTrajectory to 'msg'
     # Convert AnyMsg to trajectory_msgs/JointTrajectory
     msg_class = self.topic_name_class_map[topic_name]
     if msg_class == JointTrajectory:
         msg = JointTrajectory().deserialize(anymsg._buff)
     elif msg_class == FollowJointTrajectoryActionGoal:
         msg = FollowJointTrajectoryActionGoal().deserialize(anymsg._buff).goal.trajectory
     else:
         rospy.logerr('Wrong message type %s'%msg_class)
         return;
     self.time = np.array([0.0] * len(msg.points))
     (self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {})
     for joint_name in msg.joint_names:
         self.dis[joint_name] = np.array([0.0] * len(msg.points))
         self.vel[joint_name] = np.array([0.0] * len(msg.points))
         self.acc[joint_name] = np.array([0.0] * len(msg.points))
         self.eff[joint_name] = np.array([0.0] * len(msg.points))
     for i in range(len(msg.points)):
         point = msg.points[i]
         self.time[i] = point.time_from_start.to_sec()
         for j in range(len(msg.joint_names)):
             joint_name = msg.joint_names[j]
             if point.positions:
                 self.dis[joint_name][i] = point.positions[j]
             if point.velocities:
                 self.vel[joint_name][i] = point.velocities[j]
             if point.accelerations:
                 self.acc[joint_name][i] = point.accelerations[j]
             if point.effort:
                 self.eff[joint_name][i] = point.effort[j]
     if self.joint_names != msg.joint_names:
         self.joint_names = msg.joint_names
         self.refresh_tree()
     self.joint_names = msg.joint_names
     self.plot_graph()
Пример #60
-1
def move_arm( positions, time_from_start = 3.0, arm = 'r', client = None ):

    if (client == None):
        client = init_jt_client(arm)
    else:
        arm = client.action_client.ns[0:1]; # ignore arm argument

    rospy.loginfo( 'move arm \'%s\' to position %s'%( arm, positions ) )    

    joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) )
    
    jt = JointTrajectory()
    jt.joint_names = joint_names
    jt.points = []
    jp = JointTrajectoryPoint()
    jp.time_from_start = rospy.Time.from_seconds( time_from_start )
    jp.positions = positions
    jt.points.append( jp )
    
    # push trajectory goal to ActionServer
    jt_goal = JointTrajectoryGoal()
    jt_goal.trajectory = jt
    jt_goal.trajectory.header.stamp = rospy.Time.now() # + rospy.Duration.from_sec( time_from_start )
    client.send_goal( jt_goal )
    client.wait_for_result()    
    return client.get_state()