def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.position)
    pub = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=10)

    joints_str = JointTrajectory()
    joints_str.header = Header()
    joints_str.header.stamp = rospy.Time.now()
    joints_str.joint_names = ['base_joint', 'shoulder', 'elbow', 'wrist']
    point=JointTrajectoryPoint()
    point.positions = [data.position[0], data.position[1], data.position[2], data.position[3]]
    point.time_from_start = rospy.Duration(2)
    joints_str.points.append(point)

    pub.publish(joints_str)


    pub = rospy.Publisher('gripper_controller/command', JointTrajectory, queue_size=10)

    joints_str = JointTrajectory()
    joints_str.header = Header()
    joints_str.header.stamp = rospy.Time.now()
    joints_str.joint_names = ['gripper']
    point=JointTrajectoryPoint()
    point.positions = [data.position[4]]
    point.time_from_start = rospy.Duration(2)
    joints_str.points.append(point)

    pub.publish(joints_str)
    rospy.loginfo("position updated")
def publisher():
    panda1_publisher = rospy.Publisher(
        'panda1/position_joint_trajectory_controller/command',
        JointTrajectory,
        queue_size=1)
    panda2_publisher = rospy.Publisher(
        'panda2/position_joint_trajectory_controller/command',
        JointTrajectory,
        queue_size=1)
    rospy.init_node('joint_pose_publisher', anonymous=True)
    rate = rospy.Rate(10)  # 10hz

    panda1_trajectory = JointTrajectory()
    panda1_trajectory.joint_names = [
        'panda1_joint1', 'panda1_joint2', 'panda1_joint3', 'panda1_joint4',
        'panda1_joint5', 'panda1_joint6', 'panda1_joint7'
    ]
    panda1_trajectory.points.append(JointTrajectoryPoint())
    panda1_trajectory.points[0].time_from_start = rospy.Duration(1.0)
    panda1_trajectory.points[0].velocities = [
        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
    ]
    panda1_trajectory.points[0].effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    panda1_trajectory.points[0].accelerations = [
        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
    ]

    panda2_trajectory = JointTrajectory()
    panda2_trajectory.joint_names = [
        'panda2_joint1', 'panda2_joint2', 'panda2_joint3', 'panda2_joint4',
        'panda2_joint5', 'panda2_joint6', 'panda2_joint7'
    ]
    panda2_trajectory.points.append(JointTrajectoryPoint())
    panda2_trajectory.points[0].time_from_start = rospy.Duration(1.0)
    panda2_trajectory.points[0].velocities = [
        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
    ]
    panda2_trajectory.points[0].effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    panda2_trajectory.points[0].accelerations = [
        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
    ]

    while not rospy.is_shutdown():
        panda1_trajectory.header = std_msgs.msg.Header()
        panda2_trajectory.header = std_msgs.msg.Header()

        panda1_trajectory.points[0].positions = [
            1.0, -1.0, 0.5, 0.5, 0.5, 0.0, 0.0
        ]
        panda2_trajectory.points[0].positions = [
            1.0, -1.0, 0.5, 0.5, 0.5, 0.0, 0.0
        ]

        panda1_publisher.publish(panda1_trajectory)
        panda2_publisher.publish(panda2_trajectory)
        rate.sleep()
Exemplo n.º 3
0
    def spin(self):
        rate = rospy.Rate(0.5)
        dmp_count = 1
        n_dmps = len(glob.glob('./*.npy'))
        while not rospy.is_shutdown():
            if self.header is not None:
                dmp = self.load_dmp(dmp_count)
                y_r, dy_r, _ = dmp.rollout()
                jt = JointTrajectory()
                jt.header = self.header
                jt.joint_names = dmp.joint_names
                jtp = JointTrajectoryPoint()

                for i in range(y_r.shape[0]):
                    jtp.positions = y_r[i, :].tolist()
                    jtp.velocities = dy_r[i, :].tolist()
                    jtp.time_from_start = rospy.Duration(1)
                    jt.points.append(jtp)
                self.traj_pub.publish(jt)
                print('Publishing dmp {}'.format(dmp_count))
                dmp_count += 1

                if dmp_count > n_dmps:
                    break
            rate.sleep()
Exemplo n.º 4
0
def main():

    rospy.init_node('send_joints')
    pub = rospy.Publisher('/arm_controller/command',
                          JointTrajectory,
                          queue_size=10)

    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR5
    traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
                        'elbow_joint', 'wrist_1_joint', 'wrist_2_joint',
                        'wrist_3_joint']

    rate = rospy.Rate(1)
    pts = JointTrajectoryPoint()
    traj.header.stamp = rospy.Time.now()

    while not rospy.is_shutdown():
        for i in range(0,1):
            pts.positions = waypoints[i]

            pts.time_from_start = rospy.Duration(1.0)

            # Set the points to the trajectory
            traj.points = []
            traj.points.append(pts)
            # Publish the message
            pub.publish(traj)
            rate.sleep()
Exemplo n.º 5
0
def main():

    rospy.init_node('send_joints')
    pub = rospy.Publisher('/trajectory_controller/command',
                          JointTrajectory,
                          queue_size=10)

    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR5
    traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
                        'elbow_joint', 'wrist_1_joint', 'wrist_2_joint',
                        'wrist_3_joint']

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        traj.header.stamp = rospy.Time.now()
        pts = JointTrajectoryPoint()
        pts.positions = [0.180397344042591, -0.46159712847377854,  0.34107758901149676,-1.8290689486244203, -0.45481156632822817, 1.9918058620776087]
        pts.time_from_start = rospy.Duration(2.0)

        # Set the points to the trajectory
        traj.points = []
        traj.points.append(pts)
        # Publish the message
        pub.publish(traj)
Exemplo n.º 6
0
    def spin(self):
        rate = rospy.Rate(0.5)
        dmp_count = 1
        Ndmp = len(glob.glob('./*.npy'))
        while not rospy.is_shutdown():
            if not (self.header == None):
                dmp = self.load_dmp(dmp_count)
                y_r, dy_r, ddy_r = dmp.rollout()
                #plt.subplot(1,2,1)
                #plt.plot(y_r)
                #plt.subplot(1,2,2)
                #plt.plot(dy_r)
                #plt.show()
                jt = JointTrajectory()
                jt.header = self.header
                jt.joint_names = dmp.joint_names
                jtp = JointTrajectoryPoint()

                for i in range(y_r.shape[0]):
                    jtp.positions = y_r[i, :].tolist()
                    jtp.velocities = dy_r[i, :].tolist()
                    jtp.time_from_start = rospy.Duration(1)
                    jt.points.append(jtp)
                self.traj_pub.publish(jt)
                print('Publishing dmp %d' % dmp_count)
                dmp_count += 1

                if dmp_count > Ndmp:
                    break
            rate.sleep()
Exemplo n.º 7
0
def main():

    rospy.init_node('send_joints')
    pub = rospy.Publisher('/arm_controller/command',
                          JointTrajectory,
                          queue_size=10)
    sub = rospy.Subscriber('/arm_controller/state',
                           JointTrajectoryControllerState,
                           _observation_callback, queue_size=10)
    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR5
    traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
                        'elbow_joint', 'wrist_1_joint', 'wrist_2_joint',
                        'wrist_3_joint']

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        traj.header.stamp = rospy.Time.now()
        pts = JointTrajectoryPoint()
        pts.positions = [0.0, 0.0, 1.0, 0.0, 0.0, 0.0]
        pts.time_from_start = rospy.Duration(0.1)

        # Set the points to the trajectory
        traj.points = []
        traj.points.append(pts)
        # Publish the message
        pub.publish(traj)
Exemplo n.º 8
0
    def move(self, p, v_scale=0.1, duration_low_bound=1, start_duration=0.5):
        target_q = self.get_inverse_kin(p)
        if target_q is None:
            return False

        traj = JointTrajectory()
        traj.header = rospy.Header(frame_id="1", stamp=rospy.Time())
        traj.joint_names = [
            'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
            'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
        ]
        dist = np.abs(np.subtract(target_q,
                                  self.current_state.positions)).max()
        duration = rospy.Duration(
            nsecs=int(max(dist / v_scale, duration_low_bound) * 1e9))
        # print(dist,duration.secs,duration.nsecs)
        end_point = JointTrajectoryPoint(
            positions=target_q,
            velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            time_from_start=duration)
        start_point = JointTrajectoryPoint(
            positions=(np.array(self.current_state.positions) +
                       np.array(self.current_state.velocities) *
                       (start_duration + 0.01)).tolist(),
            velocities=self.current_state.velocities,
            time_from_start=rospy.Duration(0, int(start_duration * 1e9)))
        traj.points = [start_point, end_point]
        self.pub.publish(traj)
        return True
Exemplo n.º 9
0
def main(data):

    pub = rospy.Publisher('/arm_controller/command',
                          JointTrajectory,
                          queue_size=10)

    # Create the topic message
    arm = JointTrajectory()
    arm.header = Header()
    # Joint names for UR5
    arm.joint_names = [
        'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
        'arm_5_joint', 'arm_gripper_right_joint', 'arm_gripper_left_joint'
    ]

    rate = rospy.Rate(50)  # 50hz
    rospy.loginfo(arm)
    arm.header.stamp = rospy.Time.now()
    pts = JointTrajectoryPoint()
    pts.positions = [
        data.position[3], data.position[0], data.position[4], data.position[1],
        data.position[2]
    ]
    pts.time_from_start = rospy.Duration(1.0)
    # Set the points to the trajectory
    arm.points = []
    arm.points.append(pts)
    # Publish the message
    pub.publish(arm)
    rate.sleep()
