def plan_to_pose(self, target, orientation_constraints=None):
"""
Generates a plan given an end effector pose subject to orientation constraints
Inputs:
target: A geometry_msgs/PoseStamped message containing the end effector pose goal
orientation_constraints: A list of moveit_msgs/OrientationConstraint messages
Outputs:
path: A moveit_msgs/RobotTrajectory path
"""
print "!!!!!!!IN PLAN_TO_POSE!!!!!!"
self._group.set_pose_target(target)
print "!!!!!!!SECOND!!!!!!"
self._group.set_start_state_to_current_state()
if orientation_constraints is not None:
constraints = Constraints()
constraints.orientation_constraints = orientation_constraints
self._group.set_path_constraints(constraints)
plan = self._group.plan()
return plan
def init_orient_constraints(x_tol, y_tol, z_tol):
if x_tol < 0.1:
x_tol = 0.1
if y_tol < 0.1:
y_tol = 0.1
if z_tol < 0.1:
z_tol = 0.1
print x_tol, y_tol, z_tol
cur_pose = manipulator.get_current_pose().pose
upright_constraints = Constraints()
upright_constraints.name = "upright"
orientation_constraint = OrientationConstraint()
# orientation_constraint.header = pose.header
orientation_constraint.header.frame_id = "world"
orientation_constraint.link_name = manipulator.get_end_effector_link()
print "end link: ", manipulator.get_end_effector_link()
orientation_constraint.orientation = cur_pose.orientation
#IIWA
orientation_constraint.absolute_x_axis_tolerance = x_tol
orientation_constraint.absolute_y_axis_tolerance = y_tol
orientation_constraint.absolute_z_axis_tolerance = z_tol
orientation_constraint.weight = 1.0
upright_constraints.orientation_constraints.append(orientation_constraint)
return upright_constraints
def set_pose_constraints(self, tol_joint1, tol_joint2, tol_joint3):
angle_constraints = Constraints()
joint_constraint = JointConstraint()
angle_constraints.name = "angle"
joint_constraint.position = self.joint1_con
joint_constraint.tolerance_above = tol_joint1
joint_constraint.tolerance_below = tol_joint1
joint_constraint.weight = 0.1
joint_constraint.joint_name = "arm_shoulder_pan_joint"
angle_constraints.joint_constraints.append(joint_constraint)
joint_constraint2 = JointConstraint()
joint_constraint2.position = self.joint2_con
joint_constraint2.tolerance_above = tol_joint2
joint_constraint2.tolerance_below = tol_joint2
joint_constraint2.weight = 0.2
joint_constraint2.joint_name = "arm_wrist_2_joint"
angle_constraints.joint_constraints.append(joint_constraint2)
joint_constraint3 = JointConstraint()
joint_constraint3.position = self.joint3_con
joint_constraint3.tolerance_above = tol_joint3
joint_constraint3.tolerance_below = tol_joint3
joint_constraint3.weight = 0.3
joint_constraint3.joint_name = "arm_wrist_1_joint"
angle_constraints.joint_constraints.append(joint_constraint3)
group.set_path_constraints(angle_constraints)
def init_upright_path_constraints(): cur_pose = manipulator.get_current_pose().pose upright_constraints = Constraints() upright_constraints.name = "upright" orientation_constraint = OrientationConstraint() # orientation_constraint.header = pose.header orientation_constraint.header.frame_id = "world" orientation_constraint.link_name = manipulator.get_end_effector_link() print "end link: ", manipulator.get_end_effector_link() orientation_constraint.orientation = cur_pose.orientation #IIWA orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 #UR5 # orientation_constraint.absolute_x_axis_tolerance = 3.14 # orientation_constraint.absolute_y_axis_tolerance = 0.1 # orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 upright_constraints.orientation_constraints.append(orientation_constraint) return upright_constraints
def add_way_points(self, waypoints):
arm = self.arm
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
arm.set_path_constraints(consts)
wpose = arm.get_current_pose().pose
wpose.orientation.x = Qux
wpose.orientation.y = Quy
wpose.orientation.z = Quz
wpose.orientation.w = Quw
wpose.position.x = a[5]
wpose.position.y = a[3]
wpose.position.z = a[4]
waypoints.append(copy.deepcopy(wpose))
# print waypoints
# print "----------------\n"
return waypoints
def set_pose_quat_target(self, pose):
############################
rospy.logwarn("set_pose_quat_target")
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
start_pose = self.arm.get_current_pose(self.end_effector_link)
rospy.logwarn(self.end_effector_link)
rospy.logwarn("start_pose:")
rospy.logwarn(start_pose)
rospy.logwarn("pose:")
rospy.logwarn(pose)
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = self.end_effector_link
orientation_constraint.orientation.x = start_pose.pose.orientation.x
orientation_constraint.orientation.y = start_pose.pose.orientation.y
orientation_constraint.orientation.z = start_pose.pose.orientation.z
orientation_constraint.orientation.w = start_pose.pose.orientation.w
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
rospy.logwarn("constraints:")
rospy.logwarn(constraints)
# Set the path constraints on the right_arm
self.arm.set_path_constraints(constraints)
############################
self.arm.set_pose_target(pose, self.end_effector_link)
def move_ik(self, xyz):
move_group = self.arm_group
fixed_end_effector = OrientationConstraint()
fixed_end_effector.link_name = "end_effector_link"
fixed_end_effector.header.frame_id = "link1"
fixed_end_effector.orientation.w = 1.0
fixed_end_effector.absolute_x_axis_tolerance = 0.1
fixed_end_effector.absolute_y_axis_tolerance = 0.1
fixed_end_effector.absolute_z_axis_tolerance = 3.14
fixed_end_effector.weight = 1.0
constraint = Constraints()
constraint.name = "tilt constraint"
constraint.orientation_constraints = [fixed_end_effector]
move_group.set_path_constraints(constraint)
move_group.set_position_target(xyz)
try:
move_group.go(wait=True)
except MoveItCommanderException:
print "sorry cant move here!"
# move_group.stop()
self.moving = False
def plan_straight(self, waypoint, orientation_constraints):
"""
Generates a plan given an end effector pose subject to orientation constraints
Inputs:
target: A geometry_msgs/PoseStamped message containing the end effector pose goal
orientation_constraints: A list of moveit_msgs/OrientationConstraint messages
Outputs:
path: A moveit_msgs/RobotTrajectory path
"""
# waypoint is a list of poses
# self._group.set_pose_target(target)
# self._group.set_start_state_to_current_state()
constraints = Constraints()
constraints.orientation_constraints = orientation_constraints
self._group.set_path_constraints(constraints)
#self._scene.setMaxVelocityScalingFactor(0.5)
(plan, fraction) = self._group.compute_cartesian_path(
waypoint, # waypoints to follow
0.01, # eef_step
0.0)
return plan, fraction
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# Create a path constraint for the arm
# UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
joint_const = JointConstraint()
joint_const.joint_name = "gripper_r_joint_r"
joint_const.position = 0
consts = Constraints()
consts.joint_constraints = [joint_const]
right_arm.set_path_constraints(consts)
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# print (a)
# print (Qux, Quy, Quz, Quw)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Right_Qux
pose_goal.orientation.y = Right_Quy
pose_goal.orientation.z = Right_Quz
pose_goal.orientation.w = Right_Quw
pose_goal.position.x = Neurondata[5]
pose_goal.position.y = Neurondata[3]
pose_goal.position.z = Neurondata[4]
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def plan_motion_constrained(commander, goal_pose):
#Define Pose
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = goal_pose[0]
goal.pose.position.y = goal_pose[1]
goal.pose.position.z = goal_pose[2]
goal.pose.orientation.x = goal_pose[3]
goal.pose.orientation.y = goal_pose[4]
goal.pose.orientation.z = goal_pose[5]
goal.pose.orientation.w = goal_pose[6]
commander.set_pose_target(goal)
commander.set_start_state_to_current_state()
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
commander.set_path_constraints(consts)
#Plan a path
plan = commander.plan()
#Return plan
return plan
def append_traj_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
""" Appends a trajectory_point to the given move_group goal, returns it appended"""
if goal_to_append == None:
rospy.logerr("append_trajectory_point_to_move_group_goal needs a goal!")
return
#goal_to_append = MoveGroupGoal()
#rospy.logwarn("goal_to_append is: \n" + str(goal_to_append))
traj_c = TrajectoryConstraints()
goal_c = Constraints()
goal_c.name = "traj_constraint"
# Position constraint
position_c = PositionConstraint()
position_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
position_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 2.0
goal_c.position_constraints.append(position_c)
# Orientation constraint
orientation_c = OrientationConstraint()
orientation_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
orientation_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
traj_c.constraints.append(goal_c)
goal_to_append.request.trajectory_constraints = traj_c
return goal_to_append
def plan_to_pose(self, target, orientation_constraints=None):
"""
Generates a plan given an end effector pose subject to orientation constraints
Inputs:
target: A geometry_msgs/PoseStamped message containing the end effector pose goal
orientation_constraints: A list of moveit_msgs/OrientationConstraint messages
Outputs:
success: boolean success flag.
plan: A moveit_msgs/RobotTrajectory path.
time_taken: planning time.
error_code: MoveitErrorCodes.
"""
self._group.set_pose_target(target)
self._group.set_start_state_to_current_state()
if orientation_constraints is not None:
constraints = Constraints()
constraints.orientation_constraints = orientation_constraints
self._group.set_path_constraints(constraints)
success, plan, time_taken, error_code = self._group.plan()
return success, plan, time_taken, error_code
def create_basic_mp_position_request(
planning_frame,
link_name,
target_point_offset,
planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
position_constraints = []
position_constraint = PositionConstraint()
header = std_msgs.msg.Header()
header.frame_id = planning_frame
position_constraint.header = header
position_constraint.link_name = link_name
position_constraint.target_point_offset = target_point_offset
position_constraints = [position_constraint]
constraint = Constraints()
constraint.position_constraints = position_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
def setConstr(self, orien_const, right):
constraints = Constraints()
constraints.orientation_constraints = orien_const
if right > 0:
self._groupR.set_path_constraints(constraints)
else:
self._groupL.set_path_constraints(constraints)
def move(self, num):
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node("moveit_node")
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander('right_arm')
right_arm = baxter_interface.Limb("right")
right_arm_pose = right_arm.endpoint_pose()
# print(right_arm_pose)
initial_orient = [
right_arm_pose['orientation'].x, right_arm_pose['orientation'].y,
right_arm_pose['orientation'].z, right_arm_pose['orientation'].w
]
dig_class = dg.Digits()
digit_shapes = dig_class.digit
num_shape = digit_shapes[str(num)]
i = 0
# print(num_shape, num, len(num_shape))
while i < len(num_shape):
(x, y) = num_shape[i]
pose_target = PoseStamped()
pose_target.header.frame_id = "base"
# print(pose_target.pose.position)
right_arm_pose = right_arm.endpoint_pose()
x = right_arm_pose['position'].x
y = right_arm_pose['position'].y
z = right_arm_pose['position'].z
print(right_arm_pose)
pose_target.pose.position.x = x + num_shape[i][0] * 0.1
pose_target.pose.position.y = y + num_shape[i][1] * 0.1
pose_target.pose.position.z = z
pose_target.pose.orientation.x = initial_orient[0]
pose_target.pose.orientation.y = initial_orient[1]
pose_target.pose.orientation.z = initial_orient[2]
pose_target.pose.orientation.w = initial_orient[3]
group.set_pose_target(pose_target)
group.set_start_state_to_current_state()
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.x = initial_orient[0]
orien_const.orientation.y = initial_orient[1]
orien_const.orientation.z = initial_orient[2]
orien_const.orientation.w = initial_orient[3]
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
group.set_path_constraints(consts)
plan = group.plan()
group.execute(plan)
i += 1
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
# if Rightfinger > -55 :
# right_joint_const.position = 0.024
# else:
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
right_arm.set_path_constraints(consts)
# 计算夹取姿态
if obj_theta <= 0:
(Qux, Quy, Quz,
Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
(-180 - obj_theta) / 180 * pi)
else:
(Qux, Quy, Quz,
Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
(180 - obj_theta) / 180 * pi)
# (Qux, Quy, Quz, Quw) = quaternion_from_euler(90/180*pi, 0, -180/180*pi)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Qux
pose_goal.orientation.y = Quy
pose_goal.orientation.z = Quz
pose_goal.orientation.w = Quw
# pose_goal.position.y = 0.0244387395252 + (-0.595877665686-0.0244387395252)*obj_x/320
# pose_goal.position.x = 0.625989932306 + (0.197518221397-0.625989932306)*obj_y/240
pose_goal.position.x = 0.1819576873 + (160 - obj_y) * (
0.494596343128 - 0.1819576873) / 160
pose_goal.position.y = obj_x * (
-0.455644324437 + 0.0238434066464) / 220 - 0.0238434066464
pose_goal.position.z = 0.0942404372508
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 清除路径约束
right_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def plan_to_pose(self, target, orientation_constraints):
self._group.set_pose_target(target)
self._group.set_start_state_to_current_state()
constraints = Constraints()
constraints.orientation_constraints = orientation_constraints
self._group.set_path_constraints(constraints)
plan = self._group.plan()
return plan
def move_to_coord(trans,
rot,
arm,
which_arm='left',
keep_oreint=False,
base="base"):
#coordinates are in baxter's torso!
goal = PoseStamped()
goal.header.frame_id = base
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = which_arm + "_gripper"
orien_const.header.frame_id = "base"
#constrain it to be the same as my goal state. Seems reasonable.
orien_const.orientation.x = rot[0]
orien_const.orientation.y = rot[1]
orien_const.orientation.z = rot[2]
orien_const.orientation.w = rot[3]
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
print(consts)
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
arm.execute(arm_plan)
def constraint_planner(start_robot_state, goal_config, group_handle, planner_name, planning_attemps, planning_time): planning_workspace = WorkspaceParameters(); planning_workspace.header.frame_id = "/base_link"; planning_workspace.header.stamp = rospy.Time.now(); planning_workspace.min_corner.x = -1; planning_workspace.min_corner.y = -1; planning_workspace.min_corner.z = -1; planning_workspace.max_corner.x = 1; planning_workspace.max_corner.y = 1; planning_workspace.max_corner.z = 1; # Set Start start_state = RobotState(); start_state = start_robot_state; print ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"; #print start_robot_state; # Set Goal goal_state = Constraints(); Jnt_constraint = setJointConstraints(goal_config, group_handle); goal_state.joint_constraints = Jnt_constraint; #print Jnt_constraint; # Set Constraints des_w = -0.070099; des_x = 0.41382; des_y = -0.57302; des_z = 0.70391; rotation_constraints = setOrientationConstraints(des_w,des_x,des_y,des_z,group_handle, weight = 1.0); # Generating Request planningRequest = MotionPlanRequest(); planningRequest.workspace_parameters = planning_workspace; planningRequest.start_state = start_robot_state; planningRequest.goal_constraints.append(goal_state); # Setting Constraint planningRequest.path_constraints.name = 'scoop_constraint'; planningRequest.path_constraints.orientation_constraints.append(rotation_constraints); #traj_constraint = Constraints(); #traj_constraint.orientation_constraints.append(rotation_constraints); #planningRequest.trajectory_constraints.constraints.append(traj_constraint); planningRequest.planner_id = "RRTConnectkConfigDefault"; planningRequest.group_name = group_handle.get_name(); planningRequest.num_planning_attempts = planning_attemps; planningRequest.allowed_planning_time = planning_time; planningRequest.max_velocity_scaling_factor = 1.0; Planning_req.publish(planningRequest);
def test_MoveTo(self):
stage = stages.MoveTo("move", self.planner)
self._check(stage, "group", "group")
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "path_constraints", Constraints())
stage.setGoal(PoseStamped())
stage.setGoal(PointStamped())
stage.setGoal(RobotState())
stage.setGoal("named pose")
stage.setGoal(dict(joint1=1.0, joint2=2.0))
self._check(stage, "path_constraints", Constraints())
def move_to_coord(trans, rot, keep_oreint=False):
#coordinates are in baxter's torso!
global arm
goal = PoseStamped()
goal.header.frame_id = "base"
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = 'left'+"_gripper";
orien_const.header.frame_id = "base";
#constrain it to be the same as my goal state. Seems reasonable.
orien_const.orientation.x = rot[0];
orien_const.orientation.y = rot[1];
orien_const.orientation.z = rot[2];
orien_const.orientation.w = rot[3];
orien_const.absolute_x_axis_tolerance = 0.02;
orien_const.absolute_y_axis_tolerance = 0.02;
orien_const.absolute_z_axis_tolerance = 0.02;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
print(consts)
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
#arm.execute(arm_plan)
arm.go(arm_plan,wait=True)
def create_constraint(name): orien_const = OrientationConstraint() orien_const.link_name = name; orien_const.header.frame_id = "base"; orien_const.orientation.y = 2**.5/2 orien_const.orientation.w = 2**.5/2 orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 3.0; consts = Constraints() consts.orientation_constraints = [orien_const] return consts
def right_to_player(right_arm, right_gripper):
# get current number of cards player has
global player_cards
# get global deck coordinates
global deck_x
global deck_y
global deck_z
# create pose above drop point for player's card
card = PoseStamped()
card.header.frame_id = "base"
# x, y, and z position of card
card.pose.position.x = deck_x + .25
card.pose.position.y = deck_y + .2 + (.1 * player_cards)
card.pose.position.z = deck_z
# Orientation as a quaternion - straight down
card.pose.orientation.x = 0.0
card.pose.orientation.y = -1.0
card.pose.orientation.z = 0.0
card.pose.orientation.w = 0.0
# Plan and execute path to desired card position from current state
right_arm.set_pose_target(card)
right_arm.set_start_state_to_current_state()
# Add orientation constraint for path - straight down
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
right_plan = right_arm.plan()
right_arm.execute(right_plan)
# drop card
right_gripper.stop()
# sleep for a moment
rospy.sleep(1.0)
# increment number of cards player has
player_cards += 1
def move_to(self,
pos_x,
pos_y,
pos_z,
orien_x,
orien_y,
orien_z,
orien_w,
ik,
orien_const=None):
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = pos_x
request.ik_request.pose_stamped.pose.position.y = pos_y
request.ik_request.pose_stamped.pose.position.z = pos_z
request.ik_request.pose_stamped.pose.orientation.x = orien_x
request.ik_request.pose_stamped.pose.orientation.y = orien_y
request.ik_request.pose_stamped.pose.orientation.z = orien_z
request.ik_request.pose_stamped.pose.orientation.w = orien_w
try:
#Send the request to the service
response = ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("right_arm")
if orien_const is not None:
constraints = Constraints()
constraints.orientation_constraints = orien_const
group.set_path_constraints(constraints)
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
# group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print("Service call failed: %s")
def right_arm_go_down_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
waypoints = []
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 添加路径起始点
waypoints.append(copy.deepcopy(pose_goal))
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
# if Rightfinger > -55 :
# right_joint_const.position = 0.024
# else:
right_joint_const.position = 0.0239
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.position.z = pose_goal.position.z - 0.045
# pose_goal.position.y = pose_goal.position.y - 0.1
# 添加路径末端点
waypoints.append(copy.deepcopy(pose_goal))
# 路径规划
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "End effector pose %s" % waypoints
# robot = self.robot
# display_trajectory_publisher = self.display_trajectory_publisher
# display_trajectory = moveit_msgs.msg.DisplayTrajectory()
# display_trajectory.trajectory_start = robot.get_current_state()
# display_trajectory.trajectory.append(plan)
# display_trajectory_publisher.publish(display_trajectory)
# 规划和输出动作
right_arm.execute(plan, wait=False)
def set_upright_constraints(self,pose):
upright_constraints = Constraints()
orientation_constraint = OrientationConstraint()
upright_constraints.name = "upright"
orientation_constraint.header = pose.header
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.14
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
#orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
upright_constraints.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(upright_constraints)
def planMoveToPosHoldOrientation(pos=None, orientation=None, arm=None):
arm = arm or left_arm
if arm is left_arm:
pos = pos or left_arm_default_state[0]
orientation = orientation or left_arm_default_state[1]
if arm is right_arm:
pos = pos or right_arm_default_state[0]
orientation = orientation or right_arm_default_state[1]
#Second goal pose -----------------------------------------------------
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = pos[0]
goal_2.pose.position.y = pos[1]
goal_2.pose.position.z = pos[2]
#Orientation as a quaternion
orientation = orientation/np.linalg.norm(orientation)
goal_2.pose.orientation.x = orientation[0]
goal_2.pose.orientation.y = orientation[1]
goal_2.pose.orientation.z = orientation[2]
goal_2.pose.orientation.w = orientation[3]
#Set the goal state to the pose you just defined
arm.set_pose_target(goal_2)
#Set the start state for the left arm
arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[3]
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
#Plan a path
return (arm, arm.plan())
def right_to_deck(right_arm, right_gripper):
# get global deck coordinates
global deck_x
global deck_y
global deck_z
# create pose above deck of cards
deck = PoseStamped()
deck.header.frame_id = "base"
# x, y, and z position of deck
deck.pose.position.x = deck_x
deck.pose.position.y = deck_y
deck.pose.position.z = deck_z
# Orientation as a quaternion - straight down
deck.pose.orientation.x = 0.0
deck.pose.orientation.y = -1.0
deck.pose.orientation.z = 0.0
deck.pose.orientation.w = 0.0
# Plan and execute path from current state
right_arm.set_pose_target(deck)
right_arm.set_start_state_to_current_state()
# Add orientation constraint for path - straight down
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
right_plan = right_arm.plan()
right_arm.execute(right_plan)
# pick up card
right_gripper.command_suction(True)
# increment z position as deck height decreases
deck_z -= .001
# sleep for a moment
rospy.sleep(1.0)
def planPath(positions, orientation=None, arm=None, holdOrientation=False, step=0.01, threshold=1000):
arm = arm or left_arm
if arm is left_arm:
positions = (left_arm_default_state[0],) if positions is None else positions
orientation = left_arm_default_state[1] if orientation is None else orientation
if arm is right_arm:
positions = (right_arm_default_state[0],) if positions is None else positions
orientation = right_arm_default_state[1] if orientation is None else orientation
# Compute path
waypoints = []
# start with the current pose
#waypoints.append(arm.get_current_pose().pose)
wpose = Pose()
orientation = orientation/np.linalg.norm(orientation)
wpose.orientation.x = orientation[0]
wpose.orientation.y = orientation[1]
wpose.orientation.z = orientation[2]
wpose.orientation.w = orientation[3]
for pos in positions:
wpose.position.x = pos[0]
wpose.position.y = pos[1]
wpose.position.z = pos[2]
waypoints.append(copy.deepcopy(wpose))
if holdOrientation:
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[3]
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
#Plan a path
(plan, fraction) = arm.compute_cartesian_path(waypoints, step, threshold)
return (arm, plan, fraction)
def place(robot):
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = 0.0
p.pose.position.z = 0.5
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1.0
g = PlaceLocation()
g.place_pose = p
g.pre_place_approach.direction.vector.z = -1.0
g.post_place_retreat.direction.vector.x = -1.0
g.post_place_retreat.direction.header.frame_id = robot.get_planning_frame()
g.pre_place_approach.direction.header.frame_id = "r_wrist_roll_link"
g.pre_place_approach.min_distance = 0.1
g.pre_place_approach.desired_distance = 0.2
g.post_place_retreat.min_distance = 0.1
g.post_place_retreat.desired_distance = 0.25
g.post_place_posture = open_gripper(g.post_place_posture)
group = robot.get_group("right_arm")
group.set_support_surface_name("table")
# Add path constraints
constr = Constraints()
constr.orientation_constraints = []
ocm = OrientationConstraint()
ocm.link_name = "r_wrist_roll_link"
ocm.header.frame_id = p.header.frame_id
ocm.orientation.x = 0.0
ocm.orientation.y = 0.0
ocm.orientation.z = 0.0
ocm.orientation.w = 1.0
ocm.absolute_x_axis_tolerance = 0.2
ocm.absolute_y_axis_tolerance = 0.2
ocm.absolute_z_axis_tolerance = math.pi
ocm.weight = 1.0
constr.orientation_constraints.append(ocm)
# group.set_path_constraints(constr)
group.set_planner_id("RRTConnectkConfigDefault")
group.place("part", g)
def add_robot_constraints():
constraint = Constraints()
constraint.name = "camera constraint"
roll_constraint = OrientationConstraint()
# 'base_link' is equal to the world link
roll_constraint.header.frame_id = 'world'
# The link that must be oriented upwards
roll_constraint.link_name = "camera"
roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0))
# Allow rotation of 45 degrees around the x and y axis
roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees
roll_constraint.absolute_y_axis_tolerance = np.pi
roll_constraint.absolute_z_axis_tolerance = np.pi/2
# The roll constraint is the only constraint
roll_constraint.weight = 1
#constraint.orientation_constraints = [roll_constraint]
joint_1_constraint = JointConstraint()
joint_1_constraint.joint_name = "joint_1"
joint_1_constraint.position = 0
joint_1_constraint.tolerance_above = np.pi/2
joint_1_constraint.tolerance_below = np.pi/2
joint_1_constraint.weight = 1
joint_4_constraint = JointConstraint()
joint_4_constraint.joint_name = "joint_4"
joint_4_constraint.position = 0
joint_4_constraint.tolerance_above = np.pi/2
joint_4_constraint.tolerance_below = np.pi/2
joint_4_constraint.weight = 1
joint_5_constraint = JointConstraint()
joint_5_constraint.joint_name = "joint_5"
joint_5_constraint.position = np.pi/2
joint_5_constraint.tolerance_above = np.pi/2
joint_5_constraint.tolerance_below = np.pi/2
joint_5_constraint.weight = 1
joint_6_constraint = JointConstraint()
joint_6_constraint.joint_name = "joint_6"
joint_6_constraint.position = np.pi-0.79
joint_6_constraint.tolerance_above = np.pi
joint_6_constraint.tolerance_below = np.pi
joint_6_constraint.weight = 1
constraint.joint_constraints = [joint_1_constraint, joint_6_constraint]
return constraint
def __check_joint_validity_moveit(self, joints):
"""
Check target joint validity (no collision) with MoveIt.
:return: The target joint validity in a boolean, and the two links colliding if available
"""
robot_state_target = RobotStateMoveIt()
robot_state_target.joint_state.header.frame_id = "world"
robot_state_target.joint_state.position = joints
robot_state_target.joint_state.name = self.__arm_state.joints_name
group_name = self.__arm.get_name()
null_constraints = Constraints()
try:
response = self.__parameters_validator.check_state_validity(
robot_state_target, group_name, null_constraints)
if not response.valid:
if len(response.contacts) > 0:
rospy.logwarn(
'Jog Controller - Joints target unreachable because of collision between %s and %s',
response.contacts[0].contact_body_1,
response.contacts[0].contact_body_2)
return False, response.contacts[
0].contact_body_1, response.contacts[0].contact_body_2
else: # didn't succeed to get the contacts on the real robot
rospy.logwarn_throttle(
1,
'Jog Controller - Joints target unreachable because of '
'collision between two parts of Ned')
return False, None, None
else:
return True, None, None
except AttributeError as _e: # maybe delete later, useful for the test on real robot when using the service
return True, None, None
def clear_goal_constraints(self):
"""
Clear all goal constraints that were previously set.
Note that this function is called automatically after each `plan_kinematic_path()`.
"""
self.kinematic_path_request.motion_plan_request.goal_constraints = \
[Constraints()]
def limit_base_joint_constraint(): ur5_constraints = Constraints() shoulder_pan_joint_constraint = JointConstraint() shoulder_lift_joint_constraint = JointConstraint() wrist_1_joint_constraint = JointConstraint() shoulder_pan_joint_constraint.joint_name = "shoulder_pan_joint" shoulder_pan_joint_constraint.position = 0 shoulder_pan_joint_constraint.tolerance_above = 3.14 shoulder_pan_joint_constraint.tolerance_below = 3.14 shoulder_pan_joint_constraint.weight = 1 shoulder_lift_joint_constraint.joint_name = "shoulder_lift_joint" shoulder_lift_joint_constraint.position = 0 shoulder_lift_joint_constraint.tolerance_above = 0 shoulder_lift_joint_constraint.tolerance_below = 3.14 shoulder_lift_joint_constraint.weight = 1 wrist_1_joint_constraint.joint_name = "wrist_1_joint" wrist_1_joint_constraint.position = 0 wrist_1_joint_constraint.tolerance_above = 3.14 wrist_1_joint_constraint.tolerance_below = 3.14 wrist_1_joint_constraint.weight = 1 ur5_constraints.joint_constraints.append(shoulder_pan_joint_constraint) ur5_constraints.joint_constraints.append(shoulder_lift_joint_constraint) ur5_constraints.joint_constraints.append(wrist_1_joint_constraint) return ur5_constraints
def quick_motion_test_client():
rospy.wait_for_service('/lightning/lightning_get_path')
print "Working on it..."
req = GetMotionPlanRequest()
req.motion_plan_request.group_name = "torso"
req.motion_plan_request.start_state.joint_state.name = ["torso_lift_joint"]
req.motion_plan_request.start_state.joint_state.position = [float(0.2)]
tempConstraint = JointConstraint()
tempConstraint.joint_name = req.motion_plan_request.start_state.joint_state.name[0]
tempConstraint.position = float(0.15)
fullConstraint = Constraints()
fullConstraint.joint_constraints = [tempConstraint]
req.motion_plan_request.goal_constraints = [fullConstraint]
req.motion_plan_request.allowed_planning_time = 50.0
try:
client_func = rospy.ServiceProxy('/lightning/lightning_get_path', GetMotionPlan)
res = client_func(req)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def move_to_coord(trans, rot, arm, which_arm='left', keep_oreint=False):
goal = PoseStamped()
goal.header.frame_id = "base"
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = which_arm+"_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
arm.execute(arm_plan)
def move_group_client():
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
goal = MoveGroupGoal()
goal.request.group_name = "right_arm"
goal_constraints = Constraints()
goal_constraints.name = 'goal_test'
#goal_constraints.joint_constraints.extend(createJointConstraints())
#goal_constraints.joint_constraints.extend(createJointConstraintsZero())
goal_constraints.position_constraints.extend(createPositionConstraint())
goal_constraints.orientation_constraints.extend(createOrientationConstraint())
goal.request.goal_constraints.append(goal_constraints)
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s"%e
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def callback(message):
global last_x
global last_y
global last_z
global error
try:
if message.transforms[0].child_frame_id == 'ar_marker_23':
#Read the position of artag
(trans,rot) = listener.lookupTransform('/base', '/ar_marker_23', rospy.Time(0))
#Execute only when the difference of the current position and the last position exceed the threshold
if ((abs(trans[0]-last_x) < threshold) and (abs(trans[1]-last_y) < threshold) and (abs(trans[2]-last_z) < threshold)):
pass
else:
last_x = trans[0]
last_y = trans[1]
last_z = trans[2]
print trans
#Goal position
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]-0.1
#Orientation as a quaternion: default orientation
goal.pose.orientation.x = 0.5
goal.pose.orientation.y = 0.5
goal.pose.orientation.z = 0.5
goal.pose.orientation.w = -0.5
#Set the goal state
right_arm.set_pose_target(goal)
#Set the start state
right_arm.set_start_state_to_current_state()
#Create a orientation constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = 0.5;
orien_const.orientation.y = 0.5;
orien_const.orientation.z = 0.5;
orien_const.orientation.w = -0.5;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
right_arm.execute(right_plan)
else:
pass
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print 'exception'
jc1 = JointConstraint()
jc1.joint_name = "j5"
jc1.position = 1.25752837976
jc1.tolerance_above = 0.0001
jc1.tolerance_below = 0.0001
jc1.weight = 1.0
jc0 = JointConstraint()
jc0.joint_name = "flange"
jc0.position = -4.43859335239
jc0.tolerance_above = 0.0001
jc0.tolerance_below = 0.0001
jc0.weight = 1.0
cons = Constraints()
cons.name = ""
cons.joint_constraints.append(jc2)
cons.joint_constraints.append(jc3)
cons.joint_constraints.append(jc4)
cons.joint_constraints.append(jc5)
cons.joint_constraints.append(jc1)
cons.joint_constraints.append(jc0)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
left_gripper.calibrate()
rospy.sleep(2.0)
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = 0.6
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
left_arm.execute(left_plan)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
print('Done!')
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = 0.4
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 2: ')
left_arm.execute(left_plan)
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.0
goal_3.pose.position.y = 0.7
goal_3.pose.position.z = 0.0
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_3)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 3: ')
left_arm.execute(left_plan)
#Fourth goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_4 = PoseStamped()
goal_4.header.frame_id = "base"
#x, y, and z position
goal_4.pose.position.x = 0.4
goal_4.pose.position.y = 0.7
goal_4.pose.position.z = 0.3
#Orientation as a quaternion
goal_4.pose.orientation.x = 0.0
goal_4.pose.orientation.y = -1.0
goal_4.pose.orientation.z = 0.0
goal_4.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_4)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
#orien_const = OrientationConstraint()
#orien_const.link_name = "left_gripper";
#orien_const.header.frame_id = "base";
#orien_const.orientation.y = -1.0;
#orien_const.absolute_x_axis_tolerance = 0.1;
#orien_const.absolute_y_axis_tolerance = 0.1;
#orien_const.absolute_z_axis_tolerance = 0.1;
#orien_const.weight = 1.0;
#consts = Constraints()
#consts.orientation_constraints = [orien_const]
#left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 4: ')
left_arm.execute(left_plan)
rospy.sleep(2.0)
#Close the left gripper
print('Closing...')
left_gripper.close(block=True)
rospy.sleep(0.5)
#Open the left gripper
print('Opening...')
left_gripper.open(block=True)
rospy.sleep(1.0)
print('Done!')
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def kinect_planner():
global fresh_data
global joint_target, pose_target, goal
global joints_req, pose_req
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('kinect_trajectory_planner', anonymous=True)
rate = rospy.Rate(0.5)
#rospy.sleep(3)
# Instantiate a RobotCommander object.
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot).
scene = moveit_commander.PlanningSceneInterface()
print "Remember to disable firewall if it's not working"
# Instantiate MoveGroupCommander objects for arms and Kinect2.
group = moveit_commander.MoveGroupCommander("Kinect2_Target")
group_left_arm = moveit_commander.MoveGroupCommander("left_arm")
group_right_arm = moveit_commander.MoveGroupCommander("right_arm")
# We create this DisplayTrajectory publisher to publish
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher('planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=5)
# Set the planner for Moveit
group.set_planner_id("RRTConnectkConfigDefault")
group.set_planning_time(8)
group.set_pose_reference_frame('base_footprint')
# Setting tolerance
group.set_goal_tolerance(0.08)
group.set_num_planning_attempts(10)
# Suscribing to the desired pose topic
target = rospy.Subscriber("desired_pose", PoseStamped, callback)
target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints)
# Locating the arms and the Kinect2 sensor
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
#group_kinect_values = group_kinect.get_current_joint_values()
neck_init_joints = group.get_current_joint_values()
neck_init_joints[0] = -1.346
neck_init_joints[1] = -1.116
neck_init_joints[2] = -2.121
neck_init_joints[3] = 0.830
neck_init_joints[4] = 1.490
neck_init_joints[5] = 0.050
neck_init_joints[6] = 0
neck_init_joints[7] = 0
neck_init_joints[8] = 0
neck_init_joints[9] = 0
# Creating a box to limit the arm position
box_pose = PoseStamped()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = 0.6
box_pose.pose.position.y = 0.03
box_pose.pose.position.z = 1.5
box_pose.header.frame_id = 'base_footprint'
scene.add_box('box1', box_pose, (0.4, 0.4, 0.1))
rospy.sleep(2)
# Defining position constraints for the trajectory of the kinect
neck_const = Constraints()
neck_const.name = 'neck_constraints'
target_const = JointConstraint()
target_const.joint_name = "neck_joint_end"
target_const.position = 0.95
target_const.tolerance_above = 0.45
target_const.tolerance_below = 0.05
target_const.weight = 0.9 # Importance of this constraint
neck_const.joint_constraints.append(target_const)
# Talking to the robot
#client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client = actionlib.SimpleActionClient('/neck/follow_joint_trajectory', FollowJointTrajectoryAction)
print "====== Waiting for server..."
client.wait_for_server()
print "====== Connected to server"
while not rospy.is_shutdown():
print('.')
skip_iter = False
if fresh_data == True: #If a new pose is received, plan.
# Update arms position
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
# Set the target pose (or joint values) and generate a plan
if pose_req:
group.set_pose_target(pose_target)
if joints_req:
print('about to set joint target')
print(joint_target)
try:
if len(joint_target) == 6:
#Setting also values for the four virtual joints (one prismatic, and three rotational)
new_joint_target = joint_target + [0.1]*4
else:
new_joint_target = joint_target
print('setting: '),
print new_joint_target
group.set_joint_value_target(new_joint_target)
except:
print('cannot set joint target')
skip_iter = True
print "=========== Calculating trajectory... \n"
# Generate several plans and compare them to get the shorter one
plan_opt = dict()
differ = dict()
if not skip_iter:
try:
num = 1
rep = 0
# Generate several plans and compare them to get the shortest one
#for num in range(1,8):
while num < 7:
num += 1
plan_temp = group.plan()
move(plan_temp)
plan_opt[num] = goal.trajectory
diff=0
for point in goal.trajectory.points:
# calculate the distance between initial pose and planned one
for i in range(0,6):
diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff)
differ[num] = diff
# If the current plan is good, take it
if diff < 110:
break
# If plan is too bad, don't consider it
if diff > 400:
num = num - 1
print "Plan is too long. Replanning."
rep = rep + 1
if rep > 4:
num = num +1
# If no plan was found...
if differ == {}:
print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range."
break
else:
# Select the shortest plan
min_plan = min(differ.itervalues())
select = [k for k, value in differ.iteritems() if value == min_plan]
goal.trajectory = plan_opt[select[0]]
#print " Plan difference:======= ", differ
#print " Selected plan:========= ", select[0]
# Remove the last 4 names and data from each point (dummy joints) before sending the goal
goal.trajectory.joint_names = goal.trajectory.joint_names[:6]
for point in goal.trajectory.points:
point.positions = point.positions[:6]
point.velocities = point.velocities[:6]
point.accelerations = point.accelerations[:6]
print "Sending goal"
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
# Change the position of the virtual joint to avoid collision
neck_joints = group.get_current_joint_values()
print('neck joints:')
print neck_joints
#neck_joints[6] = 0.7
#group.set_joint_value_target(neck_joints)
#group.go(wait=True)
except (KeyboardInterrupt, SystemExit):
client.cancel_goal()
raise
rate.sleep()
fresh_data = False
rate.sleep()
