Пример #1
2
def scale_trajectory_speed(traj, scale):
    # Create a new trajectory object
    new_traj = RobotTrajectory()
    # Initialize the new trajectory to be the same as the planned trajectory
    new_traj.joint_trajectory = traj.joint_trajectory
    # Get the number of joints involved
    n_joints = len(traj.joint_trajectory.joint_names)
    # Get the number of points on the trajectory
    n_points = len(traj.joint_trajectory.points)
    # Store the trajectory points
    points = list(traj.joint_trajectory.points)
    # Cycle through all points and scale the time from start, speed and acceleration
    for i in range(n_points):
        point = JointTrajectoryPoint()
        point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
        point.velocities = list(traj.joint_trajectory.points[i].velocities)
        point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
        point.positions = traj.joint_trajectory.points[i].positions
        for j in range(n_joints):
            point.velocities[j] = point.velocities[j] * scale
            point.accelerations[j] = point.accelerations[j] * scale * scale
        points[i] = point
    # Assign the modified points to the new trajectory
    new_traj.joint_trajectory.points = points
    # Return the new trajectory
    return new_traj
Пример #2
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)
Пример #3
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)
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)
Пример #5
0
    def control_loop(self):
        if self.commands is None:
            raise Exception('Command list is empty.  Was the control plan loaded?')
        
        # loop through the command list
        for cmd in self.commands:

            # wait for proxy to be in active state
            self.wait_for_state(self._proxy_state_running)
            
            # process commands
            cmd_spec_str = None
            spec = None
            t = cmd[ProxyCommand.key_command_type]
            if not (t == "noop"):
                cmd_spec_str = cmd[ProxyCommand.key_command_spec]
                if not isinstance(cmd_spec_str, basestring):
                    spec = float(cmd_spec_str)
                else:
                    spec = self.positions[cmd_spec_str]
                rospy.loginfo("Command type: " + t + ", spec: " + str(cmd_spec_str) + ", value: " + str(spec))
                       
            # check for any wait depends
            self.wait_for_depend(cmd)

            # execute command
            # could do this with a dictionary-based function lookup, but who cares
            if t == 'noop':
                rospy.loginfo("Command type: noop")
                self.wait_for_depend(cmd)
            elif t == 'sleep':
                rospy.loginfo("sleep command")
                v = float(spec)
                rospy.sleep(v)
            elif t == 'move_gripper':
                rospy.loginfo("gripper command")
                self.move_gripper(spec)
            elif t == 'move_arm':
                rospy.loginfo("move_arm command")
                rospy.logdebug(spec)                
                goal = FollowJointTrajectoryGoal()
                goal.trajectory.joint_names = self.arm_joint_names
                jtp = JointTrajectoryPoint()
                jtp.time_from_start = rospy.Duration(0.5)  # fudge factor for gazebo controller
                jtp.positions = spec
                jtp.velocities = [0]*len(spec)
                goal.trajectory.points.append(jtp)
                self._arm_goal = copy.deepcopy(goal)
                self.move_arm()
            elif t == 'plan_exec_arm':
                rospy.loginfo("plan and execute command not implemented")
                raise NotImplementedError()
            else:
                raise Exception("Invalid command type: " + str(cmd.type))

            # check for any set dependencies action
            self.set_depend(cmd)

            # check for any clear dependencies action
            self.clear_depend(cmd)
    def __init__(self, arm = 'l', record_data = False):
        self.arm = arm
        if arm == 'r':
            print "using right arm on Darci"
            self.RIGHT_ARM = 0  # i don't think this was used anywhere
        else:
            self.LEFT_ARM = 1
        #     print "Left and both arms not implemented in this client yet ... \n"
        #     print "will require writing different function calls since joint data for left and right arm come in together from the meka server"
        #     sys.exit()

        self.kinematics = dak.DarciArmKinematics(arm)
        self.joint_pub = rospy.Publisher('/'+self.arm+'_arm_controller/command', JointTrajectory)
        self.joint_names = None
        self.lock = RLock()
        self.joint_angles = None
        self.joint_velocities = None
        self.J_h = None
        self.time = None
        self.desired_joint_angles = None
        self.stiffness_percent = 0.75
        self.ee_force = None
        self.ee_torque = None
        
        self.skins = None #will need code for all of these skin interfaces
        self.Jc_l = []
        self.n_l = []
        self.values_l = []

        #values from m3gtt repository in robot_config folder
        # These could be read in from a yaml file like this (import yaml; stream = open("FILE_NAME_HERE", 'r'); data = yaml.load(stream))
        # However, not clear if absolute path to yaml file (possibly on another computer) is better then just defining it here
        # The downside is obviously if someone tunes gains differently on the robot.
        self.joint_stiffness = (np.array([1, 1, 1, 1, 0.06, 0.08, 0.08])*180/np.pi*self.stiffness_percent).tolist()
        self.joint_damping = (np.array([0.06, 0.1, 0.015, 0.015, 0.0015, 0.002, 0.002])*180/np.pi*self.stiffness_percent).tolist()
        
        self.record_data = record_data

        if self.record_data:
            from collections import deque
            self.q_record = deque()
            self.qd_record = deque()
            self.times = deque()

        self.state_sub = rospy.Subscriber('/'+self.arm+'_arm_controller/state', JointTrajectoryControllerState, self.robotStateCallback)
        rospy.sleep(1.0)

        while self.joint_angles is None:
            rospy.sleep(0.05)
        self.desired_joint_angles = copy.copy(self.joint_angles)

        self.joint_cmd = JointTrajectory()
        self.joint_cmd.header.stamp = rospy.Time.now()
        self.joint_cmd.header.frame_id = '/torso_lift_link'
        self.joint_cmd.joint_names = self.joint_names
        jtp = JointTrajectoryPoint()
        jtp.positions = self.desired_joint_angles
        jtp.velocities = [1.]*len(self.joint_names)
        self.joint_cmd.points = [jtp]
        self.joint_pub.publish(self.joint_cmd)
Пример #7
0
def traj_speed_up(traj, spd=3.0):
    new_traj = RobotTrajectory()
    new_traj = traj

    n_joints = len(traj.joint_trajectory.joint_names)
    n_points = len(traj.joint_trajectory.points)

    points = list(traj.joint_trajectory.points)
    
    for i in range(n_points):
        point = JointTrajectoryPoint()
        point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd
        point.velocities = list(traj.joint_trajectory.points[i].velocities)
        point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
        point.positions = traj.joint_trajectory.points[i].positions

        for j in range(n_joints):
            point.velocities[j] = point.velocities[j] * spd
            point.accelerations[j] = point.accelerations[j] * spd * spd

        points[i] = point

    new_traj.joint_trajectory.points = points

    return new_traj
Пример #8
0
	def test_5(self):

		# Test 5: move each joint in robotic arm to a specific value at the same time (mixed speeds)
		rospy.loginfo("Begin test 5")
		# Create a joint state publisher
		self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory)
		rospy.sleep(1)
		# How fast will we update the robot's movement? in Hz
        	rate = 10
		# Set the equivalent ROS rate variable
        	r = rospy.Rate(rate)
		# Define a joint trajectory message for grasp position
		rospy.loginfo("Position arm in stowed position")
		msg = JointTrajectory()
		msg.header.seq = 1
		msg.header.stamp = rospy.Time.now()
		msg.header.frame_id = '0'
		msg.joint_names.append('joint_1')
		msg.joint_names.append('joint_2')
		msg.joint_names.append('joint_3')
		msg.joint_names.append('joint_4')
		msg.joint_names.append('joint_5')
		# Define joint trajectory points
		point = JointTrajectoryPoint()
		point.positions  = [0.0, -1.3, 1.7, 1.2, 0.1]
		point.velocities = [90.0, 50.0, 20.0, 10.0, 10.0]
		point.time_from_start = rospy.Duration(3.0)
		msg.points.append(point)
            	# publish joint stage message
            	self.arm_pub.publish(msg)
		rospy.loginfo("End test 5")
Пример #9
0
 def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
     if self._mode == "velocity":
         velocities = [0.0] * len(joint_names)
         cmd = dict(zip(joint_names, velocities))
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_velocities(cmd)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
     elif self._mode == "position" or self._mode == "position_w_id":
         raw_pos_mode = self._mode == "position_w_id"
         if raw_pos_mode:
             pnt = JointTrajectoryPoint()
             pnt.positions = self._get_current_position(joint_names)
             if dimensions_dict["velocities"]:
                 pnt.velocities = [0.0] * len(joint_names)
             if dimensions_dict["accelerations"]:
                 pnt.accelerations = [0.0] * len(joint_names)
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
             # zero inverse dynamics feedforward command
             if self._mode == "position_w_id":
                 pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
                 ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
                 self._pub_ff_cmd.publish(ff_pnt)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
Пример #10
0
 def add_point(self, positions, velocities, accelerations, time):
     point = JointTrajectoryPoint()
     point.positions = copy(positions)
     point.velocities = copy(velocities)
     point.accelerations = copy(accelerations)
     point.time_from_start = rospy.Duration(time)
     self._goal.trajectory.points.append(point)
Пример #11
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)  # 始点なので、待ち時間 = 0 [sec]

  # 中間地点定義
  middle_pt = JointTrajectoryPoint()
  for j in joint_names:
    idx = end.name.index(j)
    middle_pt.positions.append((start.position[idx] + end.position[idx])/2.0)
    middle_pt.velocities.append(0)
  middle_pt.time_from_start = rospy.Duration(duration) # 中間地点までの到達時間 = duration [sec]

  # 執着地点
  end_pt = JointTrajectoryPoint()
  for j in joint_names:
    idx = end.name.index(j)
    end_pt.positions.append(end.position[idx])
    end_pt.velocities.append(0.0)
  end_pt.time_from_start = rospy.Duration(duration*2) # 終点までの到達時間 = duration*2 [sec]
  
  return JointTrajectory(joint_names=joint_names, points=[start_pt, middle_pt, end_pt])
def build_traj(start, end, duration):

  # print start.name
  # print end.name
  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.05]*len(start.position)
  start_pt.time_from_start = rospy.Duration(0.0)

  end_pt = JointTrajectoryPoint()
  # end_pt = start_pt;
  # end_pt.positions[3] = end_pt.positions[3]+.1
  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])
Пример #13
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
Пример #14
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 runTrajectory(req):

    global Traj_server;
    
    print "---------------------------------"
    print req.task
    print " "
    print req.bin_num
    print " "
    print req.using_torso
    print "---------------------------------"
    
    if req.using_torso == "y":
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
    else:
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);

    if req.task == "Forward":
        file_name = file_root+"/forward";
    elif req.task == "Drop":
        file_name = file_root+"/drop";
    else :
        return taskResponse(False);

    f = open(file_name,"r");
    plan_file = RobotTrajectory();
    buf = f.read();
    plan_file.deserialize(buf);
    
    plan = copy.deepcopy(plan_file); 
  
        
    arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); 
    arm_left_group = moveit_commander.MoveGroupCommander("arm_left");	
    arm_left_group.set_start_state_to_current_state();    
        
    StartPnt = JointTrajectoryPoint();
    StartPnt.positions = arm_left_group.get_current_joint_values();
    StartPnt.velocities = [0]*len(StartPnt.positions);
    StartPnt. accelerations = [0]*len(StartPnt.positions);
    
    #print StartPnt;
    plan.joint_trajectory.points[0] = StartPnt;
    #print plan;
    
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
    display_trajectory = moveit_msgs.msg.DisplayTrajectory();    
    robot = moveit_commander.RobotCommander();
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    display_trajectory_publisher.publish(display_trajectory);  
   
   
    print "============ Waiting while", file_name, " is visualized (again)..." 
    arm_left_group.execute(plan);
    print "Trajectory ", file_name, " finished!"   
    
    f.close();
    
    return taskResponse(True);
	def stage_3(self):

		# Move arm to grasping position
		rospy.loginfo("Begin stage 3")
		# Create a joint state publisher
		self.arm_pub = rospy.Publisher('arm_controller/command', JointTrajectory)
		rospy.sleep(1)
		# How fast will we update the robot's movement? in Hz
        	rate = 10
		# Set the equivalent ROS rate variable
        	r = rospy.Rate(rate)
		# Define a joint trajectory message for grasp position
		rospy.loginfo("Position arm in grasp position")
		msg = JointTrajectory()
		msg.header.seq = 1
		msg.header.stamp = rospy.Time.now()
		msg.header.frame_id = '0'
		msg.joint_names.append('joint_1')
		msg.joint_names.append('joint_2')
		msg.joint_names.append('joint_3')
		msg.joint_names.append('joint_4')
		msg.joint_names.append('joint_5')
		# Define joint trajectory points
		point = JointTrajectoryPoint()
		point.positions  = [0.0, 0.0, 0.0, 1.6, 0.1]
		point.velocities = [20.0, 20.0, 20.0, 10.0, 10.0]
		point.time_from_start = rospy.Duration(3.0)
		msg.points.append(point)
            	# publish joint stage message
            	self.arm_pub.publish(msg)	
		rospy.sleep(3)
    def test_trajectory(self):
        # our goal variable
        goal = FollowJointTrajectoryGoal()

        # First, the joint names, which apply to all waypoints
        goal.trajectory.joint_names = ('shoulder_pitch_joint',
                                       'shoulder_pan_joint',
                                       'upperarm_roll_joint',
                                       'elbow_flex_joint',
                                       'forearm_roll_joint',
                                       'wrist_pitch_joint',
                                       'wrist_roll_joint')

        # We will have two waypoints in this goal trajectory

        # First trajectory point
        # To be reached 1 second after starting along the trajectory
        point = JointTrajectoryPoint()
        point.positions = [0.0] * 7
        point.velocities = [1.0] * 7
        point.time_from_start = rospy.Duration(3.0)
        goal.trajectory.points.append(point)

        #we are done; return the goal
        return goal
Пример #18
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 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
Пример #21
0
    def test_controller(self):

        from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

        topic = "/pr2/torso_controller/command"

        rospy.init_node("test_pr2_torso_controller", log_level=rospy.DEBUG, disable_signals=True)

        rospy.loginfo("Preparing to publish on %s" % topic)
        ctl = rospy.Publisher(topic, JointTrajectory)

        self.assertEquals(getjoint("torso_lift_joint"), 0.0)

        duration = 0.1

        traj = JointTrajectory()
        traj.joint_name = "torso_lift_joint"

        initialpoint = JointTrajectoryPoint()
        initialpoint.positions = 0.0
        initialpoint.velocities = 0.0
        initialpoint.time_from_start = rospy.Duration(0.0)

        finalpoint = JointTrajectoryPoint()
        finalpoint.positions = 0.195
        finalpoint.velocities = 0.0
        finalpoint.time_from_start = rospy.Duration(duration)

        # First, move up
        traj.points = [initialpoint, finalpoint]

        ctl.publish(traj)
        time.sleep(duration + 0.1)
        self.assertEquals(getjoint("torso_lift_joint"), 0.195)

        # Go back to initial position
        finalpoint.time_from_start = rospy.Duration(0.0)
        initialpoint.time_from_start = rospy.Duration(duration)
        traj.points = [finalpoint, initialpoint]

        ctl.publish(traj)
        time.sleep(duration + 0.1)
        self.assertEquals(getjoint("torso_lift_joint"), 0.0)
Пример #22
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)
Пример #23
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
Пример #24
0
def np(pts, pos, time):
    pt = JointTrajectoryPoint()
    pt.positions = [pos]

    if len(pts)==0:
        st = rospy.Duration(0)
    else:
        st = pts[-1].time_from_start
    pt.velocities = [0]
    pt.time_from_start = st + rospy.Duration(time)
    pts.append(pt)
Пример #25
0
    def create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None):
        # Hack
        vel_arr = [[0.]*7]*len(pos_arr)
        acc_arr = [[0.]*7]*len(pos_arr)

        ##
        # Compute joint velocities and acclereations.
        def get_vel_acc(q_arr, d_arr):
            vel_arr = [[0.]*7]
            acc_arr = [[0.]*7]
            for i in range(1, len(q_arr)):
                vel, acc = [], []
                for j in range(7):
                    vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]]
                    acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]]
                vel_arr += [vel]
                acc_arr += [acc]
                print vel, acc
            return vel_arr, acc_arr

        if arm != 1:
            arm = 0
        jtg = JointTrajectoryGoal()
        if stamp is None:
            stamp = rospy.Time.now()
        else:
            jtg.trajectory.header.stamp = stamp

        if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None):
            v_arr, a_arr = get_vel_acc(pos_arr, dur_arr)
            if vel_arr is None:
                vel_arr = v_arr
            if acc_arr is None:
                acc_arr = a_arr

        jtg.trajectory.joint_names = self.joint_names_list[arm]
        for i in range(len(pos_arr)):
            if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType:
                continue
            jtp = JointTrajectoryPoint()
            jtp.positions = pos_arr[i]
            if vel_arr is None:
                vel = [0.] * 7
            else:
                vel = vel_arr[i]
            jtp.velocities = vel
            if acc_arr is None:
                acc = [0.] * 7
            else:
                acc = acc_arr[i]
            jtp.accelerations = acc
            jtp.time_from_start = rospy.Duration(dur_arr[i])
            jtg.trajectory.points.append(jtp)
        return jtg
	def stage_8(self):

		# move arms to stowed position
		rospy.loginfo("Begin stage 8")
		# Stow robotic arm
		# Create a joint state publisher for each turtlebot
		self.arm_1_pub = rospy.Publisher('turtlebot_1/arm_controller/command', JointTrajectory)
		self.arm_2_pub = rospy.Publisher('turtlebot_2/arm_controller/command', JointTrajectory)
		rospy.sleep(1)
		# How fast will we update the robot's movement? in Hz
        	rate = 10
		# Set the equivalent ROS rate variable
        	r = rospy.Rate(rate)
		# Define a joint trajectory message for grasp position
		rospy.loginfo("Position arm in stowed position")
		rospy.sleep(3)
		msg = JointTrajectory()
		msg.header.seq = 1
		msg.header.stamp = rospy.Time.now()
		msg.header.frame_id = '0'
		msg.joint_names.append('joint_1')
		msg.joint_names.append('joint_2')
		msg.joint_names.append('joint_3')
		msg.joint_names.append('joint_4')
		msg.joint_names.append('joint_5')
		# Define joint trajectory points
		point = JointTrajectoryPoint()
		point.positions  = [0.0, 1.5, 0.7, 0.0, -1.8]
		point.velocities = [2.0, 2.0, 20.0, 10.0, 1.0]
		point.time_from_start = rospy.Duration(2.0)
		msg.points.append(point)
		# Define joint trajectory points
		point = JointTrajectoryPoint()
		point.positions  = [0.0, -1.0, 1.9, 1.2, 0.1]
		point.velocities = [50.0, 50.0, 50.0, 50.0, 10.0]
		point.time_from_start = rospy.Duration(3.0)
		msg.points.append(point)
            	# publish joint stage message
            	self.arm_1_pub.publish(msg)
		self.arm_2_pub.publish(msg)
		rospy.sleep(3)
Пример #27
0
 def move_gripper(self, opening_mm): 
     # Creates a goal to send to the action server.
     goal = FollowJointTrajectoryGoal()
     goal.trajectory.joint_names = self.gripper_joint_names
     jtp = JointTrajectoryPoint()
     jtp.positions = [opening_mm/2000.0, opening_mm/2000.0]
     jtp.velocities = [0, 0]
     jtp.time_from_start = rospy.Duration(1.0)
     goal.trajectory.points.append(jtp)
     self._ac_gripper.send_goal(goal)
     self._ac_gripper.wait_for_result()
     return self._ac_gripper.get_result()
Пример #28
0
	def init_left_arm_cb(userdata, goal):
		point = JointTrajectoryPoint()
		point.time_from_start = rospy.Duration.from_sec(2)
		point.positions = [ -0.5, 0.1, -0.1, 0.6109, 0.0, 0.0, 0.0 ]
		point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
		joint_goal = JointTrajectoryGoal()
		joint_goal.trajectory.joint_names = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 
						     'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 
						     'arm_left_7_joint']
		joint_goal.trajectory.header.stamp = rospy.get_rostime()
		joint_goal.trajectory.points.append(point)
		return joint_goal
Пример #29
0
	def tuck_right_arm_torso_cb(userdata,goal):
		point = JointTrajectoryPoint()
		point.time_from_start = rospy.Duration.from_sec(2)
		point.positions = [ 0.0, 0.0, 0.3, 0.1, -1.0, 0.8, 0.0, 0.0, 0.0 ]
		point.velocities = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
		joint_goal = JointTrajectoryGoal()
		joint_goal.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint',
		                                     'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint',
		                                     'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint']
		joint_goal.trajectory.header.stamp = rospy.get_rostime()
		joint_goal.trajectory.points.append(point)
		return joint_goal
Пример #30
0
 def robotTrajectoryFromPlan(self, plan, joint_names):
     """Given a dmp plan (GetDMPPlanResponse) create a RobotTrajectory to be able to visualize what it consists and also
     to be able to send it to execution"""
     rt = RobotTrajectory()
     rt.joint_trajectory.joint_names = joint_names
     for point, time in zip(plan.plan.points, plan.plan.times):
         jtp = JointTrajectoryPoint()
         jtp.positions = point.positions
         jtp.velocities = point.velocities
         jtp.time_from_start = rospy.Duration( time )
         #rospy.logwarn("jtp is: " + str(jtp))
         rt.joint_trajectory.points.append(jtp)
     return rt
