Python MoveGroupCommander.get_current_joint_values方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_joint_values [as 别名]

#.........这里部分代码省略.........
        
        self.tmp_pose_left.pose.orientation = self.last_hydra_message.paddles[0].transform.rotation
        if self.last_hydra_message.paddles[1].buttons[0] == True:
            self.pub_right_hand_pose.publish(self.tmp_pose_right)
        if self.last_hydra_message.paddles[0].buttons[0] == True:
            self.pub_left_hand_pose.publish(self.tmp_pose_left)
            
        self.pub_right_hand_pose_reference.publish(self.tmp_pose_right)
        self.pub_left_hand_pose_reference.publish(self.tmp_pose_left)
        self.read_message = False

    def run(self):
        rospy.loginfo("Press LB / RB to send the current pose")
        
        while self.last_hydra_message == None:
            rospy.sleep(0.1)
            
        rospy.loginfo("Got the first data of the razer... Now we can do stuff")
        
        sleep_rate=0.05 # check at 20Hz
        counter = 0
        while True:
            counter += 1
            rospy.loginfo("Loop #" + str(counter))
            if not self.read_message:
                self.read_message = True
                if self.last_hydra_message.paddles[1].buttons[0] == True:
                    # send curr left paddle pos to move_group right
                    rospy.loginfo("sending curr right hand")
                    self.right_arm_mgc.set_pose_target(self.tmp_pose_right)
                    self.right_arm_mgc.go(wait=False)
                    
                if self.last_hydra_message.paddles[0].buttons[0] == True:
                    # send curr right paddle pos to move_group left
                    rospy.loginfo("sending curr left hand")
                    self.left_arm_mgc.set_pose_target(self.tmp_pose_left)
                    self.left_arm_mgc.go(wait=False)
                    
                if self.last_hydra_message.paddles[1].trigger > 0.0:
                    # send goal right hand close proportional to trigger value (2.0 max?)
                    rospy.loginfo("Closing right hand to value: " + str(self.last_hydra_message.paddles[1].trigger * 2.0))
                    right_hand_goal = self.create_hand_goal(hand_side="right", hand_pose="intermediate", values=self.last_hydra_message.paddles[1].trigger * 2.0)
                    self.right_hand_as.send_goal(right_hand_goal)
                    
                if self.last_hydra_message.paddles[0].trigger > 0.0:
                    # send goal left hand close proportional to trigger value (2.0 max?)
                    rospy.loginfo("Closing left hand to value: " + str(self.last_hydra_message.paddles[0].trigger * 2.0))
                    left_hand_goal = self.create_hand_goal(hand_side="left", hand_pose="intermediate", values=self.last_hydra_message.paddles[0].trigger * 2.0)
                    self.left_hand_as.send_goal(left_hand_goal)
                    
                if self.last_hydra_message.paddles[1].joy[0] != 0.0:
                    # send torso rotation left(neg)/right (pos)
                    rospy.loginfo("Rotation torso")
                    curr_joint_val = self.torso_mgc.get_current_joint_values()
                    self.torso_mgc.set_joint_value_target("torso_1_joint", curr_joint_val[0] + (self.last_hydra_message.paddles[1].joy[0] * 0.1 * -1))
                    self.torso_mgc.go(wait=True)
                    rospy.loginfo("Rotation torso sent!")
                    
                if self.last_hydra_message.paddles[1].joy[1] != 0.0:
                    # send torso inclination front(pos)/back(neg)
                    rospy.loginfo("Inclination torso")
                    curr_joint_val = self.torso_mgc.get_current_joint_values()
                    self.torso_mgc.set_joint_value_target("torso_2_joint", curr_joint_val[1] + (self.last_hydra_message.paddles[1].joy[1] * 0.1))
                    self.torso_mgc.go(wait=True)
                    rospy.loginfo("Inclination torso sent!")
                    
                if self.last_hydra_message.paddles[0].joy[0] != 0.0 or self.last_hydra_message.paddles[0].joy[1] != 0.0:
                    twist_goal = Twist()
                    twist_goal.linear.x = 1.0 * self.last_hydra_message.paddles[0].joy[1]
                    twist_goal.angular.z = 1.0 * self.last_hydra_message.paddles[0].joy[0] * -1.0
                    self.pub_move_base.publish(twist_goal)
                    # move base rotate left (neg)/ right(pos)
                    rospy.loginfo("Move base")
                            
                    
                if self.last_hydra_message.paddles[1].buttons[3] == True:
                    # thumb up
                    rospy.loginfo("Right thumb up")
                    right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="open")
                    self.right_hand_as.send_goal(right_thumb_up)
                  
                if self.last_hydra_message.paddles[0].buttons[3] == True:
                    # thumb up
                    rospy.loginfo("Left thumb up") 
                    left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="open")
                    self.left_hand_as.send_goal(left_thumb_up)
                
                if self.last_hydra_message.paddles[1].buttons[1] == True:
                    # thumb down
                    rospy.loginfo("Right thumb down")
                    right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="closed")
                    self.right_hand_as.send_goal(right_thumb_up)
     
                if self.last_hydra_message.paddles[0].buttons[1] == True:
                    # thumb down
                    rospy.loginfo("Left thumb down")
                    left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="closed")
                    self.left_hand_as.send_goal(left_thumb_up)            
                
            rospy.sleep(sleep_rate)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_joint_values [as 别名]
    # reach
    rospy.loginfo("reach")
    arm.set_pose_target([0.90, 0.16, 0.255, 0, 0, 0])
    arm.plan() and arm.go()
    arm.plan() and arm.go()
    # approach
    rospy.loginfo("approach")
    arm.set_pose_target([1.13, 0.16, 0.255, 0, 0, 0])
    arm.plan() and arm.go()
    # rotate
    for i in range(4):
        # close
        rospy.loginfo("close")
        close_hand()
        # rotate
        angles = arm.get_current_joint_values()
        import numpy
        start_angle = angles[5]
        print("current angles=", start_angle)
        for r in numpy.arange(start_angle, start_angle-3.14*2, -1.0):
            rospy.loginfo(angles)
            angles[5] = r
            rospy.loginfo("rotate (%f)" % (r))
            msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
            msg.trajectory.points = [JointTrajectoryPoint(positions=angles, time_from_start=rospy.Duration(1))]
            client.send_goal(msg)
            client.wait_for_result()

        # open
        rospy.loginfo("open")
        open_hand()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_joint_values [as 别名]
import moveit_msgs.msg

from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown

from math import sin, copysign, sqrt, pi
   
if __name__ == '__main__':
    print "============ Dynamic hand gestures"
    roscpp_initialize(sys.argv)
    rospy.init_node('pumpkin_planning', anonymous=True)

    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)

    group_variable_values = right_arm.get_current_joint_values()
    print "============ Joint values: ", group_variable_values

    group_variable_values[0] = -0.357569385437786
    group_variable_values[1] = -0.6320268016619928
    group_variable_values[2] = 0.24177580736353846
    group_variable_values[3] = 1.586101553004471
    group_variable_values[4] = -0.5805943181752088
    group_variable_values[5] = -1.1821499952996368
    right_arm.set_joint_value_target(group_variable_values)

    right_arm.go(wait=True)

    print "============ Waiting while RVIZ displays Up..."

    group_variable_values[0] = 1.0