def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05#rospy.get_param(padd_name)
joint_names = ['joint1',
'joint2',
'joint3',
'joint4',
'joint5',
'joint6']
motion_plan_request = MotionPlanRequest()
motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
motion_plan_request.group_name = "irp6p"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
motion_plan_request.goal_constraints.joint_constraints[0].position = -0.0
motion_plan_request.goal_constraints.joint_constraints[1].position = -1.57
motion_plan_request.goal_constraints.joint_constraints[3].position = 0.0
motion_plan_request.goal_constraints.joint_constraints[4].position = 4.71
motion_plan_request.goal_constraints.joint_constraints[5].position = -2.8 #1.57
goal = MoveArmGoal()
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request = motion_plan_request
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05#rospy.get_param(padd_name)
joint_names = ['right_arm_0_joint',
'right_arm_1_joint',
'right_arm_2_joint',
'right_arm_3_joint',
'right_arm_4_joint',
'right_arm_5_joint',
'right_arm_6_joint']
motion_plan_request = MotionPlanRequest()
motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
motion_plan_request.group_name = "right_arm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
motion_plan_request.goal_constraints.joint_constraints[0].position = 1.5
motion_plan_request.goal_constraints.joint_constraints[1].position = -1.5
motion_plan_request.goal_constraints.joint_constraints[5].position = 0.0
req = GetMotionPlanRequest()
req.motion_plan_request = motion_plan_request
res = self.motion_planning_service.call(req)
path = DisplayTrajectory()
path.model_id = "right_arm"
path.trajectory.joint_trajectory = res.trajectory.joint_trajectory
self.traj_pub.publish(path)
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05 #rospy.get_param(padd_name)
joint_names = [
'right_arm_0_joint', 'right_arm_1_joint', 'right_arm_2_joint',
'right_arm_3_joint', 'right_arm_4_joint', 'right_arm_5_joint',
'right_arm_6_joint'
]
motion_plan_request = MotionPlanRequest()
motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
motion_plan_request.group_name = "right_arm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
motion_plan_request.goal_constraints.joint_constraints[
0].position = 1.5
motion_plan_request.goal_constraints.joint_constraints[
1].position = -1.5
motion_plan_request.goal_constraints.joint_constraints[
5].position = 0.0
req = GetMotionPlanRequest()
req.motion_plan_request = motion_plan_request
res = self.motion_planning_service.call(req)
path = DisplayTrajectory()
path.model_id = "right_arm"
path.trajectory.joint_trajectory = res.trajectory.joint_trajectory
self.traj_pub.publish(path)
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05 #rospy.get_param(padd_name)
joint_names = [
'right_arm_0_joint', 'right_arm_1_joint', 'right_arm_2_joint',
'right_arm_3_joint', 'right_arm_4_joint', 'right_arm_5_joint',
'right_arm_6_joint'
]
motion_plan_request = MotionPlanRequest()
motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
motion_plan_request.group_name = "right_arm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
motion_plan_request.goal_constraints.joint_constraints[
0].position = 1.5
motion_plan_request.goal_constraints.joint_constraints[
1].position = -1.8
motion_plan_request.goal_constraints.joint_constraints[3].position = .5
goal = MoveArmGoal()
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request = motion_plan_request
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.0
motion_plan_request.goal_constraints.joint_constraints[
1].position = 0.0
motion_plan_request.goal_constraints.joint_constraints[
3].position = 0.0
goal = MoveArmGoal()
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request = motion_plan_request
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05 #rospy.get_param(padd_name)
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "right_arm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.position_constraints = [
PositionConstraint() for _ in range(1)
]
motion_plan_request.goal_constraints.position_constraints[
0].header.stamp = rospy.Time.now()
motion_plan_request.goal_constraints.position_constraints[
0].header.frame_id = "base_link"
motion_plan_request.goal_constraints.position_constraints[
0].link_name = "right_arm_hand_link"
motion_plan_request.goal_constraints.position_constraints[
0].position.x = .65
motion_plan_request.goal_constraints.position_constraints[
0].position.y = -0.55
motion_plan_request.goal_constraints.position_constraints[
0].position.z = .9
motion_plan_request.goal_constraints.position_constraints[
0].constraint_region_shape.type = Shape.BOX
motion_plan_request.goal_constraints.position_constraints[
0].constraint_region_shape.dimensions = [
float(.02) for _ in range(3)
]
motion_plan_request.goal_constraints.position_constraints[
0].constraint_region_orientation.w = 1.0
motion_plan_request.goal_constraints.position_constraints[
0].weight = 1.0
motion_plan_request.goal_constraints.orientation_constraints = [
OrientationConstraint() for _ in range(1)
]
motion_plan_request.goal_constraints.orientation_constraints[
0].header.stamp = rospy.Time.now()
motion_plan_request.goal_constraints.orientation_constraints[
0].header.frame_id = "base_link"
motion_plan_request.goal_constraints.orientation_constraints[
0].link_name = "right_arm_hand_link"
vals = [float() for _ in range(4)]
vals = [-.895249, -.187638, 0.092325, .393443]
mag = math.sqrt(vals[0] * vals[0] + vals[1] * vals[1] +
vals[2] * vals[2] + vals[3] * vals[3])
vals[0] /= mag
vals[1] /= mag
vals[2] /= mag
vals[3] /= mag
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.x = vals[0]
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.y = vals[1]
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.z = vals[2]
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.w = vals[3]
motion_plan_request.goal_constraints.orientation_constraints[
0].absolute_roll_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[
0].absolute_pitch_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[
0].absolute_yaw_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[
0].weight = 1.0
goal = MoveArmGoal()
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request = motion_plan_request
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05#rospy.get_param(padd_name)
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "right_arm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.position_constraints = [PositionConstraint() for _ in range(1)]
motion_plan_request.goal_constraints.position_constraints[0].header.stamp = rospy.Time.now()
motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link"
motion_plan_request.goal_constraints.position_constraints[0].link_name = "right_arm_hand_link"
motion_plan_request.goal_constraints.position_constraints[0].position.x = .65
motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.55
motion_plan_request.goal_constraints.position_constraints[0].position.z = .9
motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = Shape.BOX
motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions = [float(.02) for _ in range(3)]
motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0
motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0
motion_plan_request.goal_constraints.orientation_constraints = [OrientationConstraint() for _ in range(1)]
motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = rospy.Time.now()
motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link"
motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "right_arm_hand_link"
vals = [float() for _ in range(4)]
vals = [-.895249, -.187638, 0.092325, .393443]
mag = math.sqrt(vals[0]*vals[0]+vals[1]*vals[1]+vals[2]*vals[2]+vals[3]*vals[3])
vals[0] /= mag
vals[1] /= mag
vals[2] /= mag
vals[3] /= mag
motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = vals[0]
motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = vals[1]
motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = vals[2]
motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = vals[3]
motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0
goal = MoveArmGoal()
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request = motion_plan_request
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
