Advertisement
Guest User

Constrained move function

a guest
Mar 17th, 2021
308
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 2.25 KB | None | 0 0
  1. def move_ee_pose(arm_group):
  2.     """
  3.     Move end-effector to specified pose
  4.     """
  5.     current_pose = arm_group.get_current_pose(end_effector_link="tool0")
  6.     print "current_pose:", current_pose
  7.     # pose home
  8.   # position:
  9.     # x: 0.422562465178
  10.     # y: 0.191221247515
  11.     # z: 0.418782506626
  12.   # orientation:
  13.     # x: -0.000369305197861
  14.     # y: 0.707106486605
  15.     # z: 0.707106290394
  16.     # w: 0.000987066022963
  17.    
  18.   # pose home + joint_goal[4] = pi/2
  19.   # position:
  20.     # x: 0.504844264658
  21.     # y: 0.108320076232
  22.     # z: 0.418621428128
  23.   # orientation:
  24.     # x: 0.500459099487
  25.     # y: 0.498982558429
  26.     # z: 0.49980940138
  27.     # w: 0.500747100257
  28.     joint_values = arm_group.get_current_joint_values()
  29.     pose_goal.position.x = 0.504
  30.     pose_goal.position.y = 0.108
  31.     pose_goal.position.z = 0.418
  32.     arm_group.set_position_target([pose_goal.position.x, pose_goal.position.y, pose_goal.position.z])
  33.     # arm_group.set_pose_target(pose_goal)
  34.     raw_input("Set constraints?")
  35.     print "Const-0:", arm_group.get_known_constraints()
  36.     joint_names = arm_group.get_active_joints()
  37.     print "joint_names:", joint_names
  38.     for i in range(len(joint_names)):
  39.         if i<3:
  40.             joint_constraint.joint_name = joint_names[i]
  41.             joint_constraint.position = joint_values[i]
  42.             joint_constraint.weight = 10.0 # Closer to zero means less important
  43.             goal_constraint.joint_constraints.append(joint_constraint)
  44.         elif i>=3:
  45.             joint_constraint.weight = 0.0 # Closer to zero means less important
  46.         else:
  47.             print "Sth is wrong"
  48.         print "i:", i
  49.    
  50.     print "Const-1:", arm_group.get_known_constraints()
  51.     moveit_goal.request.goal_constraints.append(goal_constraint)
  52.     print "Const-2:", arm_group.get_known_constraints()
  53.     moveit_goal.request.goal_constraints.append(goal_constraint)
  54.     moveit_goal.request.num_planning_attempts = 1
  55.     moveit_goal.request.allowed_planning_time = 5.0
  56.     moveit_goal.planning_options.plan_only = False
  57.     moveit_goal.planning_options.planning_scene_diff.is_diff = True
  58.     moveit_goal.request.group_name = arm_group
  59.    
  60.     pose_goal.motion_plan_request.goal_constraints.joint_constraints.append(goal_constraint)
  61.     print "Const-3:", arm_group.get_known_constraints()
  62.     arm_group.go(pose_goal, wait=True)
  63.     print "Final joint values:", arm_group.get_current_joint_values()
  64.     raw_input("Done")
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement