Python JointTrajectoryPoint.positions方法代码示例

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
    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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
    def readInPoses(self):
        poses = rospy.get_param('~poses')
        rospy.loginfo("Found %d poses on the param server", len(poses))

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

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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
def get_grasp_posture(pre):
    pre_grasp_posture = JointTrajectory()
    pre_grasp_posture.header.frame_id = END_EFFECTOR
    pre_grasp_posture.header.stamp = rospy.Time.now()
    pre_grasp_posture.joint_names = [GRIPPER_JOINTS]
    pos = JointTrajectoryPoint()
    if pre:  # gripper open
        pos.positions = [0.0]
    else:  # gripper closed
        pos.positions = [0.04]
    pre_grasp_posture.points.append(pos)
    return pre_grasp_posture

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
    def send_cmd_msg(self):
        jtm = JointTrajectory()
        jtm.joint_names = self.joint_names
        jtp = JointTrajectoryPoint()

        # Check if we're dealing with one single float value
        if len(self.joint_names) > 1:
            jtp.positions = self.position
        else:
            jtp.positions = [self.position]*len(self.joint_names)
        jtp.time_from_start = rospy.Duration(1.0)
        jtm.points = [jtp]
        return jtm

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
        def move_torso(self, string_operation):
		jt = JointTrajectory()
		jt.joint_names = ['torso_lift_joint']
		jtp = JointTrajectoryPoint()
		if string_operation == "lift":
			jtp.positions = [0.34]
		elif string_operation == "lower":
			jtp.positions = [0.15]
		else:
			return

		jtp.time_from_start = rospy.Duration(2.5)
		jt.points.append(jtp)
		rospy.loginfo("Moving torso " + string_operation)
		self.torso_cmd.publish(jt)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
 def __init__(self, test="idle", output="screen", ):
    	if output == "file":
    	    try:
    	        os.mkdir(os.path.expanduser("~/cwru_battery_analyzer/"))
    	    except Exception:
    	    	pass
    	    dateTime = datetime.now().strftime("%Y-%m-%d_%H%M%S")
    	    self.file_handle = open(os.path.expanduser("~/cwru_battery_analyzer/"+dateTime+".csv"), "w")
         self.file_handle.write('Time,Battery Voltage,13.8v Rail Voltage,cRIO Voltage\n')
    	    self.file_output = True
     else:
         self.file_output = False
     
     if test == "idle":
     	self.behavior = self.idle
     elif test == "drivetrain":
         self.commandPublisher = rospy.Publisher("cmd_vel", Twist)
         self.twistMsg = Twist()
         self.twistMsg.angular.x = 0.0
         self.twistMsg.angular.y = 0.0
         self.twistMsg.angular.z = 0.0
         self.twistMsg.linear.x = 0.0
         self.twistMsg.linear.y = 0.0
         self.twistMsg.linear.z = 0.0
         self.twistIncrement = 0.1
         self.behavior = self.exerciseDT
         rospy.on_shutdown(self.stopDT)
     elif test == "arm":
         self.armPublisher = rospy.Publisher("command", JointTrajectory)
         self.gripperService = rospy.ServiceProxy('/abby_gripper/gripper', gripper)
         self.gripperStatus = True
         self.jointTrajectoryMsg = JointTrajectory()
         self.jointTrajectoryMsg.joint_names= ["joint1","joint2","joint3","joint4","joint5","joint6"]
         jointAngles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #initial arm position in radians
         jointRange = 2*.77777770 #radians
         jointNum = 0 
         for seq in range(0,10):
             jointAngles[jointNum] = jointAngles[jointNum] + (jointRange/11)
             jointTrajectoryPt = JointTrajectoryPoint()
             jointTrajectoryPt.positions = copy.deepcopy(jointAngles)
             self.jointTrajectoryMsg.points.append(jointTrajectoryPt)
         for seq in range(10,0):
             jointAngles[jointNum] = jointAngles[jointNum] + (jointRange/11)
             jointTrajectoryPt = JointTrajectoryPoint()
             jointTrajectoryPt.positions = copy.deepcopy(jointAngles)
             self.jointTrajectoryMsg.points.append(jointTrajectoryPt)
         self.behavior = self.exerciseArm
     self.batteryListener = rospy.Subscriber("power_state", PowerState, self.batteryCallback)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
def states_to_trajectory(states, time_step=0.1):
    """
    Converts a list of RobotState/JointState in Robot Trajectory
    :param robot_states: list of RobotState
    :param time_step: duration in seconds between two consecutive points
    :return: a Robot Trajectory
    """
    rt = RobotTrajectory()
    for state_idx, state in enumerate(states):
        if isinstance(state, RobotState):
            state = state.joint_state

        jtp = JointTrajectoryPoint()
        jtp.positions = state.position
        # jtp.velocities = state.velocity
        # Probably does not make sense to keep velocities and efforts here
        # jtp.effort = state.effort
        jtp.time_from_start = rospy.Duration(state_idx*time_step)
        rt.joint_trajectory.points.append(jtp)

    if len(states) > 0:
        if isinstance(states[0], RobotState):
            rt.joint_trajectory.joint_names = states[0].joint_state.name
        else:
            rt.joint_trajectory.joint_names = states[0].joint_names

    return rt

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
	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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
    def test_joint_angles(self):
        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
        larm.wait_for_server()

        try:
            self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
        except tf.Exception:
            self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
        goal = JointTrajectoryGoal()
        goal.trajectory.header.stamp = rospy.get_rostime()
        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
        goal.trajectory.joint_names.append("LARM_ELBOW")
        goal.trajectory.joint_names.append("LARM_WRIST_Y")
        goal.trajectory.joint_names.append("LARM_WRIST_P")

        point = JointTrajectoryPoint()
        point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ]
        point.time_from_start = rospy.Duration(5.0)
        goal.trajectory.points.append(point)
        larm.send_goal(goal)
        larm.wait_for_result()
        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
        self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
    def test_joint_trajectory_action(self):
        larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
        larm.wait_for_server()

        try:
            self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
        except tf.Exception:
            self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
        (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
        goal = JointTrajectoryGoal()
        goal.trajectory.header.stamp = rospy.get_rostime()
        goal.trajectory.joint_names.append("J1")
        goal.trajectory.joint_names.append("J2")
        goal.trajectory.joint_names.append("J3")
        goal.trajectory.joint_names.append("J4")
        goal.trajectory.joint_names.append("J5")
        goal.trajectory.joint_names.append("J6")
        goal.trajectory.joint_names.append("J7")

        point = JointTrajectoryPoint()
        point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
        point.time_from_start = rospy.Duration(5.0)
        goal.trajectory.points.append(point)
        larm.send_goal(goal)
        larm.wait_for_result()
        (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
        rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
        self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
    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)

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import positions [as 别名]
    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)