Exemplo n.º 10
0
    def create_ee_traj(self, state):
        gripper_traj = JointTrajectory()
        gripper_current_states = self.request_joint_states('gripper')
        gripper_state = gripper_current_states[3]   #we only care about the 3rd joint which is the active joint

        gripper_traj.header = Header()
        gripper_traj.header.frame_id = '/world'
        gripper_traj.joint_names = ['robotiq_85_left_knuckle_joint']
        point_ = JointTrajectoryPoint()
        gripper_traj.points.append(point_)
        if self.gripper_state_initial is not None: #if this is the first operation
            gripper_traj.points[0].positions = [self.gripper_state_initial]
            self.gripper_state_initial = None
        elif self.gripper_state_current is not None:
            gripper_traj.points[0].positions = self.gripper_state_current
        else:
            rospy.logwarn('PlanningServiceClient::create_ee_traj() -- Damn dude, not even sure how you did that')

        point_ = JointTrajectoryPoint()
        if 'open' in state:    # add open point to trajectory
            point_.positions = [0.0]
            self.gripper_state_current = [0.0]
        elif 'close' in state:  #add closed point to trajectory
            point_.positions = [0.803]
            self.gripper_state_current = [0.803]
        else:
            rospy.logwarn('PlanningServiceClient::create_ee_traj() -- incorrect state sent')
            return False
        point_.time_from_start.secs = 2 # how long it will take to actuate

        gripper_traj.points.append(point_)

        rospy.loginfo('PlanningServiceClient::create_ee_traj() -- gripper traj = {}'.format(gripper_traj))
        return gripper_traj
Exemplo n.º 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
    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
Exemplo n.º 13
0
def joint_client(position):

    rospy.init_node('send_joints')
    pub = rospy.Publisher('/trajectory_controller/command',
                          JointTrajectory,
                          queue_size=10)

    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR5
    traj.joint_names = [
        'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
        'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
    ]

    rate = rospy.Rate(10)
    i = 0
    while not rospy.is_shutdown():
        traj.header.stamp = rospy.Time.now()
        pts = JointTrajectoryPoint()
        pts.time_from_start = rospy.Duration(1.0)
        pts.positions = position

        # Set the point to the trajectory
        traj.points = []
        traj.points.append(pts)
        # Publish the message
        pub.publish(traj)

        if (i == 20000): break
        i = i + 1
Exemplo n.º 14
0
    def _transform_base_trajectory(self, base_traj, odom_to_frame_pose):
        odom_to_frame = geometry.pose_to_tuples(odom_to_frame_pose)

        num_points = len(base_traj.points)
        odom_base_traj = JointTrajectory()
        odom_base_traj.points = list(
            utils.iterate(JointTrajectoryPoint, num_points))
        odom_base_traj.header = base_traj.header
        odom_base_traj.joint_names = self._base_joint_names

        # Transform each point into odom frame
        previous_theta = 0.0
        for i in range(num_points):
            t = base_traj.points[i].transforms[0]
            frame_to_base = geometry.transform_to_tuples(t)

            # odom_to_base = odom_to_frame * frame_to_base
            (odom_to_base_trans, odom_to_base_rot) = geometry.multiply_tuples(
                odom_to_frame, frame_to_base)

            odom_base_traj.points[i].positions = [
                odom_to_base_trans[0], odom_to_base_trans[1], 0
            ]
            roll, pitch, yaw = T.euler_from_quaternion(odom_to_base_rot)
            dtheta = geometry.shortest_angular_distance(previous_theta, yaw)
            theta = previous_theta + dtheta

            odom_base_traj.points[i].positions[2] = theta
            previous_theta = theta
        return odom_base_traj
def main():
    rospy.init_node('send_joints')
    rospy.loginfo("###########################################################")
    pub = rospy.Publisher('/arm_controller/command',
                          JointTrajectory,
                          queue_size=10)
    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR5
    traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
                        'elbow_joint', 'wrist_1_joint', 'wrist_2_joint',
                        'wrist_3_joint']
    rate = rospy.Rate(1)
    pts = JointTrajectoryPoint()
    traj.header.stamp = rospy.Time.now()
    pts.positions = [0.0, -1.57, 1.57, -1.57, -1.57, 0.0]
    pts.time_from_start = rospy.Duration(1.0)
    # Set the points to the trajectory
    traj.points = []
    traj.points.append(pts)
    # Publish the message
    pub.publish(traj)
    rospy.loginfo("-------------------------------------------------------------")
    rospy.spin()
Exemplo n.º 16
0
    def execute_cb(self, goal):
        if not self.equalJoints(goal.trajectory.joint_names):
            rospy.logerr("inaccurate joint names received")
            return

        if self._active_flag:
            nans = JointTrajectory()
            nans.header = goal.trajectory.header
            nans.joint_names = goal.trajectory.joint_names
            nans.points = JointTrajectoryPoint()
            nans.points.positions = [float("nan")] * len(nans.joint_names)

        print "execute" + rospy.get_name()
        self._finishable = False
        command = JointTrajectory()
        command.header = goal.trajectory.header
        command.joint_names = goal.trajectory.joint_names
        command.points = [
            goal.trajectory.points[-1],
        ]
        self._active_goal = goal
        self._active_command = command
        self._active_end_time = rospy.get_rostime() + goal.trajectory.points[
            -1].time_from_start + goal.goal_time_tolerance + rospy.Duration(
                2, 0)  #2.0 sec is nantonaku
        self._pub_command.publish(command)
        self._active_flag = True
        self._execute_state = True

        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            if self._finishable:
                break
            r.sleep()

        self._active_flag = False
        if not self._execute_state:
            self._result.error_string = "execution timeout"
            self._as.set_aborted(self._result)
            return
        rospy.loginfo('%s: Succeeded' % self._action_name)
        self._result.error_string = "execution succeeded"
        self._result.error_code = 0
        self._as.set_succeeded(self._result)