Пример #31
0
def point():
    rospy.init_node('point_control')

    pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, queue_size=10)
    rospy.Subscriber('/arm_start_stop', Bool, run_cb)

    rospy.Subscriber("/offset_joint_azim", UInt16, offset_joint_azim_cb)
    rospy.Subscriber("/offset_joint_elev", UInt16, offset_joint_elev_cb)

    rospy.Subscriber("/vel_1", UInt16, vel_1_cb)
    rospy.Subscriber("/vel_2", UInt16, vel_2_cb)

    grados_joint_azim = rospy.get_param("/grados_joint_azim")
    rospy.loginfo("%s is %s", rospy.resolve_name('grados_joint_azim'),
                  grados_joint_azim)

    grados_joint_elev = rospy.get_param("/grados_joint_elev")
    rospy.loginfo("%s is %s", rospy.resolve_name("/grados_joint_elev"),
                  grados_joint_elev)

    ang1 = grados_joint_azim * np.pi / 180  #np.pi*2
    ang2 = grados_joint_elev * np.pi / 180  #np.pi*3/4

    offset_joint_azim = rospy.get_param("/offset_joint_azim")
    rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_azim"),
                  offset_joint_azim)

    offset_joint_elev = rospy.get_param("/offset_joint_elev")
    rospy.loginfo("%s is %s", rospy.resolve_name("/offset_joint_elev"),
                  offset_joint_elev)

    ang1_offset = offset_joint_azim * np.pi / 180  #np.pi*2
    ang2_offset = offset_joint_elev * np.pi / 180  #np.pi*3/4

    pub_period = 0.05

    vel_1 = rospy.get_param("/vel_1")
    rospy.loginfo("%s is %s", rospy.resolve_name('/vel_1'), vel_1)

    vel_2 = rospy.get_param("/vel_2")
    rospy.loginfo("%s is %s", rospy.resolve_name('/vel_2'), vel_2)

    start_time = time.time()
    time.sleep(0.01)
    next_point_time = 0
    dt = 0
    while not rospy.is_shutdown():
        if (time.time() > next_point_time) and run_var == True:
            dt = time.time() - start_time
            p = JointTrajectoryPoint()
            i = ang1 * np.sin((vel_1 * dt) * 2 * np.pi) + ang1_offset
            j = ang2 * np.sin((vel_2 * dt) * 2 * np.pi) + ang2_offset
            k = vel_1 * 2 * np.pi * ang1 * np.cos(
                (vel_1 * (dt - pub_period)) * 2 * np.pi)
            l = vel_2 * 2 * np.pi * ang2 * np.cos((vel_2 * dt) * 2 * np.pi)
            p.positions = [i, j]
            p.velocities = [k, l]
            p.accelerations = [0, 0]
            p.time_from_start = rospy.Time.now()

            rospy.loginfo("point \n %s ", p)
            pub.publish(p)
            next_point_time = time.time() + pub_period
        elif run_var == False:
            start_time = time.time() + dt
def main():
    step_ts = 0.004
    rospy.init_node("test_moveit_commander_custom_trajectory", anonymous=True)
    rospy.Subscriber("controller_state", ControllerState, callback_function)
    robot = urdf.robot_from_parameter_server()
    controller_commander=controller_commander_pkg.ControllerCommander()
    
    controller_commander.set_controller_mode(controller_commander.MODE_AUTO_TRAJECTORY, 0.4, [], [])
    time.sleep(0.5)
    tran = np.array([2.197026484647054, 1.2179574262842452, 0.12376598588449844])
    rot = np.array([[-0.99804142,  0.00642963,  0.06222524], [ 0.00583933,  0.99993626, -0.00966372], [-0.06228341, -0.00928144, -0.99801535]])
    pose_target2 = rox.Transform(rot, tran)
    pose_target2.p[2] += 0.20
    
    print 'Target:',pose_target2    
    print "============ Move Close to Panel"
    controller_commander.compute_cartesian_path_and_move(pose_target2, avoid_collisions=False)
    
    controller_commander.set_controller_mode(controller_commander.MODE_AUTO_TRAJECTORY, 1.0, [], [])
    time.sleep(1.0)
    Kc = 0.004
    time_save = []
    FTdata_save = []
    Tran_z = np.array([[0,0,-1],[0,-1,0],[1,0,0]])    
    Vec_wrench = 100*np.array([0.019296738361905,0.056232033265447,0.088644197659430,    
    0.620524934626544,-0.517896661195076,0.279323567303444,-0.059640563813256,   
    0.631460085138371,-0.151143175570223,-6.018321330845553]).transpose()
    
    listener=PayloadTransformListener()
    rapid_node = rapid_node_pkg.RAPIDCommander()
    #controller_commander=controller_commander_pkg.arm_composites_manufacturing_controller_commander()
    time.sleep(1.0)

    FTdata_0 = FTdata
    T = listener.lookupTransform("base", "link_6", rospy.Time(0))
    rg = 9.8*np.matmul(np.matmul(T.R,Tran_z).transpose(),np.array([0,0,1]).transpose())
    A1 = np.hstack([rox.hat(rg).transpose(),np.zeros([3,1]),np.eye(3),np.zeros([3,3])])
    A2 = np.hstack([np.zeros([3,3]),rg.reshape([3,1]),np.zeros([3,3]),np.eye(3)])
    A = np.vstack([A1,A2])
    FTdata_0est = np.matmul(A,Vec_wrench)
    #print 'Test4:',controller_commander.ControllerState

    for i in range(400):  
        tic = time.time()              
        plan=RobotTrajectory()    
        plan.joint_trajectory.joint_names=['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
        current_joint_angles = controller_commander.get_current_joint_values()
        
        plan.joint_trajectory.header.frame_id='/world'
        p1=JointTrajectoryPoint()
        p1.positions = current_joint_angles
        p1.velocities = np.zeros((6,))
        p1.accelerations = np.zeros((6,))
        p1.time_from_start = rospy.Duration(0)
        

        
        
        T = listener.lookupTransform("base", "link_6", rospy.Time(0))
        rg = 9.8*np.matmul(np.matmul(T.R,Tran_z).transpose(),np.array([0,0,1]).transpose())
        A1 = np.hstack([rox.hat(rg).transpose(),np.zeros([3,1]),np.eye(3),np.zeros([3,3])])
        A2 = np.hstack([np.zeros([3,3]),rg.reshape([3,1]),np.zeros([3,3]),np.eye(3)])
        A = np.vstack([A1,A2])
        FTdata_est = np.matmul(A,Vec_wrench)

        #print 'Test0:',FTdata,FTdata_0,FTdata_est,FTdata_0est
        FTread = FTdata-FTdata_0-FTdata_est+FTdata_0est
        print 'FTread:',FTread
        
        print 'FT:',FTdata
        print 'Z',FTread[-1]
        if FTread[-1]>-200:
            F_d = -350
        else:
            F_d = -400

        
        J = rox.robotjacobian(robot, current_joint_angles)
        Vz = Kc*(F_d - FTread[-1])
        joints_vel = np.linalg.pinv(J).dot(np.array([0,0,0,0,0,Vz]+dx))
        print 'Joint_command:',joints_vel.dot(step_ts)
                 
        p2=JointTrajectoryPoint()
        p2.positions = np.array(p1.positions) + joints_vel.dot(step_ts)#np.array([0,np.deg2rad(-2),0,0,0,0])
        p2.velocities = np.zeros((6,))
        p2.accelerations = np.zeros((6,))
        p2.time_from_start = rospy.Duration(step_ts)
        

        
        plan.joint_trajectory.points.append(p1)
        plan.joint_trajectory.points.append(p2)

        
        controller_commander.execute(plan)
        print 'Time:', time.time()-tic

        time_save.append(time.time())
        FTdata_save.append(FTread)
    
    filename = "FTdata.txt"
    f_handle = file(filename, 'a')
    np.savetxt(f_handle, FTdata_save)
    f_handle.close()   
    

    filename = "Time.txt"
    f_handle = file(filename, 'a')
    np.savetxt(f_handle, time_save)
    f_handle.close() 
    def _on_trajectory_action(self, goal):
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        # Load parameters for trajectory
        self._get_trajectory_parameters(joint_names, goal)
        # Create a new discretized joint trajectory
        num_points = len(trajectory_points)
        if num_points == 0:
            rospy.logerr("%s: Empty Trajectory" % (self._action_name, ))
            self._server.set_aborted()
            return
        rospy.loginfo("%s: Executing requested joint trajectory" %
                      (self._action_name, ))
        rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
        control_rate = rospy.Rate(self._control_rate)

        dimensions_dict = self._determine_dimensions(trajectory_points)

        if num_points == 1:
            # Add current position as trajectory point
            first_trajectory_point = JointTrajectoryPoint()
            first_trajectory_point.positions = self._get_current_position(
                joint_names)
            # To preserve desired velocities and accelerations, copy them to the first
            # trajectory point if the trajectory is only 1 point.
            if dimensions_dict['velocities']:
                first_trajectory_point.velocities = deepcopy(
                    trajectory_points[0].velocities)
            if dimensions_dict['accelerations']:
                first_trajectory_point.accelerations = deepcopy(
                    trajectory_points[0].accelerations)
            first_trajectory_point.time_from_start = rospy.Duration(0)
            trajectory_points.insert(0, first_trajectory_point)
            num_points = len(trajectory_points)

        # Force Velocites/Accelerations to zero at the final timestep
        # if they exist in the trajectory
        # Remove this behavior if you are stringing together trajectories,
        # and want continuous, non-zero velocities/accelerations between
        # trajectories
        if dimensions_dict['velocities']:
            trajectory_points[-1].velocities = [0.0] * len(joint_names)
        if dimensions_dict['accelerations']:
            trajectory_points[-1].accelerations = [0.0] * len(joint_names)

        # Compute Full Bezier Curve Coefficients for all 6 joints
        pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
        try:
            b_matrix = self._compute_bezier_coeff(joint_names,
                                                  trajectory_points,
                                                  dimensions_dict)
        except Exception as ex:
            rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
                          " arm with error \"{2}: {3}\"").format(
                              self._action_name, self._name,
                              type(ex).__name__, ex))
            self._server.set_aborted()
            return
        # Wait for the specified execution time, if not provided use now
        start_time = goal.trajectory.header.stamp.to_sec()
        if start_time == 0.0:
            start_time = rospy.get_time()
        crustcrawler_dataflow.wait_for(lambda: rospy.get_time() >= start_time,
                                       timeout=float('inf'))
        # Loop until end of trajectory time.  Provide a single time step
        # of the control rate past the end to ensure we get to the end.
        # Keep track of current indices for spline segment generation
        now_from_start = rospy.get_time() - start_time
        end_time = trajectory_points[-1].time_from_start.to_sec()
        while (now_from_start < end_time and not rospy.is_shutdown()):
            #Acquire Mutex
            now = rospy.get_time()
            now_from_start = now - start_time
            idx = bisect.bisect(pnt_times, now_from_start)
            #Calculate percentage of time passed in this interval
            if idx >= num_points:
                cmd_time = now_from_start - pnt_times[-1]
                t = 1.0
            elif idx >= 0:
                cmd_time = (now_from_start - pnt_times[idx - 1])
                t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1])
            else:
                cmd_time = 0
                t = 0

            point = self._get_bezier_point(b_matrix, idx, t, cmd_time,
                                           dimensions_dict)

            # Command Joint Position, Velocity, Acceleration
            command_executed = self._command_joints(joint_names, point,
                                                    start_time,
                                                    dimensions_dict)
            self._update_feedback(deepcopy(point), joint_names, now_from_start)
            # Release the Mutex
            if not command_executed:
                return
            control_rate.sleep()
        # Keep trying to meet goal until goal_time constraint expired
        last = trajectory_points[-1]
        last_time = trajectory_points[-1].time_from_start.to_sec()
        end_angles = dict(zip(joint_names, last.positions))

        def check_goal_state():
            for error in self._get_current_error(joint_names, last.positions):
                if (self._goal_error[error[0]] > 0
                        and self._goal_error[error[0]] < math.fabs(error[1])):
                    return error[0]
            if (self._stopped_velocity > 0.0 and max([
                    abs(cur_vel)
                    for cur_vel in self._get_current_velocities(joint_names)
            ]) > self._stopped_velocity):
                return False
            else:
                return True

        while (now_from_start < (last_time + self._goal_time)
               and not rospy.is_shutdown()):
            if not self._command_joints(joint_names, last, start_time,
                                        dimensions_dict):
                return
            now_from_start = rospy.get_time() - start_time
            self._update_feedback(deepcopy(last), joint_names, now_from_start)
            control_rate.sleep()

        now_from_start = rospy.get_time() - start_time
        self._update_feedback(deepcopy(last), joint_names, now_from_start)

        # Verify goal constraint
        result = check_goal_state()
        if result is True:
            rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" %
                          (self._action_name, self._name))
            self._result.error_code = self._result.SUCCESSFUL
            self._server.set_succeeded(self._result)
        elif result is False:
            rospy.logerr(
                "%s: Exceeded Max Goal Velocity Threshold for %s arm" %
                (self._action_name, self._name))
            self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
            self._server.set_aborted(self._result)
        else:
            rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" %
                         (self._action_name, result, self._name))
            self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
            self._server.set_aborted(self._result)
        self._command_stop(goal.trajectory.joint_names, end_angles, start_time,
                           dimensions_dict)
    rospy.wait_for_service('/ve026a/set_motor')
    try:
        res = rospy.ServiceProxy('/ve026a/set_motor', SetBool)(True)
        print(res.message)
    except rospy.ServiceException, e:
        print("Service call failed, %s" % e)

    msg = JointTrajectory()
    msg.header.stamp = rospy.Time.now()
    msg.joint_names = [
        'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
    ]

    p1 = JointTrajectoryPoint()
    p1.positions = [0, 0, 0, 0, 0, 0]
    p1.velocities = [0, 0, 0, 0, 0, 0]
    p1.time_from_start = rospy.Duration.from_sec(3.0)

    p2 = JointTrajectoryPoint()
    p2.positions = [0.5, 0.5, 0, 0, 0, 0]
    p2.velocities = [0, 0, 0, 0, 0, 0]
    p2.time_from_start = rospy.Duration.from_sec(5.0)

    p3 = JointTrajectoryPoint()
    p3.positions = [0, 0, 0, 0, 0, 0]
    p3.velocities = [0, 0, 0, 0, 0, 0]
    p3.time_from_start = rospy.Duration.from_sec(7.0)

    msg.points = [p1, p2, p3]

    r = rospy.Rate(10)
    def move_arm(self, points_list):
        # # Unpause the physics
        # rospy.wait_for_service('/gazebo/unpause_physics')
        # unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        # unpause_gazebo()

        self.client.wait_for_server()

        goal = FollowJointTrajectoryGoal()    

        # We need to fill the goal message with its components
        #         
        # check msg structure with: rosmsg info FollowJointTrajectoryGoal
        # It is composed of 4 sub-messages:
        # "trajectory" of type trajectory_msgs/JointTrajectory 
        # "path_tolerance" of type control_msgs/JointTolerance
        # "goal_tolerance" of type control_msgs/JointTolerance
        # "goal_time_tolerance" of type duration

        trajectory_msg = JointTrajectory()
        # check msg structure with: rosmsg info JointTrajectory
        # It is composed of 3 sub-messages:
        # "header" of type std_msgs/Header 
        # "joint_names" of type string
        # "points" of type trajectory_msgs/JointTrajectoryPoint

        trajectory_msg.joint_names = [
            "j2n6s300_joint_1", 
            "j2n6s300_joint_2", 
            "j2n6s300_joint_3", 
            "j2n6s300_joint_4", 
            "j2n6s300_joint_5", 
            "j2n6s300_joint_6"
            ]

        points_msg = JointTrajectoryPoint()
        # check msg structure with: rosmsg info JointTrajectoryPoint
        # It is composed of 5 sub-messages:
        # "positions" of type float64
        # "velocities" of type float64
        # "accelerations" of type float64
        # "efforts" of type float64
        # "time_from_start" of type duration
        
        points_msg.positions = points_list
        points_msg.velocities = [0, 0, 0, 0, 0, 0]
        points_msg.accelerations = [0, 0, 0, 0, 0, 0]
        points_msg.effort = [0, 0, 0, 0, 0, 0]
        points_msg.time_from_start = rospy.Duration(0.01)

        # fill in points message of the trajectory message
        trajectory_msg.points = [points_msg]

        # fill in trajectory message of the goal
        goal.trajectory = trajectory_msg

        # self.client.send_goal_and_wait(goal)
        self.client.send_goal(goal)
        self.client.wait_for_result()

        # wait for the robot to finish moving
        # rospy.sleep(2)      # wait for 2s
        while not self.robot_has_stopped():
            pass
    def interpolated_ik_motion_planner_callback(self, req):

        #names and angles for the joints in their desired order
        joint_names = req.motion_plan_request.start_state.joint_state.name
        start_angles = req.motion_plan_request.start_state.joint_state.position

        #sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified
        if start_angles and len(joint_names) != len(start_angles):
            rospy.logerr("start_state.joint_state.name needs to be the same length as start_state.joint_state.position!  Quitting")
            return 0
    
        #reorder the start angles to the order needed by IK
        reordered_start_angles = []

        #get the current joint states for the robot
        #joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0)
        #if not joint_states_msg:
        #    rospy.logerr("unable to get joint_states message")
        #    return 0

        #get the desired start angles for each IK arm joint in turn from start_state.joint_state.position
        #(use the current angle if not specified)
        for joint_name in self.ik_utils.joint_names:

            #desired start angle specified
            if joint_name in joint_names and start_angles:
                index = joint_names.index(joint_name)
                reordered_start_angles.append(start_angles[index])
            else:
                rospy.logerr("missing joint angle, can't deal")
                return 0

            #desired start angle not specified, use the current angle
#elif 0: #joint_name in joint_states_msg.name:
#                index = joint_states_msg.name.index(joint_name)
#                current_position = joint_states_msg.position[index]
#                reordered_start_angles.append(current_position)

            #malformed joint_states message?
#            else:
#                rospy.logerr("an expected arm joint,"+joint_name+"was not found!")
#                return 0

        #get additional desired joint angles (such as for the gripper) to pass through to IK
        additional_joint_angles = []
        additional_joint_names = []
        for (ind, joint_name) in enumerate(joint_names):
            if joint_name not in self.ik_utils.joint_names:
                #rospy.loginfo("found %s"%joint_name)
                additional_joint_angles.append(start_angles[ind])
                additional_joint_names.append(joint_name)
        IK_robot_state = None
        if additional_joint_angles:
            #rospy.loginfo("adding additional start angles for:"+str(additional_joint_names))
            #rospy.loginfo("additional joint angles:"+str(additional_joint_angles))
            IK_robot_state = RobotState()
            IK_robot_state.joint_state.name = additional_joint_names
            IK_robot_state.joint_state.position = additional_joint_angles

        #check that the desired link is in the list of possible IK links (only r/l_wrist_roll_link for now)
        link_name = req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids[0]
        if link_name != self.ik_utils.link_name:
            rospy.logerr("link_name not allowed: %s"%link_name)
            return 0

        #the start pose for that link
        start_pose = req.motion_plan_request.start_state.multi_dof_joint_state.poses[0]

        #the frame that start pose is in
        frame_id = req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids[0]

        #turn it into a PoseStamped
        start_pose_stamped = self.add_header(PoseStamped(), frame_id)
        start_pose_stamped.pose = start_pose

        #the desired goal position
        goal_pos = req.motion_plan_request.goal_constraints.position_constraints[0].position         
        
        #the frame that goal position is in
        goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id

        #convert the position to base_link frame
        goal_ps = self.add_header(PointStamped(), goal_pos_frame)
        goal_ps.point = goal_pos
        goal_pos_list = self.ik_utils.point_stamped_to_list(goal_ps, 'base_link')

        #the desired goal orientation
        goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation

        #the frame that goal orientation is in
        goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id 

        #convert the quaternion to base_link frame
        goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame)
        goal_qs.quaternion = goal_quat
        goal_quat_list = self.ik_utils.quaternion_stamped_to_list(goal_qs, 'base_link')

        #assemble the goal pose into a PoseStamped
        goal_pose_stamped = self.add_header(PoseStamped(), 'base_link')
        goal_pose_stamped.pose = Pose(Point(*goal_pos_list), Quaternion(*goal_quat_list))

        #get the ordered collision operations, if there are any
        ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations
        #if ordered_collision_operations.collision_operations == []:
        #    ordered_collision_operations = None

        #get the link paddings, if there are any
        link_padding = None #req.motion_plan_request.link_padding
        #if link_padding == []:
        #    link_padding = None

        #RUN!  Check the Cartesian path for consistent, non-colliding IK solutions
        (trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \
                 goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \
                 self.consistent_angle, self.collision_aware, self.collision_check_resolution, \
                 self.steps_before_abort, self.num_steps, ordered_collision_operations, \
                 self.start_from_end, IK_robot_state, link_padding)

        #find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0)
        #if we're searching from the end, keep the end; if we're searching from the start, keep the start
        start_ind = 0
        stop_ind = len(error_codes)
        if self.start_from_end:
            for ind in range(len(error_codes)-1, 0, -1):
                if error_codes[ind]:
                    start_ind = ind+1
                    break
        else:
            for ind in range(len(error_codes)):
                if error_codes[ind]:
                    stop_ind = ind
                    break
        (times, vels) = self.ik_utils.trajectory_times_and_vels(trajectory[start_ind:stop_ind], self.max_joint_vels, self.max_joint_accs)
        times = [0]*start_ind + times + [0]*(len(error_codes)-stop_ind)
        vels = [[0]*7]*start_ind + vels + [[0]*7]*(len(error_codes)-stop_ind)

        rospy.logdebug("trajectory:")
        for ind in range(len(trajectory)):
            rospy.logdebug("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
        rospy.logdebug("")
        for ind in range(len(trajectory)):
            rospy.logdebug("time: " + "%5.3f  "%times[ind] + "vels: " + self.pplist(vels[ind]))

        #the response
        res = GetMotionPlanResponse()

        #the arm joint names in the normal order, as spit out by IKQuery
        res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:]

        #a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal 
        #(all 0s if there was no IK solution for a point on the path)
        res.trajectory.joint_trajectory.points = []
        for i in range(len(trajectory)):
            joint_trajectory_point = JointTrajectoryPoint()
            joint_trajectory_point.positions = trajectory[i]
            joint_trajectory_point.velocities = vels[i]
            joint_trajectory_point.time_from_start = rospy.Duration(times[i])
            res.trajectory.joint_trajectory.points.append(joint_trajectory_point)

        #a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows:
        #ArmNavigationErrorCodes.SUCCESS (1): no problem
        #ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided)
        #ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point
        #ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution)
        #ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point
        error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \
                           2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \
                           4:ArmNavigationErrorCodes.PLANNING_FAILED}

        trajectory_error_codes = [ArmNavigationErrorCodes(val=error_code_dict[error_code]) for error_code in error_codes]
        res.trajectory_error_codes = trajectory_error_codes 
        res.error_code.val = ArmNavigationErrorCodes.SUCCESS
