Python JointTrajectoryPoint.effort方法代码示例

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import effort [as 别名]
 def make_gripper_posture(self, pose):
     t = JointTrajectory()
     t.joint_names = GRIPPER_JOINT_NAMES
     tp = JointTrajectoryPoint()
     tp.positions = [pose for j in t.joint_names]
     tp.effort = GRIPPER_EFFORT
     t.points.append(tp)
     return t

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

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

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import effort [as 别名]
 def create_joint_trajectory(self, hand_joints, position, effort):
     jt = JointTrajectory()
     jt.joint_names = hand_joints
     #jt.joint_names.append('r_gripper_joint') ##
     jtp = JointTrajectoryPoint()
     jtp.positions = position
     #jtp.positions.append(0.08) ##
     jtp.effort = effort
     #jtp.effort.append(100.0) ##
     jt.points.append(jtp)
     return jt

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import effort [as 别名]
    def make_grab_posture(self, joint_positions):
        t = JointTrajectory()

        t.header.stamp = rospy.get_rostime()
        t.joint_names = self.hand_group.get_joints()

        tp= JointTrajectoryPoint()

        tp.positions = joint_positions
        tp.effort = [1.0]
        tp.time_from_start = rospy.Duration(0.0)

        t.points.append(tp)
        return t

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import effort [as 别名]
 def make_gripper_posture(self, joint_positions):
     # Initialize the joint trajectory for the gripper joints
     t = JointTrajectory()
     
     # Set the joint names to the gripper joint names
     t.joint_names = GRIPPER_JOINT_NAMES
     
     # Initialize a joint trajectory point to represent the goal
     tp = JointTrajectoryPoint()
     
     # Assign the trajectory joint positions to the input positions
     tp.positions = joint_positions
     
     # Set the gripper effort
     tp.effort = GRIPPER_EFFORT
     
     tp.time_from_start = rospy.Duration(1.0)
     
     # Append the goal point to the trajectory points
     t.points.append(tp)
     
     return t

# 需要导入模块: from trajectory_msgs.msg import JointTrajectoryPoint [as 别名]
# 或者: from trajectory_msgs.msg.JointTrajectoryPoint import effort [as 别名]
  def sendWaypointTrajectory(self, waypoints, durations = 0., vels = 0., accs = 0., effs = 0.):
    if not self.ang_cmd_wait(waypoints[0]):
      print 'Cannot go to the first point in the trajectory'
      return None
#    else: 
#      print 'Went to first'
      
    if not self.traj_connection:
      print 'Action server connection was not established'
      return None
    joint_traj = JointTrajectory()
    joint_traj.joint_names = self.arm_joint_names;
    
    if not durations == 0:
      if not len(durations) == waypoints:
        raise Exception('The number of duration points is not equal to the number of provided waypoints')
    if not vels == 0:
      if not len(vels) == waypoints:
        raise Exception('The number velocity points is not equal to the number of provided waypoints')
    if not accs == 0:
      if not len(accs) == waypoints:
        raise Exception('The number acceleration points is not equal to the number of provided waypoints')
    if not effs == 0:
      if not len(effs) == waypoints:
        raise Exception('The number effort points is not equal to the number of provided waypoints')

    if not effs == 0:    
      if not (vels == 0 and accs == 0):
        raise Exception('Cannot specify efforts with velocities and accelerations at the same time')
    if (not accs == 0) and vels == 0:
      raise Exception('Cannot specify accelerations without velocities')
   
    total_time_from_start = 0.5; 
    for t in range(0, len(waypoints)):
      point = JointTrajectoryPoint()
  
      waypoint = waypoints[t]   
      if not len(waypoint) == len(joint_traj.joint_names):
        raise Exception('The number of provided joint positions is not equal to the number of available joints for index: ' + str(t))
      point.positions = waypoint
    
      if not vels == 0.:
        velocity = vels[t]
        if not len(velocity) == len(joint_traj.joint_names):
          raise Exception('The number of provided joint velocities is not equal to the number of available joints for index: ' + str(t))
        point.velocities = velocity

      if not accs == 0.:
        acceleration = accs[t]
        if not len(acceleration) == len(joint_traj.joint_names):
          raise Exception('The number of provided joint accelerations is not equal to the number of available joints for index: ' + str(t))
        point.accelerations = accelerations
      
      if not effs == 0.:
        effort = effs[t]
        if not len(effort) == len(joint_traj.joint_names):
          raise Exception('The number of provided joint efforts is not equal to the number of available joints for index: ' + str(t))
        point.effort = effort 

      if not durations == 0.:
        point.duration = duration

      # Deal with increasing time for each trajectory point
      point.time_from_start = rospy.Duration(total_time_from_start)
      total_time_from_start = total_time_from_start + 1.0 

      # Set the points
      joint_traj.points.append(point)
     
    traj_goal = FollowJointTrajectoryGoal()
    traj_goal.trajectory = joint_traj

    self.smooth_joint_trajectory_client.send_goal(traj_goal)
    self.smooth_joint_trajectory_client.wait_for_result()
    return self.smooth_joint_trajectory_client.get_result()