Exemplo n.º 17
0
 def jointTrajectory(self, ID, jointNames, points):
     # Command a joint trajectory of arm
     msg = JointTrajectory()
     msg.header = Header()
     msg.header.frame_id = ID
     msg.joint_names = jointNames
     msg.points = []
     msg.points.append(points)
     # Publish
     self.jointTrajectoryPublisher.publish(msg)
Exemplo n.º 18
0
def main():
    os.makedirs(os.path.join(assets_dir(), 'data', '{}_ur10_train_data'.format(args.tag)))
    rospy.init_node('send_joints') # create a publish node named send_joints
    pub = rospy.Publisher('/trajectory_controller/command',
                          JointTrajectory,
                          queue_size=10)

    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR
    traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
                        'elbow_joint', 'wrist_1_joint', 'wrist_2_joint',
                        'wrist_3_joint']

    rate = rospy.Rate(10)

    f_joint = open(os.path.join(assets_dir(),
        'data/{}_ur10_train_data/joint_data.txt'.format(args.tag)),"a+")
    f_end_effector = open(os.path.join(assets_dir(),
        'data/{}_ur10_train_data/end_effector_data.txt'.format(args.tag)),"a+")
    iteration = 0
    while not rospy.is_shutdown():
        traj.header.stamp = rospy.Time.now()
        pts = JointTrajectoryPoint()
        random_pos_shoulder_lift_joint = np.random.uniform(-3.14,0) # set shoulder_lift_joint angle
        random_pos = np.random.uniform(-3.14,3.14,size=(5)) # set other joints angle
        random_pos = np.insert(random_pos,1, random_pos_shoulder_lift_joint)
        print("random_pos",random_pos)
        pts.positions = random_pos
        pts.time_from_start = rospy.Duration(1.0)

        # Set the points to the trajectory
        traj.points = []
        traj.points.append(pts)
        # Publish the message
        pub.publish(traj)
        time.sleep(5)   # Delays for 5 seconds.
        x_position, y_position, z_position = pos_main()
        end_effector_position = np.array([x_position, y_position, z_position])

        # pickle store data process......
        # pickle.dump(random_pos, open(os.path.join(assets_dir(),
        #         'data/{}_ur5_train_data/joint_angle_data.pkl'.format(args.tag)), 'wb'),
        #         protocol=pickle.HIGHEST_PROTOCOL)
        # pickle.dump((x_position, y_position, z_position), open(os.path.join(assets_dir(),
        #         'data/{}_ur5_train_data/end_effector_position_data.pkl'.format(args.tag)), 'wb'),
        #         protocol=pickle.HIGHEST_PROTOCOL)
        print >> f_joint, random_pos
        print >> f_end_effector, end_effector_position

        iteration += 1
        print("iteration",iteration)
        if iteration == 1000:
            break
Exemplo n.º 19
0
    def get_joint_trajectory_msg(self, path_config):
        trajectory = JointTrajectory()
        trajectory.header = self.group.get_current_pose().header
        trajectory.joint_names = self._get_joint_names()
        time_from_start = 0
        for config in path_config:
            trajectory.points.append(
                self.get_joint_trajectory_point_msg(config, time_from_start))
            time_from_start += 0.5

        return trajectory
Exemplo n.º 20
0
def main():

    rospy.init_node('send_joints')
    pub = rospy.Publisher('/trajectory_controller/command',
                          JointTrajectory,
                          queue_size=10)

    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR5
    traj.joint_names = [
        'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
        'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
    ]

    reader.next()
    rate = rospy.Rate(50)
    while not rospy.is_shutdown():
        traj.header.stamp = rospy.Time.now()
        pts = JointTrajectoryPoint()
        # for row in reader:
        #     shoulder_pan_joint = (row[0])
        #     shoulder_lift_joint = (row[1])
        #     elbow_joint = (row[2])
        #     wrist_1_joint = (row[3])
        #     wrist_2_joint = (row[4])
        #     wrist_3_joint = (row[5])
        #     pts.positions = [shoulder_pan_joint, shoulder_lift_joint, \
        #         elbow_joint, wrist_1_joint, wrist_2_joint, \
        #         wrist_3_joint]

        #     pts.time_from_start = rospy.Duration(0.02)

        #     # Set the points to the trajectory
        #     traj.points = []
        #     traj.points.append(pts)
        #     # Publish the message
        #     pub.publish(traj)

        pts.positions = [
            1.2778283886, -0.6807085686, -0.2895342907, -0.5970439271,
            -0.2896611142, 8.45356745875492E-05
        ]

        pts.time_from_start = rospy.Duration(0.02)

        # Set the points to the trajectory
        traj.points = []
        traj.points.append(pts)
        # Publish the message
        pub.publish(traj)
Exemplo n.º 21
0
 def push_one_arm(self, new_plan, left_arm_joints):
     # Build a new joint trajectory with left_arm_joint values at those times
     temp_traj = JointTrajectory()
     new_traj = new_plan.joint_trajectory
     temp_traj.header = new_traj.header
     temp_traj.joint_names = self.traj.joint_names
     zero_vec = [0.0]*7
     for i in range(len(new_traj.points)):
         temp_traj.points[i].positions = left_arm_joints.values() + new_traj.points[i].positions
         temp_traj.points[i].velocities = zero_vec + new_traj.points[i].velocities
         temp_traj.points[i].accelerations = zero_vec + new_traj.points[i].accelerations
         temp_traj.points[i].time_from_start = new_traj.points[i].time_from_start
     self.traj = temp_traj
Exemplo n.º 22
0
def build_jt(header, joint_names, y_r, dy_r):
    jt = JointTrajectory()
    jt.header = header
    jt.joint_names = joint_names

    jtp = JointTrajectoryPoint()
    for i in range(y_r.shape[0]):
        jtp.positions = y_r[i, :].tolist()
        jtp.velocities = dy_r[i, :].tolist()
        jtp.time_from_start = rospy.Duration(1.0)
        jt.points.append(jtp)

    return jt
Exemplo n.º 23
0
    def move_all_joints(self, point):
        joints_str = JointTrajectory()
        joints_str.header = Header()
        joints_str.header.stamp = rospy.Time.now()

        joints_str.joint_names = [
            'neck_1', 'neck_2', 'neck_3', 'neck_4', 'neck_5', 'shoulder_l1',
            'shoulder_l2', 'biceps_l', 'elbow_l', 'forearm_l', 'shoulder_r1',
            'shoulder_r2', 'biceps_r', 'elbow_r', 'forearm_r'
        ]

        joints_str.points.append(point)
        self.goal_dynamixel_position_publisher.publish(joints_str)
Exemplo n.º 24
0
    def min_jerk(self, setpoint, frequency, move_time):
        current = np.array(self.joint_states_msg.position)
        timefreq = int(move_time * frequency)

        trajectory = []
        trajectory_derivative = []

        rospy.logwarn("Min_Jerk trajectory generation...")

        for time in range(1, timefreq):
            trajectory.append(current + (setpoint - current) *
                              (10.0 * (time / timefreq)**3 - 15.0 *
                               (time / timefreq)**4 + 6.0 *
                               (time / timefreq)**5))

            trajectory_derivative.append(
                frequency * (1.0 / timefreq) * (setpoint - current) *
                (30.0 * (time / timefreq)**2.0 - 60.0 *
                 (time / timefreq)**3.0 + 30.0 * (time / timefreq)**4.0))

        joints_str = JointTrajectory()
        joints_str.header = Header()
        joints_str.header.stamp = rospy.Time.now()
        joints_str.joint_names = [
            'neck_1', 'neck_2', 'neck_3', 'neck_4', 'neck_5', 'shoulder_l1',
            'shoulder_l2', 'biceps_l', 'elbow_l', 'forearm_l', 'shoulder_r1',
            'shoulder_r2', 'biceps_r', 'elbow_r', 'forearm_r'
        ]

        rospy.logwarn("Waypoints generation...")

        #Adding the point to the points list
        for i in range(1, timefreq - 1):
            point = JointTrajectoryPoint()
            point.positions = np.around(trajectory[i], 2)

            rospy.loginfo(point.positions)

            point.velocities = np.around(trajectory_derivative[i], 2)

            rospy.loginfo(point.velocities)

            point.time_from_start = rospy.Duration(
                move_time)  ##dipende da quanti punti prendiamo? testare
            joints_str.points.append(point)

        self.goal_dynamixel_position_publisher.publish(joints_str)

        rospy.logwarn("Trajectory published!")

        rospy.loginfo(joints_str)
Exemplo n.º 25
0
def go_position(goal):
    pub = rospy.Publisher('/trajectory_controller/command',JointTrajectory,queue_size=10)
    goal_x=goal.fpos_x
    goal_y=goal.fpos_y
    print("goal_x:",goal_x)
    print("goal_y:",goal_y)
    #goal_z=goal.fpos_z
    now_position=np.array([0,0])
    # Create the topic message
    traj = JointTrajectory()
    traj.header = Header()
    # Joint names for UR5
    traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint','elbow_joint','wrist_3_joint']
    rate = rospy.Rate(10)
    #th1, th2, nx, ny, nz, ox, oy, oz, ax, ay, az, Px, Py, Pz, a1, a2=symbols('th1,th2,nx,ny,nz,ox,oy,oz,ax,ay,az,Px,Py,Pz,a1,a2')
    while not rospy.is_shutdown():
        #input position, output angle algori
        #RotZ1 = np.array([[cos(th1), sin(th1), 0, 0],[sin(th1), cos(th1), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
        #RotZ2 = np.array([[cos(th2), -sin(th2), 0, 0], [sin(th2), cos(th2), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
        #TransA1 = np.array([[1, 0, 0, a1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
        #TransA2 = np.array([[1, 0, 0, a2], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
        #T_0_2 = RotZ1*TransA1*RotZ2*TransA2
        #TT = np.array([[nx, ox, ax, Px], [ny, oy, ay, Py], [nz, oz, az, Pz], [0, 0, 0, 1]])        
        #LS1 = simplify(np.linalg.inv(RotZ1)*TT)
        #RS1 = simplify(np.linalg.inv(RotZ1)*T_0_2)

        #assume link lenght
        a1=5 #link1 length
        a2=3 #link2 length
        Px=goal_x #goal_x
        Py=goal_y #goal_y
        th2 = np.array([2*atan((float((a1+a2)**2-(Px**2+Py**2))/float(Px**2+Py**2-(a1-a2)**2))**0.5), -2*atan((float((a1+a2)**2-(Px**2+Py**2))/float(Px**2+Py**2-(a1-a2)**2))**0.5)])
        th1 = np.array([atan2(float(Py),float(Px)) - atan(float(a2*sin(th2[0]))/float(a1+a2*cos(th2[0]))),atan2(float(Py),float(Px)) - atan(float(a2*sin(th2[1]))/float(a1+a2*cos(th2[1])))])
        traj.header.stamp = rospy.Time.now()
        pts = JointTrajectoryPoint()
        #choice th1[1],th2[1] // th1[2],th2[2]
        pts.positions = [th1[1],th2[1] ,0.0 , 0.0]
        pts.time_from_start = rospy.Duration(1.0)

        # Set the points to the trajectory
        traj.points = []
        traj.points.append(pts)
        # Publish the message
        pub.publish(traj)

    result=godestinationResult()
    result.fpos_x=now_position[0]
    result.fpos_y=now_position[1]
    result.task=True
    server.set_succeeded(result,"destination completed successfully")
 def callback(self, msg):
     pub_msg = JointTrajectory()
     pub_msg.header = msg.header
     pub_msg.joint_names = ['rhand_thumb_roll', 'rhand_thumb_pitch', 'rhand_middle_pitch']
     rhand_thumb_roll_id = msg.goal.trajectory.joint_names.index('R_THUMB_JOINT0')
     rhand_thumb_pitch_id = msg.goal.trajectory.joint_names.index('R_THUMB_JOINT1')
     rhand_middle_pitch_id = msg.goal.trajectory.joint_names.index('R_MIDDLE_JOINT0')
     pub_msg.points = []
     for p in msg.goal.trajectory.points:
         point = JointTrajectoryPoint()
         point.positions = [p.positions[head_pitch_id], p.positions[head_yaw_id]]#TOFIX
         point.velocities = [p.velocities[head_pitch_id], p.velocities[head_yaw_id]]
         point.time_from_start = p.time_from_start
         pub_msg.points.append(point)
     self.joint_command_pub.publish(pub_msg)
Exemplo n.º 27
0
    def publishArmControllerCommand(self, waypoints):
        """
        Publishes a list of waypoints of given positions, velocities and durations to the trajectory controller

        Structure
        waypoints = [
            {
                'positions': []
                'velocities': []
                'duration': Float
            }
        ]
        """

        # Create the topic message
        traj = JointTrajectory()
        traj.header = Header()
        traj.header.stamp = rospy.Time.now()

        # Joint names for UR5
        traj.joint_names = [
            'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
            'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
        ]

        t = 0.0
        traj.points = []
        for waypoint in waypoints:
            t += waypoint['duration']
            pts = JointTrajectoryPoint()
            pts.positions = waypoint['positions']
            pts.velocities = waypoint['velocities']
            pts.time_from_start = rospy.Duration(t)

            traj.points.append(pts)

        self._publishers['arm_controller_command'].publish(traj)

        # Sleep for duration
        motionIsFinished = False
        counter = 0
        while not motionIsFinished and not rospy.is_shutdown():
            if counter >= (t + 0.1) * SLEEP_RATE: motionIsFinished = True
            counter += 1

            self._rate.sleep()

        return
Exemplo n.º 28
0
    def _add_motion_command_to_queue(self, command):
        tmp = JointTrajectory()
        tmp.header = command.header
        tmp.joint_names = ['pan_joint', 'pan_joint']
        tmp.points = [JointTrajectoryPoint()]
        tmp.points[0].positions = [
            command.pan_cmd.pos_rad, command.tilt_cmd.pos_rad
        ]
        tmp.points[0].velocities = [
            command.pan_cmd.vel_rps, command.tilt_cmd.vel_rps
        ]
        tmp.points[0].accelerations = [
            command.pan_cmd.acc_rps2, command.tilt_cmd.acc_rps2
        ]

        self._cmd_buffer.put(tmp.points[0])