#         rospy.loginfo("trajectory:")
#         for ind in range(len(trajectory)):
#             rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
#         rospy.loginfo("")
#         for ind in range(len(trajectory)):
#             rospy.loginfo("time: " + "%5.3f  "%times[ind] + "vels: " + self.pplist(vels[ind]))

        return res
Пример #37
0
    d = rospy.Duration
    while not rospy.is_shutdown():
        fake_traj = FollowJointTrajectoryActionGoal()
        fake_traj.header = Header()
        # fake_traj.header.stamp

        fake_traj.goal = FollowJointTrajectoryGoal()

        fake_traj.goal.trajectory = JointTrajectory()
        fake_traj.goal.trajectory.joint_names = [
            'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5'
        ]

        # fake_traj.goal.trajectory.points = JointTrajectoryPoint()[]
        for j in xrange(20):
            point = JointTrajectoryPoint()
            point.positions = [random.random() for i in xrange(5)]
            point.velocities = [random.random() for i in xrange(5)]
            point.time_from_start = rospy.Duration.from_sec(j)
            fake_traj.goal.trajectory.points.append(point)

        # fake_traj.goal.trajectory.points.positions = [[random.random() ]
        # fake_traj.goal.trajectory.points.velocities = [[random.random() for i in xrange(5)] for j in xrange(20)]
        # fake_traj.goal.trajectory.points.time_from_start = [d.from_sec(i * 0.1) for i in xrange(20)]
        # fake_traj.goal.trajectory.points.append(positions)
        # fake_traj.goal.trajectory.points.append(velocities)
        # fake_traj.goal.trajectory.points.append(time_from_start)

        pub.publish(fake_traj)
        rate.sleep()
Пример #38
0
g.command = goal

gms.goal=g

# gp.publish(gms)

msg = JointTrajectory()
subMsg = JointTrajectoryPoint()
# print(msg)
# print(subMsg)

# print(type(zeros))
# solver.printDoubleVec(q_targ)
print(ths)
subMsg.positions = ths
subMsg.velocities = zeros
subMsg.accelerations = zeros
msg.joint_names = joints
msg.points=[subMsg]
rate=rospy.Rate(1)
subMsg.time_from_start = rospy.Duration(0.5)




for i in range(2):
    msg.header.stamp = rospy.Time.now()
    pub.publish(msg)
    gp.publish(gms)
    rate.sleep()
Пример #39
0
from actionlib import ActionClient
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

rospy.init_node('initial_joint_state_setter', anonymous=True)
ac = ActionClient('/whole_body_controller/body/follow_joint_trajectory',
                  FollowJointTrajectoryAction)
ac.wait_for_server()
goal = FollowJointTrajectoryGoal()
traj = JointTrajectory()
traj.header.stamp = rospy.get_rostime()
traj.joint_names = [
    'torso_lift_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint',
    'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint',
    'r_wrist_flex_joint', 'r_wrist_roll_joint', 'l_upper_arm_roll_joint',
    'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint',
    'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint',
    'head_pan_joint', 'head_tilt_joint'
]

p = JointTrajectoryPoint()
p.positions = [
    0.1687062500000004, 0, 0, 0, 0, -1.1400000000000001, -1.0499999999999998,
    0, 0, 0, 0, 0, -1.1399999999999995, -1.0499999999999998, 0, 0, 0
]
p.velocities = [0] * len(p.positions)
p.time_from_start = rospy.Duration(1)
traj.points.append(p)
goal.trajectory = traj
ac.send_goal(goal)
rospy.init_node('send_open_manipulator_gripper_joint_angles')

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

controller_name = "gripper_controller"
joint_names = rospy.get_param("/%s/joints" % controller_name)

rospy.loginfo("Joint names: %s" % joint_names)

rate = rospy.Rate(10)

trajectory_command = JointTrajectory()

trajectory_command.joint_names = joint_names

point = JointTrajectoryPoint()
#Joint names: ['gripper']
point.positions = [-0.1]

point.velocities = [0.0]
point.time_from_start = rospy.rostime.Duration(1, 0)

trajectory_command.points = [point]

while not rospy.is_shutdown():
    trajectory_command.header.stamp = rospy.Time.now()
    pub.publish(trajectory_command)
    rate.sleep()
Пример #41
0
        rospy.logerr(msg)
        rospy.signal_shutdown(msg)
        sys.exit(1)

    # Creates a goal to send to the action server.
    goal = FollowJointTrajectoryGoal()
    goal.trajectory.joint_names = [
        'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
        'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint',
        'l_wrist_flex_joint', 'l_wrist_roll_joint'
    ]

    goal.trajectory.points = []
    point0 = JointTrajectoryPoint()
    point0.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    point0.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    # To be reached 1 second after starting along the trajectory
    point0.time_from_start = rospy.Duration(1.0)
    goal.trajectory.points.append(point0)

    point1 = JointTrajectoryPoint()
    point1.positions = [-0.3, 0.2, -0.1, -1.2, 1.5, 0.5, 0.0]
    point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0]
    point1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    point1.time_from_start = rospy.Duration(2.0)
    goal.trajectory.points.append(point1)

    point2 = JointTrajectoryPoint()
    point2.positions = [0.0, 0.1, 0.0, 0.0, 0.0, 0.1, 0.0]
    point2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    point2.time_from_start = rospy.Duration(3.0)
Пример #42
0
def trayectoria_2():
    
    pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=10) 
    rospy.init_node('trayectoria_2', anonymous=True)

    trajectory = JointTrajectory()
    trajectory.joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "gripper_revolute_joint"]
    trajectory.header.stamp = rospy.Time.now()
    trajectory.header.frame_id = "base_footprint";
    #Posicion default
    jtp = JointTrajectoryPoint()
    jtp.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp.velocities = [0.1, 0.1, 0.1, 0.1, 0.0, 0.0]
    jtp.time_from_start = rospy.Duration(3)
    trajectory.points.append(jtp)
    #Aproximacion a la pieza
    jtp_2 = JointTrajectoryPoint()
    jtp_2.positions = [0.0, 0.8560, 0.5522, -1.3622, 0.0, 0.0]
    jtp_2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_2.time_from_start = rospy.Duration(6)
    trajectory.points.append(jtp_2)
    #Cierra la pinza
    jtp_3 = JointTrajectoryPoint()
    jtp_3.positions = [0.0, 0.8560, 0.5522, -1.3622, 0.0, 1.5]
    jtp_3.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_3.time_from_start = rospy.Duration(9)
    trajectory.points.append(jtp_3)
    #Sube
    jtp_4 = JointTrajectoryPoint()
    jtp_4.positions = [0.0, -0.2362, -0.5844, 0.4488, 0.0, 1.5]
    jtp_4.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_4.time_from_start = rospy.Duration(12)
    trajectory.points.append(jtp_4)
    #Giro del brazo
    jtp_5 = JointTrajectoryPoint()
    jtp_5.positions = [1.6, -0.2362, -0.5844, 0.4488, 0.0, 1.5]
    jtp_5.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_5.time_from_start = rospy.Duration(15)
    trajectory.points.append(jtp_5)
    #Aproximacion a destino
    jtp_6 = JointTrajectoryPoint()
    jtp_6.positions = [1.6, -0.1212, -0.8375, 0.9741, 0.0, 1.5]
    jtp_6.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_6.time_from_start = rospy.Duration(18)
    trajectory.points.append(jtp_6)
    #Suelta la pieza
    jtp_7 = JointTrajectoryPoint()
    jtp_7.positions = [1.6, -0.1212, -0.8375, 0.9741, 0.0, 0.0]
    jtp_7.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_7.time_from_start = rospy.Duration(21)
    trajectory.points.append(jtp_7)
    #Gira de vuelta
    jtp_8 = JointTrajectoryPoint()
    jtp_8.positions = [0.0, -0.1212, -0.8375, 0.9741, 0.0, 0.0]
    jtp_8.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_8.time_from_start = rospy.Duration(24)
    trajectory.points.append(jtp_8)
    #Posicion default
    jtp_9 = JointTrajectoryPoint()
    jtp_9.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_9.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    jtp_9.time_from_start = rospy.Duration(27)
    trajectory.points.append(jtp_9)    

    pub.publish(trajectory)
def main(whole_body, gripper, wrist_wrench):

    rospy.sleep(5)

    armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
                             JointTrajectory,
                             queue_size=1)

    #1.READ THE ANGLE (topic angle_detector)
    #angle_Msg = rospy.wait_for_message("angle_detector", Float64)
    #angle = angle_Msg.data

    #2.READ THE HANDLE POSITION when the door is closed - SERVICE!!!
    #target_pose_Msg = rospy.wait_for_message("localize_handle", PoseStamped)
    #recog_pos.pose.position.x=target_pose_Msg.pose.position.x
    #recog_pos.pose.position.y=target_pose_Msg.pose.position.y
    #recog_pos.pose.position.z=target_pose_Msg.pose.position.z

    if (angle > -1 and angle < 4):  #the door is closed

        ##3.GRAB THE DOOR HANDLE
        whole_body.move_to_neutral()
        # whole_body.impedance_config= 'grasping'
        switch = ImpedanceControlSwitch()
        # wrist_wrench.reset()
        gripper.command(1.0)

        grab_pose = geometry.multiply_tuples(
            geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
                          y=recog_pos.pose.position.y,
                          z=recog_pos.pose.position.z,
                          ej=math.pi / 2), geometry.pose(ek=math.pi / 2))

        whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
        wrist_wrench.reset()
        # whole_body.impedance_config= 'compliance_middle'
        switch.activate("grasping")
        # gripper.command(0.01)
        gripper.grasp(-0.008)
        rospy.sleep(1.0)
        switch.inactivate()

        wrist_wrench.reset()
        rospy.sleep(8.0)

        #### test manipulation
        whole_body.impedance_config = 'grasping'

        #4.ROTATE HANDLE
        # print(whole_body.impedance_config)
        # desired_rot=-1.95
        # whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
        wrist_roll = latest_positions["wrist_roll_joint"] - 0.55

        traj = JointTrajectory()
        # This controller requires that all joints have values
        traj.joint_names = [
            "arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
            "wrist_flex_joint", "wrist_roll_joint"
        ]
        p = JointTrajectoryPoint()
        current_positions = [
            latest_positions[name] for name in traj.joint_names
        ]
        current_positions[0] = latest_positions["arm_lift_joint"] - 0.04
        current_positions[1] = latest_positions["arm_flex_joint"] - 0.015
        current_positions[2] = latest_positions["arm_roll_joint"]
        current_positions[3] = latest_positions["wrist_flex_joint"]
        current_positions[4] = wrist_roll
        p.positions = current_positions
        p.velocities = [0, 0, 0, 0, 0]
        p.time_from_start = rospy.Time(3)
        traj.points = [p]

        armPub.publish(traj)

        rospy.sleep(5.0)

        whole_body.impedance_config = 'grasping'

        #5.PUSH MOTION - he moves with the door - first option - streight movement
        phi = 90 * (2 * math.pi / 360)
        l = HANDLE_TO_DOOR_HINGE_POS
        d = 2 * l * math.cos(phi)
        #open_pose = geometry.pose(x = d*math.sin(phi), y=d*math.cos(phi), ek=math.pi/2 - angle_rad )
        #whole_body.move_end_effector_pose(open_pose, _ROBOT_TF)
        #angleeee=math.pi/2 - angle_rad
        omni_base.go(d * math.sin(phi),
                     d * math.cos(phi),
                     math.pi / 2 - angle_rad,
                     300.0,
                     relative=True)

        whole_body.move_to_neutral()

    elif (angle >= 4 and angle < 60):  #the door is half-open

        #3.ROTATE PARALLEL TO THE DOOR
        angle_rad = angle * (2 * math.pi / 360)
        #omni_base.go_rel(0.0, 0.0, angle_rad , 300.0)

        ##4.GRAB THE DOOR HANDLE
        whole_body.move_to_neutral()
        # whole_body.impedance_config= 'grasping'
        switch = ImpedanceControlSwitch()
        # wrist_wrench.reset()
        gripper.command(0.0)

        #change coordinates of the handle - now the door is open so the handle is moved - the data of the handle position are given for the closed door - I m supposing
        #that the coordinates of the handle are wrt the base_footprint tf
        phi = math.pi / 2 - angle_rad / 2
        l = HANDLE_TO_DOOR_HINGE_POS
        d1 = 2 * l * math.sin(angle_rad / 2)
        recog_pos.pose.position.x = recog_pos.pose.position.x + d1 * math.sin(
            phi)
        recog_pos.pose.position.y = recog_pos.pose.position.y + d1 * math.cos(
            phi)

        grab_pose = geometry.multiply_tuples(
            geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS,
                          y=recog_pos.pose.position.y - 10,
                          z=recog_pos.pose.position.z,
                          ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
        whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
        wrist_wrench.reset()

        rospy.sleep(1.0)
        switch.inactivate()

        wrist_wrench.reset()
        rospy.sleep(8.0)

        #### test manipulation
        whole_body.impedance_config = 'grasping'

        #5.PUSH MOTION - he moves with the door - first option - streight movement
        phi = math.pi / 4 + angle_rad / 2
        l = HANDLE_TO_DOOR_HINGE_POS
        d = 2 * l * math.cos(phi)

        omni_base.go(d * math.sin(phi),
                     d * math.cos(phi),
                     math.pi / 2 - angle_rad,
                     300.0,
                     relative=True)
                      queue_size=10)
rospy.sleep(0.5)

traj = JointTrajectory()
traj.joint_names = [
    'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
    'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]

now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
traj.header.stamp = now

p1 = JointTrajectoryPoint()
p1.positions = [1.5, -0.2, -1.57, 0, 0, 0]
p1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p1.time_from_start = rospy.Duration(5.0)
traj.points.append(p1)

p2 = JointTrajectoryPoint()
p2.positions = [1.5, 0, -1.57, 0, 0, 0]
p2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p2.time_from_start = rospy.Duration(10.0)
traj.points.append(p2)

p3 = JointTrajectoryPoint()
p3.positions = [2.2, 0, -1.57, 0, 0, 0]
p3.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p3.time_from_start = rospy.Duration(15.0)
traj.points.append(p3)
Пример #45
0
# wait for the action server to establish connection
cli.wait_for_server()
head_cli.wait_for_server()

traj_goal = FollowJointTrajectoryGoal()
traj = JointTrajectory()

traj.joint_names = [
    "odom_x", "odom_y", "odom_t", "arm_lift_joint", "arm_flex_joint",
    "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"
]

p = JointTrajectoryPoint()
p.positions = [0.0, 0.0, 0.0, 0.69, -2.60, 0.0, -0.54, 0.0]
p.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p.time_from_start = rospy.Time(3)

traj.points = [p]
traj_goal.trajectory = traj

cli.send_goal(traj_goal)

cli.wait_for_result()

head_traj_goal = FollowJointTrajectoryGoal()
head_traj = JointTrajectory()

head_traj.joint_names = ["head_pan_joint", "head_tilt_joint"]

h_p = JointTrajectoryPoint()
Пример #46
0
import rospy
from trajectory_msgs.msg import JointTrajectoryPoint
''' Script for interacting with group_node from an interactive sessions

run as "python -i group_setpoint_control.py"

Modify the fields of 'msg' in the python console
to change the setpoint of the actuator group
'''

if __name__ == "__main__":
    rospy.init_node("trajectory_gen")
    t_pub = rospy.Publisher("/joint_target",
                            JointTrajectoryPoint,
                            queue_size=1)
    msg = JointTrajectoryPoint()
    msg.positions = [0, 0, 0]
    msg.velocities = [0, 0, 0]
    msg.accelerations = [0, 0, 0]
    timer = rospy.Timer(rospy.Duration(0.1), lambda x: t_pub.publish(msg))
    print("Modify 'msg' variable to control arm...")
Пример #47
0
    def __init__(self):
        self.rate = rospy.Rate(10)
        self.sentencec = "待機中"
        # topics
        #        self.sub_joint = rospy.Subscriber("/hsrb/joint_states", JointState ,self.now_pose,queue_size=10)
        self.sub = rospy.Subscriber("/hsrb_07/joy",
                                    Joy,
                                    self.callback,
                                    queue_size=10)
        rospy.Subscriber("/hsrb/wrist_wrench/compensated",
                         WrenchStamped,
                         self.cb_wrench_compensated,
                         queue_size=10)
        self.grip_act = rospy.Publisher("/hsrb/gripper_controller/grasp/goal",
                                        GripperApplyEffortActionGoal,
                                        queue_size=10)
        self.pub_list = rospy.Publisher('/hsrb/gripper_controller/command',
                                        JointTrajectory,
                                        queue_size=10)
        self.sub_obj = rospy.Subscriber("/hsr7/recog_obj/name",
                                        String,
                                        self.recog_obj,
                                        queue_size=10)
        #        self.vision_obj = rospy.Subscriber("/ext/vision_module/object_recognition_info", ObjectInfo, self.recog_vision,queue_size=10)
        self.pub_sig = rospy.Publisher(
            '/hsrb/gripper_trajectory_controller/command',
            JointTrajectory,
            queue_size=10)
        rospy.Subscriber("/AudioSentence",
                         AudioSentence,
                         self.set_audio,
                         queue_size=10)

        # service
        #
        self.hand_traj = JointTrajectory()
        self.hand_sig = JointTrajectory()
        self.hand_traj.joint_names = ["hand_motor_joint"]
        self.hand_sig.joint_names = [
            "hand_l_proximal_joint", "hand_r_proximal_joint"
        ]
        pp = JointTrajectoryPoint()
        pp.positions = [
            1.2,
        ]
        pp.velocities = [0]
        pp.effort = [0.1]
        pp.time_from_start = rospy.Time(3)
        self.hand_traj.points = [pp]
        self.hand_sig.points = [pp]
        self.hand_flug = True

        self.srv_record_start = rospy.ServiceProxy("rosbag_record",
                                                   RosbagRecord)
        self.srv_record_stop = rospy.ServiceProxy("rosbag_record_stop",
                                                  RosbagStop)
        self.srv_marker = rospy.ServiceProxy("marker_data_saver", Empty)
        self.rosbag_number = 0
        self.record_flag = False
        self.rosbag_time = rospy.Time(0)
        self.record_number = 0
        self.number = 0
        date = time.ctime()
        date = date.replace("  ", " ")
        self.date = date.split(" ")
        self.image_number = 0
        self.image_time = 0
        # TF
        self.buf = tf2_ros.Buffer()
        self.lis = tf2_ros.TransformListener(self.buf)
        self._wrench = WrenchStamped()
        self.obj_name = ""
        self.obj_time = rospy.Time.now().to_sec()
        self.vis_obj_name = ""
        self.vis_obj_num = 0
        self.vis_obj_time = rospy.Time.now().to_sec()
        self.flag_record = True
Пример #48
0
    def compose_trajectory(self, component_name, parameter_name):
        # get joint_names from parameter server
        param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
        if not rospy.has_param(param_string):
                rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",param_string)
                return (JointTrajectory(), 2)
        joint_names = rospy.get_param(param_string)

        # check joint_names parameter
        if not type(joint_names) is list: # check list
                rospy.logerr("no valid joint_names for %s: not a list, aborting...",component_name)
                print "joint_names are:",joint_names
                return (JointTrajectory(), 3)
        else:
            for i in joint_names:
                    #print i,"type1 = ", type(i)
                    if not type(i) is str: # check string
                            rospy.logerr("no valid joint_names for %s: not a list of strings, aborting...",component_name)
                            print "joint_names are:",param
                            return (JointTrajectory(), 3)
                    else:
                            rospy.logdebug("accepted joint_names for component %s",component_name)
        '''
        # get joint values from parameter server
        if type(parameter_name) is str:
                full_parameter_name = self.ns_global_prefix + "/" + component_name + "/" + parameter_name # parameter_name: joint_state
                if not rospy.has_param(full_parameter_name):
                        rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",full_parameter_name)
                        return (JointTrajectory(), 2)
                param = rospy.get_param(full_parameter_name)
        else:
                param = parameter_name

        # check trajectory parameters
        if not type(param) is list: # check outer list
                        rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
                        print "parameter is:",param
                        return (JointTrajectory(), 3)
        '''
        param = self.points

        traj = []
        for point in param:
                #print point,"type1 = ", type(point)
                if type(point) is str:
                        full_parameter_name = self.ns_global_prefix + "/" + component_name + "/" + point
                        if not rospy.has_param(full_parameter_name):
                                rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",full_parameter_name)
                                return (JointTrajectory(), 2)
                        point = rospy.get_param(full_parameter_name)
                        point = point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
                        #print point
                elif type(point) is list:
                        rospy.logdebug("point is a list")
                else:
                        rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
                        print "parameter is:",param
                        return (JointTrajectory(), 3)

                # here: point should be list of floats/ints
                #print point
                if not len(point) == len(joint_names): # check dimension
                        rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,len(joint_names),len(point))
                        print "parameter is:",param
                        return (JointTrajectory(), 3)

                for value in point:
                        #print value,"type2 = ", type(value)
                        if not ((type(value) is float) or (type(value) is int)): # check type
                                #print type(value)
                                rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
                                print "parameter is:",param
                                return (JointTrajectory(), 3)

                        rospy.logdebug("accepted value %f for %s",value,component_name)
                traj.append(point)

        rospy.logdebug("accepted trajectory for %s",component_name)

        # get current pos
        timeout = 3.0
        try:
                start_pos = rospy.wait_for_message("/" + component_name + "/joint_states", JointState, timeout = timeout).position
        except rospy.ROSException as e:
                rospy.logwarn("no joint states received from %s within timeout of %ssec. using default point time of 8sec.", component_name, str(timeout))
                start_pos = []

        # convert to ROS trajectory message
        traj_msg = JointTrajectory()
        # if no timestamp is set in header, this means that the trajectory starts "now"
        traj_msg.joint_names = joint_names
        point_nr = 0
        traj_time = 0

        param_string = self.ns_global_prefix + "/" + component_name + "/default_vel"
        if not rospy.has_param(param_string):
                default_vel = 0.1 # rad/s
                rospy.logwarn("parameter %s does not exist on ROS Parameter Server, using default of %f [rad/sec].",param_string,default_vel)
        else:
                default_vel = rospy.get_param(param_string)

        param_string = self.ns_global_prefix + "/" + component_name + "/default_acc"
        if not rospy.has_param(param_string):
                default_acc = 1.0 # rad^2/s
                rospy.logwarn("parameter %s does not exist on ROS Parameter Server, using default of %f [rad^2/sec].",param_string,default_acc)
        else:
                default_acc = rospy.get_param(param_string)

        for point in traj:
                point_nr = point_nr + 1
                point_msg = JointTrajectoryPoint()
                point_msg.positions = point

                # set zero velocities for last trajectory point only
                #if point_nr == len(traj):
                #	point_msg.velocities = [0]*len(joint_names)

                # set zero velocity and accelerations for all trajectory points
                point_msg.velocities = [0]*len(joint_names)
                point_msg.accelerations = [0]*len(joint_names)

                # use hardcoded point_time if no start_pos available
                if start_pos != []:
                        print "start pose is not defined"
                        point_time = self.calculate_point_time(component_name, start_pos, point, default_vel, default_acc)
                else:
                        print "********Hello*******"
                        point_time = 8*point_nr

                start_pos = point
                point_msg.time_from_start=rospy.Duration(point_time + traj_time)
                traj_time += point_time
                traj_msg.points.append(point_msg)
        return (traj_msg, 0)