Exemplo n.º 29
0
def issue_commands(publishers):
    for pub in publishers:
        point = JointTrajectoryPoint()
        point.positions = [
            random() * 2 * np.pi,
            random() + 0.1,
            random() * 0.05
        ]
        point.time_from_start = rospy.Duration(
            1)  # max(dur) / self._speed_scale

        cmd = JointTrajectory()
        cmd.header = Header()
        cmd.joint_names = ['joint1', 'joint2', 'joint5']
        cmd.points = [point]

        pub.publish(cmd)
Exemplo n.º 30
0
 def push_one_arm(self, new_plan, left_arm_joints):
     # Build a new joint trajectory with left_arm_joint values at those times
     temp_traj = JointTrajectory()
     new_traj = new_plan.joint_trajectory
     temp_traj.header = new_traj.header
     temp_traj.joint_names = self.traj.joint_names
     zero_vec = [0.0] * 7
     for i in range(len(new_traj.points)):
         temp_traj.points[i].positions = left_arm_joints.values(
         ) + new_traj.points[i].positions
         temp_traj.points[
             i].velocities = zero_vec + new_traj.points[i].velocities
         temp_traj.points[
             i].accelerations = zero_vec + new_traj.points[i].accelerations
         temp_traj.points[i].time_from_start = new_traj.points[
             i].time_from_start
     self.traj = temp_traj
Exemplo n.º 31
0
def talker():
    pub_traj = rospy.Publisher('joint_path_command', JointTrajectory)
    rospy.init_node('path_publisher')
        
    traj = JointTrajectory()
    traj.header = Header(frame_id='base_link', stamp=rospy.Time.now() + rospy.Duration(1.0))
    traj.joint_names = ['joint_1', 'joint_2', 'joint_3', 
                        'joint_4', 'joint_5', 'joint_6']
    traj.points = [JointTrajectoryPoint(positions=[0, 0, 0, 0, 0, 0],
                                        velocities=[0, 0, 0, 0, 0, 0],
                                        time_from_start=rospy.Duration(1)),
                   JointTrajectoryPoint(positions=[1, 0, 0, 0, 0, 0],
                                        velocities=[0, 0, 0, 0, 0, 0])]
    rospy.loginfo(traj)
    pub_traj.publish(traj)
        
    rospy.sleep(1.0)
Exemplo n.º 32
0
def talker():
    pub = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=10)
    rospy.init_node('trajectory')
    rate = rospy.Rate(10) # 10hz
    joints_str = JointTrajectory()
    joints_str.header = Header()
    joints_str.header.stamp = rospy.Time.now()
    joints_str.joint_names = ['base_joint', 'shoulder', 'elbow', 'wrist']
    point=JointTrajectoryPoint()
    point.positions = [0.5, 0.5, 0.5, 0.5]
    point.time_from_start = rospy.Duration(2)
    joints_str.points.append(point)
    while not rospy.is_shutdown():
      joints_str.header.stamp = rospy.Time.now()
      pub.publish(joints_str)
      rospy.loginfo("position updated")
      rate.sleep()
Exemplo n.º 33
0
    def start(self):
        self.start = rospy.Time.now()
        rate = rospy.Rate(10)
        s = 0
        while not (rospy.is_shutdown() or s >= len(self.x_discretized)):

            # Build JointTrajectory message
            header = Header()
            header.seq = s
            header.stamp = rospy.get_rostime()
            header.frame_id = 'inertial frame'

            joint_trajectory_msg = JointTrajectory()
            joint_trajectory_msg.header = header
            joint_trajectory_msg.joint_names = ['drone']

            # Build JointTrajectoryPoint
            for i in range(min(self.WINDOW_FRAME,
                               len(self.x_discretized) - s)):
                joint_trajectory_point = JointTrajectoryPoint()
                joint_trajectory_point.positions = [
                    self.x_discretized[s + i], self.y_discretized[s + i],
                    self.z_discretized[s + i], self.ya_discretized[s + i]
                ]
                joint_trajectory_point.velocities = [
                    self.vx_discretized[s + i], self.vy_discretized[s + i],
                    self.vz_discretized[s + i]
                ] if i != (self.WINDOW_FRAME - 1) else [.0, .0, .0]
                joint_trajectory_point.accelerations = [
                    self.ax_discretized[s + i], self.ay_discretized[s + i],
                    self.az_discretized[s + i]
                ] if i != (self.WINDOW_FRAME - 1) else [.0, .0, .0]
                joint_trajectory_point.effort = [None]
                joint_trajectory_point.time_from_start = self.ti_discretized[s
                                                                             +
                                                                             i]

                joint_trajectory_msg.points.append(joint_trajectory_point)

            s = s + 1
            # trajectory = "new trajectory %s" % rospy.get_time()
            rospy.loginfo('##########################################')
            rospy.loginfo(joint_trajectory_msg)
            self.pub.publish(joint_trajectory_msg)
            rate.sleep()