Пример #49
0
def opendoor(req):
    # main(whole_body,  gripper,wrist_wrench)
    frame = req.handle_pose.header.frame_id
    hanlde_pos = req.handle_pose.pose
    # hanlde_pos=geometry_msgs.msg.PoseStamped()
    res = OpendoorResponse()

    cli = actionlib.SimpleActionClient(
        '/hsrb/omni_base_controller/follow_joint_trajectory',
        control_msgs.msg.FollowJointTrajectoryAction)
    vel_pub = rospy.Publisher('/hsrb/command_velocity',
                              geometry_msgs.msg.Twist,
                              queue_size=10)
    armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
                             JointTrajectory,
                             queue_size=1)

    robot = hsrb_interface.Robot()
    whole_body = robot.get('whole_body')
    gripper = robot.get('gripper')
    wrist_wrench = robot.get('wrist_wrench')
    base = robot.get('omni_base')
    start_theta = base.pose[2]
    # with hsrb_interface.Robot() as robot:
    # whole_body = robot.get('whole_body')
    # gripper = robot.get('gripper')
    # wrist_wrench = robot.get('wrist_wrench')

    try:
        # Open the gripper
        whole_body.move_to_neutral()
        grasp_point_client()
        global recog_pos
        global is_found
        print recog_pos.pose.position

        print("Opening the gripper")
        whole_body.move_to_neutral()
        rospy.sleep(2.5)
        switch = ImpedanceControlSwitch()
        gripper.command(1.0)

        # Approach to the door
        print("Approach to the door")
        grab_pose = geometry.multiply_tuples(
            geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_X,
                          y=recog_pos.pose.position.y - HANDLE_TO_HAND_POS_Y,
                          z=recog_pos.pose.position.z,
                          ej=math.pi / 2), geometry.pose(ek=math.pi / 2))

        whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
        # Close the gripper
        wrist_wrench.reset()
        switch.activate("grasping")
        gripper.grasp(-0.01)
        rospy.sleep(1.0)
        switch.inactivate()
        # Rotate the handle
        whole_body.impedance_config = 'grasping'
        traj = JointTrajectory()

        # This controller requires that all joints have values
        traj.joint_names = [
            "arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
            "wrist_flex_joint", "wrist_roll_joint"
        ]
        p = JointTrajectoryPoint()
        current_positions = [
            latest_positions[name] for name in traj.joint_names
        ]
        current_positions[0] = latest_positions["arm_lift_joint"] - 0.0675
        current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
        current_positions[2] = latest_positions["arm_roll_joint"]
        current_positions[3] = latest_positions["wrist_flex_joint"]
        current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
        p.positions = current_positions
        p.velocities = [0, 0, 0, 0, 0]
        p.time_from_start = rospy.Time(3)
        traj.points = [p]

        armPub.publish(traj)
        rospy.sleep(3.0)
        print("finishing rotating handle")
        ## Move by End-effector line
        whole_body.impedance_config = 'compliance_hard'
        whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)

        print("push the door")
        ## Move base with linear & Angular motion
        tw = geometry_msgs.msg.Twist()
        tw.linear.x = 0.9
        tw.angular.z = 0.45
        vel_pub.publish(tw)
        rospy.sleep(4.0)

        ## Move base with linear & Angular motion second
        tw_cmd0 = geometry_msgs.msg.Twist()
        tw_cmd0.linear.x = 0.5
        tw_cmd0.angular.z = 0.45
        vel_pub.publish(tw_cmd0)
        rospy.sleep(2.0)

        tw_cmd1 = geometry_msgs.msg.Twist()
        tw_cmd1.linear.x = 0.6
        tw_cmd1.linear.y = 0.2
        tw_cmd1.angular.z = 0.25
        vel_pub.publish(tw_cmd1)
        # rospy.sleep(4.0)
        rospy.sleep(3.0)

        tw_cmd2 = geometry_msgs.msg.Twist()
        tw_cmd2.linear.x = 0.65
        tw_cmd2.linear.y = 0.5
        tw_cmd2.angular.z = 0.35
        vel_pub.publish(tw_cmd2)
        # rospy.sleep(4.0)
        rospy.sleep(2.0)

        ## Open the gripper
        gripper.command(1.0)

        ## Move back
        base.go_rel(-1.3, 0.0, start_theta)

        gripper.command(1.0)
        whole_body.move_to_neutral()
        res.success = True

    except Exception as e:
        rospy.logerr(e)
        print "Failed to open door"
        res.success = False
    return res
            control_pan_pos_2 = makeSimpleProfile(control_pan_pos_2,
                                                  target_pan_pos_2,
                                                  (POS_STEP_SIZE / 2.0))
            control_tilt_pos_2 = makeSimpleProfile(control_tilt_pos_2,
                                                   target_tilt_pos_2,
                                                   (POS_STEP_SIZE / 2.0))

            head_msg.header.stamp = rospy.Time.now()
            # jtp_msg.points.positions = [control_pan_pos,control_tilt_pos,control_yaw_pos]

            jtp_msg.positions = [
                control_pan_pos, control_tilt_pos, control_pan_pos_2,
                control_tilt_pos_2
            ]
            jtp_msg.velocities = [0.5, 0.5, 0.5, 0.5]
            jtp_msg.time_from_start = rospy.Duration.from_sec(0.00000002)

            head_msg.points.append(jtp_msg)

            pub.publish(twist)
            head_pub.publish(head_msg)

    except:
        print e

    finally:
        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
Пример #51
0
    def __init__(self):
        # First set up service
        request_trajectory_service = rospy.ServiceProxy(
            "generate_toppra_trajectory", GenerateTrajectory)

        # This is an example for UAV and output trajectory is converted
        # accordingly
        trajectory_pub = rospy.Publisher('multi_dof_trajectory',
                                         MultiDOFJointTrajectory,
                                         queue_size=1)
        time.sleep(0.5)

        # Example of a simple square trajectory for quadcopter. All vectors
        # must be of same length
        x = [0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0]
        y = [0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0]
        z = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
        yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        # Another example. Same square defined through more points. This will
        # overwrite the first example
        x = [0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0, 0.0, 0.0]
        y = [0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0]
        z = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
        yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0]

        # Create a service request which will be filled with waypoints
        request = GenerateTrajectoryRequest()

        # Add waypoints in request
        waypoint = JointTrajectoryPoint()
        for i in range(0, len(x)):
            # Positions are defined above
            waypoint.positions = [x[i], y[i], z[i], yaw[i]]
            # Also add constraints for velocity and acceleration. These
            # constraints are added only on the first waypoint since the
            # TOPP-RA reads them only from there.
            if i == 0:
                waypoint.velocities = [2, 2, 2, 1]
                waypoint.accelerations = [1.25, 1.25, 1.25, 1]

            # Append all waypoints in request
            request.waypoints.points.append(copy.deepcopy(waypoint))

        # Set up joint names. This step is not necessary
        request.waypoints.joint_names = ["x", "y", "z", "yaw"]
        # Set up sampling frequency of output trajectory.
        request.sampling_frequency = 100.0
        # Set up number of gridpoints. The more gridpoints there are,
        # trajectory interpolation will be more accurate but slower.
        # Defaults to 100
        request.n_gridpoints = 500
        # If you want to plot Maximum Velocity Curve and accelerations you can
        # send True in this field. This is intended to be used only when you
        # have to debug something since it will block the service until plot
        # is closed.
        request.plot = False
        # Request the trajectory
        response = request_trajectory_service(request)

        # Response will have trajectory and bool variable success. If for some
        # reason the trajectory was not able to be planned or the configuration
        # was incomplete or wrong it will return False.

        print("Converting trajectory to multi dof")
        joint_trajectory = response.trajectory
        multi_dof_trajectory = self.JointTrajectory2MultiDofTrajectory(
            joint_trajectory)
        trajectory_pub.publish(multi_dof_trajectory)
 def add_point(self, positions, time):
     point = JointTrajectoryPoint()
     point.positions = copy(positions)
     point.velocities = [0.0] * len(self._goal.trajectory.joint_names)
     point.time_from_start = rospy.Duration(time)
     self._goal.trajectory.points.append(point)
Пример #53
0
 def add_point(self, positions, velocities, time):
     point = JointTrajectoryPoint()
     point.positions = copy(positions)
     point.velocities = copy(velocities)
     point.time_from_start = time
     self._goal.trajectory.points.append(point)
Пример #54
0
    omega = np.pi / time

    d_x = np.zeros(3)
    d_x[0] = -omega * r * np.sin(theta)
    d_x[1] = 0
    d_x[2] = omega * r * np.cos(theta)

    # Computing devirative of velocity
    dd_x = np.zeros(3)
    dd_x[0] = -(omega**2) * r * np.cos(theta)
    dd_x[1] = 0
    dd_x[2] = -(omega**2) * r * np.sin(theta)

    msg = JointTrajectoryPoint()
    msg.positions = [x[0], x[1], x[2]]
    msg.velocities = [d_x[0], d_x[1], d_x[2]]
    msg.accelerations = [dd_x[0], dd_x[1], dd_x[2]]

    pub.publish(msg)

    rospy.sleep(Ts)

# Last trajectory point means the system is stable, i.e. no velocity nor acceleration
x = np.zeros(3)
x[0] = x_e[0] - 2 * r
x[1] = 0
x[2] = x_e[2]

d_x = np.zeros(3)
dd_x = np.zeros(3)
Пример #55
0
# Hand gesture parameters do not need an array and are defined as:            #
# [thumb, pointer, middle, ring, pinky]                                       #
#                                                                             #
# Arm gesture parameters only include shoulder_medial_joint and elbow_joint.  #
# All others in the array should be set as 0.                                 #
# =========================================================================== #
pointer = [1, 0, 1, 1, 1]
peace = [1, 0, 0, 1, 1]
thumbs_up = [0, 1, 1, 1, 1]
fist = [1, 1, 1, 1, 1]
open_hand = [0, 0, 0, 0, 0]

pointer_gest = JointTrajectory()
pointer_point = JointTrajectoryPoint()
pointer_point.positions = pointer
pointer_point.velocities = [0] * 5
pointer_gest.points = [pointer_point]

hand_gesture1 = JointTrajectory()
hand_gesture1.points = [JointTrajectoryPoint(positions=pointer)]
hand_gesture2 = JointTrajectory()
hand_gesture2.points = [JointTrajectoryPoint(positions=peace)]
hand_gesture3 = JointTrajectory()
hand_gesture3.points = [JointTrajectoryPoint(positions=thumbs_up)]
hand_gesture4 = JointTrajectory()
hand_gesture4.points = [JointTrajectoryPoint(positions=fist)]
hand_gesture5 = JointTrajectory()
hand_gesture5.points = [JointTrajectoryPoint(positions=open_hand)]

gesture_state = String()
hand_gesture = JointTrajectory()
Пример #56
0
def createTrajectoryPoint(time, positions):
    point = JointTrajectoryPoint()
    point.time_from_start = rospy.Duration(time)
    point.positions = positions
    point.velocities = ZERO_VECTOR
    return point
imutime = 0  #for record
initialTheta = 0  #laser_scan initial theta radient
initialThetalist = []
statusJP = JointTrajectoryPoint()
statusJP_time = 0
pose_time = 0
ready = 0
enginevalue = 1500
steeringvalue = 1500

started = True
MAX_engine = 1620

#initialize goal
goal.positions = [1, 1, 0]
goal.velocities = [0, 0, 0]
goal.accelerations = [0, 0, 0]
goal.effort = [1]

#initialize statusJP
statusJP.positions = [0, 0, 0]
statusJP.velocities = [0, 0, 0]
statusJP.accelerations = [0, 0, 0]
statusJP.effort = [1]


def mpcControl(initial_state, cx, cy, cyaw, cv, ca, cj):
    """
    :param x1:
    :param y1:
    :param yaw1:
def main(whole_body, base, gripper, wrist_wrench):

    cli = actionlib.SimpleActionClient(
        '/hsrb/omni_base_controller/follow_joint_trajectory',
        control_msgs.msg.FollowJointTrajectoryAction)
    # publisher for delvering command for base move
    vel_pub = rospy.Publisher('/hsrb/command_velocity',
                              geometry_msgs.msg.Twist,
                              queue_size=10)
    # base_pub = rospy.Publisher('/move_base/move/goal',move_base_msgs.msg.MoveBaseActionGoal,queue_size=10)

    armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command',
                             JointTrajectory,
                             queue_size=1)
    ## Grab the handle of door

    #test with service - get the handle position from handle
    grasp_point_client()

    global recog_pos
    global Is_found

    print recog_pos.pose.position
    # target_pose_Msg = rospy.wait_for_message("/handle_detector/grasp_point", PoseStamped)
    # recog_pos.pose.position.x=target_pose_Msg.pose.position.x
    # recog_pos.pose.position.y=target_pose_Msg.pose.position.y
    # recog_pos.pose.position.z=target_pose_Msg.pose.position.z

    whole_body.move_to_neutral()
    # whole_body.impedance_config= 'grasping'
    switch = ImpedanceControlSwitch()
    # wrist_wrench.reset()
    gripper.command(1.0)

    grab_pose = geometry.multiply_tuples(
        geometry.pose(x=recog_pos.pose.position.x - HANDLE_TO_HAND_POS_,
                      y=recog_pos.pose.position.y - 0.012,
                      z=recog_pos.pose.position.z,
                      ej=math.pi / 2), geometry.pose(ek=math.pi / 2))
    whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
    wrist_wrench.reset()
    # whole_body.impedance_config= 'compliance_middle'
    switch.activate("grasping")
    # gripper.command(0.01)
    gripper.grasp(-0.01)
    rospy.sleep(1.0)
    switch.inactivate()

    wrist_wrench.reset()
    rospy.sleep(8.0)

    #### test manipulation
    whole_body.impedance_config = 'grasping'
    # print(whole_body.impedance_config)
    # desired_rot=-1.95
    # whole_body.move_to_joint_positions({"wrist_roll_joint":desired_rot})
    traj = JointTrajectory()
    # This controller requires that all joints have values
    traj.joint_names = [
        "arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
        "wrist_flex_joint", "wrist_roll_joint"
    ]
    p = JointTrajectoryPoint()
    current_positions = [latest_positions[name] for name in traj.joint_names]
    current_positions[0] = latest_positions["arm_lift_joint"] - 0.07
    current_positions[1] = latest_positions["arm_flex_joint"] - 0.02
    current_positions[2] = latest_positions["arm_roll_joint"]
    current_positions[3] = latest_positions["wrist_flex_joint"]
    current_positions[4] = latest_positions["wrist_roll_joint"] - 0.65
    p.positions = current_positions
    p.velocities = [0, 0, 0, 0, 0]
    p.time_from_start = rospy.Time(3)
    traj.points = [p]

    armPub.publish(traj)

    rospy.sleep(3.0)
    # goal_pose =PoseStamped()
    # goal_pose.pose.position.x=recog_pos.pose.position.x+0.4
    # goal_pose.pose.position.y=recog_pos.pose.position.y+0.2
    # goal_pose.pose.position.z=recog_pos.pose.position.z
    # goal_pose.pose.orientation.x=recog_pos.pose.orientation.x
    # goal_pose.pose.orientation.y=recog_pos.pose.orientation.y
    # goal_pose.pose.orientation.z=recog_pos.pose.orientation.z
    # goal_pose.pose.orientation.w=recog_pos.pose.orientation.w

    # yaw=math.pi/6
    # quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
    # #type(pose) = geometry_msgs.msg.Pose
    # nav_goal = move_base_msgs.msg.MoveBaseActionGoal()
    # nav_goal.header.stamp=rospy.Time(0)
    # nav_goal.goal.target_pose.header.frame_id = "map"
    # nav_goal.goal.target_pose.pose.position.x=recog_pos.pose.position.x+0.3
    # nav_goal.goal.target_pose.pose.position.y=recog_pos.pose.position.y+0.2
    # nav_goal.goal.target_pose.pose.position.z=0.0
    # nav_goal.goal.target_pose.pose.orientation.x=quaternion[0]
    # nav_goal.goal.target_pose.pose.orientation.y=quaternion[1]
    # nav_goal.goal.target_pose.pose.orientation.z=quaternion[2]
    # nav_goal.goal.target_pose.pose.orientation.w=quaternion[3]
    # base_pub.publish(nav_goal)

    # # whole_body.end_effector_frame = u'odom'
    # # whole_body.move_end_effector_by_line((0, 0, 1), -0.2)
    # # publish_arm(latest_positions["arm_lift_joint"],latest_positions["arm_flex_joint"],latest_positions["arm_roll_joint"], latest_positions["wrist_flex_joint"],wrist_roll)
    whole_body.impedance_config = 'compliance_hard'
    whole_body.move_end_effector_by_line((0.0, 0.0, 1), 0.45)
    # # whole_body.move_end_effector_by_line((0.7071, 0.7071,0), 0.5)
    # # whole_body.move_end_effector_by_line((0, -0.7071, 0.7071), 0.5)
    # whole_body.impedance_config= None

    tw = geometry_msgs.msg.Twist()
    tw.linear.x = 0.9
    tw.angular.z = 0.45
    vel_pub.publish(tw)
    rospy.sleep(4.0)

    tw_cmd0 = geometry_msgs.msg.Twist()
    tw_cmd0.linear.x = 0.3
    tw_cmd0.angular.z = 0.45
    vel_pub.publish(tw_cmd0)
    # rospy.sleep(4.0)

    rospy.sleep(4.0)
    gripper.command(1.0)
    # whole_body.move_end_effector_by_line((0, 0, -1), 0.25)

    tw_cmd = geometry_msgs.msg.Twist()
    tw_cmd.linear.x = -1.2
    vel_pub.publish(tw_cmd)
    rospy.sleep(2.0)

    tw_cmd2 = geometry_msgs.msg.Twist()
    tw_cmd2.linear.x = -1.1
    tw_cmd2.angular.z = -0.4
    vel_pub.publish(tw_cmd2)
    rospy.sleep(4.0)
    whole_body.move_to_neutral()
    tw_cmd2 = geometry_msgs.msg.Twist()
    tw_cmd2.linear.x = -0.6
    tw_cmd2.angular.z = -0.3
    vel_pub.publish(tw_cmd2)
Пример #59
0
# DEFINITIONS      #
# --------------------------------------------------------------------------- #
# Arm gesture parameters only include shoulder_medial_joint and elbow_joint.  #
# All others in the array should be set empty.                                #
# 2 is the max value for elbow_joint, while -2 is the min                     #
# 3.2 indicates a 180 degree rotation for shoulder_medial_joint               #
# =========================================================================== #

arm_names = ['shoulder_medial_joint', 'elbow_joint']

# Blank gesture that is set depending on the desired position in the main loop
arm_gesture = JointTrajectory()
arm_gesture.joint_names = arm_names
arm_gesture_points = JointTrajectoryPoint()
arm_gesture_points.positions = []
arm_gesture_points.velocities = []
arm_gesture_points.accelerations = []
arm_gesture_points.effort = []
arm_gesture_points.time_from_start = rospy.Duration(3)
arm_gesture.points = [arm_gesture_points]

position = String()

# point locations
# test = [-1.37, -1.54]
test = [-1.6, -1.3]
default = [0, 0]
max_left = [0, 2]
max_right = [3.2, 2]
middle = [1.6, 1]
Пример #60
0
def get_trajectory_point(positions, time_from_start):
    point = JointTrajectoryPoint()
    point.positions = positions
    point.velocities = [.0] * len(positions)
    point.time_from_start = time_from_start
    return point