Exemplo n.º 34
0
 def set_joints_manually(self, joint_angles, joint_names, time):
     joint_angles_np = np.array(joint_angles)
     zeros_list = (joint_angles_np * 0).tolist()
     joint_angles_list = joint_angles_np.tolist()
     h = Header()
     h.stamp = rospy.Time.now(
     )  # Note you need to call rospy.init_node() before this will work. This needs to be any number in the future
     pos = joint_angles
     vel = zeros_list
     acc = zeros_list
     eff = zeros_list
     dur = rospy.Duration(time)
     point = JointTrajectoryPoint(pos, vel, acc, eff, dur)
     trajectory = JointTrajectory()
     trajectory.joint_names = joint_names
     trajectory.points = [point]
     trajectory.header = h
     self.direct_joint_command_pub.publish(trajectory)
Exemplo n.º 35
0
 def substitute_trajectory(self, trajectory):
     jt = JointTrajectory()
     jt.joint_names = self.real_joint_names
     jt.header = trajectory.header
     for jtp_in in trajectory.points:
         jtp = JointTrajectoryPoint()
         if jtp_in.positions:
             joint_position = jtp_in.positions[0]
             # Each finger will go to half of the size
             jtp.positions = [joint_position / 2.0, joint_position / 2.0]
             jtp.time_from_start = jtp_in.time_from_start
         else:
             rospy.logwarn("Trajectory has no positions...")
         if jtp_in.velocities:
             jtp.velocities.append(jtp_in.velocities[0])
         if jtp_in.accelerations:
             jtp.accelerations.append(jtp_in.accelerations[0])
         if jtp_in.effort:
             jtp.effort.append(jtp_in.effort[0])
         jt.points.append(jtp)
     return jt
def functional(axis, angle):
    pub_rosey = rospy.Publisher(
      '/cyton_joint_trajectory_action_controller/command',
      JointTrajectory, queue_size=10)
    rospy.init_node('traj_maker', anonymous=True)
    time.sleep(1)

    axis = int(axis)
    angle = float(angle)

    rate = rospy.Rate(0.01)
    while not rospy.is_shutdown():

           #  first way to define a point
           traj_waypoint_1_rosey = JointTrajectoryPoint()

           traj_waypoint_1_rosey.positions = [0,0,0,0,0,0,0]
           traj_waypoint_1_rosey.time_from_start = Duration(2)
           
           #  second way to define a point
           traj_waypoint_2_rosey = traj_waypoint_1_rosey
           traj_waypoint_2_rosey.time_from_start = Duration(4)
           traj_waypoint_2_rosey.positions[axis-1] = angle
           
           time.sleep(1)

           traj_waypoint_3_rosey = traj_waypoint_2_rosey
           traj_waypoint_3_rosey.time_from_start = Duration(7)

           #traj_waypoint_2_rosey = JointTrajectoryPoint(positions=[.31, -.051, .33, -.55, .28, .60,0],
            #time_from_start = Duration(4))
           #traj_waypoint_3_rosey = JointTrajectoryPoint(positions=[.14726, -.014151, .166507, -.33571, .395997, .38657,0],
            #time_from_start = Duration(6))
           #traj_waypoint_4_rosey = JointTrajectoryPoint(positions=[-.09309, .003150, .003559, .16149, .524427, -.1867,0],
            #time_from_start = Duration(8))
           #traj_waypoint_5_rosey = JointTrajectoryPoint(positions=[-.27752, .077886, -.1828, .38563, .682589, -.44665,0],
            #time_from_start = Duration(10))   
           #traj_waypoint_6_rosey = JointTrajectoryPoint(positions=[0,0,0,0,0,0,0],time_from_start = Duration(12))

           #debug in terminal
           print traj_waypoint_2_rosey.positions
           
           #  making message
           message_rosey = JointTrajectory()
           
           #  required headers
           header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
           message_rosey.header = header_rosey
           
           #  adding in joints
           joint_names = ['shoulder_roll_joint', \
  'shoulder_pitch_joint', 'shoulder_yaw_joint', 'elbow_pitch_joint', \
  'elbow_yaw_joint', 'wrist_pitch_joint', 'wrist_roll_joint']
           message_rosey.joint_names = joint_names
           
           #  appending up to 100 points
           # ex. for i in enumerate(len(waypoints)): append(waypoints[i])

           message_rosey.points.append(traj_waypoint_1_rosey)
           message_rosey.points.append(traj_waypoint_2_rosey)
           message_rosey.points.append(traj_waypoint_3_rosey)
           #message_rosey.points.append(traj_waypoint_4_rosey)
           #message_rosey.points.append(traj_waypoint_5_rosey)
           #message_rosey.points.append(traj_waypoint_6_rosey)
           #  publishing to ROS node
           pub_rosey.publish(message_rosey)
         
           rate.sleep()
           
           if rospy.is_shutdown():
               break