class PythonTimeParameterizationTest(unittest.TestCase):
PLANNING_GROUP = "arm"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def plan(self):
start_pose = self.group.get_current_pose().pose
goal_pose = self.group.get_current_pose().pose
goal_pose.position.z -= 0.1
(plan,
fraction) = self.group.compute_cartesian_path([start_pose, goal_pose],
0.005, 0.0)
self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
return plan
def time_parameterization(self, plan):
ref_state = self.commander.get_current_state()
retimed_plan = self.group.retime_trajectory(
ref_state, plan, velocity_scaling_factor=0.1)
return retimed_plan
def test_plan_and_time_parameterization(self):
plan = self.plan()
retimed_plan = self.time_parameterization(plan)
def get_current_state():
# Prepare a new state object for validity check
rs = RobotState()
robot = RobotCommander()
robot_state = robot.get_current_state()
rs.joint_state.name = robot_state.joint_state.name
rs.joint_state.position = list(robot_state.joint_state.position) # filler for rest of the joint angles not found in waypoint
return rs
class PythonTimeParameterizationTest(unittest.TestCase):
PLANNING_GROUP = "manipulator"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def plan(self):
start_pose = self.group.get_current_pose().pose
goal_pose = self.group.get_current_pose().pose
goal_pose.position.z -= 0.1
(plan, fraction) = self.group.compute_cartesian_path(
[start_pose, goal_pose], 0.005, 0.0
)
self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
return plan
def time_parameterization(self, plan, algorithm):
ref_state = self.commander.get_current_state()
retimed_plan = self.group.retime_trajectory(
ref_state,
plan,
velocity_scaling_factor=0.1,
acceleration_scaling_factor=0.1,
algorithm=algorithm,
)
return retimed_plan
def test_plan_and_time_parameterization(self):
plan = self.plan()
retimed_plan = self.time_parameterization(
plan, "iterative_time_parameterization"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(
plan, "iterative_spline_parameterization"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(
plan, "time_optimal_trajectory_generation"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(plan, "")
self.assertTrue(
len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm"
)
class ClopemaRobotCommander(MoveGroupCommander):
""" Common interface to CloPeMa robor, it extends the MoveGroupCommander """
def __init__(self, group_name='arms'):
""" Initialize the robot commander for particular group."""
MoveGroupCommander.__init__(self, group_name)
self.set_planner_id("RRTConnectkConfigDefault")
# Sleep in order to load an ik module
rospy.sleep(1)
self.robot = RobotCommander()
self._ik_use_tip_link = True
self._start_state = None
self.overwrite_time_parameterization = True
# Decrease the joint tolerance
self.set_goal_joint_tolerance(0.00001)
@staticmethod
def set_servo_power_off(force=False):
"""Shut off the servo power."""
set_drive_power(power=False)
@staticmethod
def set_robot_speed(speed):
"""
Set robot speed, possible value can be from 0 to 1. This value is
then converted to robot speed units (i.e. speed*10000).
Note that there is a speed limit defined by the I001 variable on the
teach pendant. This limit is in robot speed units and will overide the
speed set by this function.
"""
set_robot_speed(speed)
@staticmethod
def get_robot_speed():
res = set_robot_speed(-1)
return res.current_speed
@staticmethod
def set_robot_io(address, value):
raise NotImplementedError()
@staticmethod
def get_robot_io(address):
raise NotImplementedError()
@staticmethod
def get_dx100_joints():
return DX100_JOINTS
@staticmethod
def set_gripper_state(gripper_action, state, wait=False):
"""
Set gripper state i.e. open or close.
:arg gripper: Gripper id, use robot.R1_GRIPPER or robot.R2_GRIPPER
:arg state: Gripper state, use robot.GRIPPER_OPEN or robot.GRIPPER_CLOSE
:arg wait: Wait for finish, True or False, False is depfault.
Note: This function requires the node to be initialize i.e. rospy.init_node()
to ba called, otherwise it will result in error.
"""
# Gripper action server client
client = actionlib.SimpleActionClient(gripper_action,
GripperCommandAction)
client.wait_for_server()
# Execute goal
goal = GripperCommandGoal()
goal.command.position = state
client.send_goal(goal)
if wait:
client.wait_for_result(rospy.Duration.from_sec(5.0))
def get_current_state(self):
""" Get a RobotState message describing the current state of the robot"""
return self.robot.get_current_state()
def append_ext_axis(self, start_state, trajectory, position):
"""Append trajectory with movement of the external axis.
INPUT
start_state [moveit_msgs/RobotState]
trajectory [moveit_msgs/RobotTrajectory]
position [Float]
OUTPUT
trajectory [moveit_msgs/RobotTrajectory or None]
"""
traj = copy.deepcopy(trajectory)
N = len(traj.joint_trajectory.points)
start = start_state.joint_state.position[
start_state.joint_state.name.index("ext_axis")]
step = (position - start) / (N - 1)
if "ext_axis" in traj.joint_trajectory.joint_names:
index = traj.joint_trajectory.joint_names.index("ext_axis")
for i in range(0, N):
traj.joint_trajectory.points[i].positions = list(
traj.joint_trajectory.points[i].positions
)[index] = start + step * i
traj.joint_trajectory.points[i].velocities = list(
traj.joint_trajectory.points[i].velocities)[index] = 0
traj.joint_trajectory.points[i].accelerations = list(
traj.joint_trajectory.points[i].accelerations)[index] = 0
else:
traj.joint_trajectory.joint_names = list(
traj.joint_trajectory.joint_names) + ["ext_axis"]
for i in range(0, N):
traj.joint_trajectory.points[i].positions = list(
traj.joint_trajectory.points[i].positions) + [
start + step * i
]
traj.joint_trajectory.points[i].velocities = list(
traj.joint_trajectory.points[i].velocities) + [0]
traj.joint_trajectory.points[i].accelerations = list(
traj.joint_trajectory.points[i].accelerations) + [0]
if services.check_trajectory(start_state, traj, persistent=True):
return traj
else:
rospy.logerr(
"The trajectory is not collision free after adding external axis movement."
)
return None
return traj
def get_ik(self,
pose=None,
link=None,
poses=None,
links=None,
robot_state=None,
constraints=None):
"""Get inverse kinematic for one or two arms of the active group.
Arguments:
pose : {PoseStamped} Single target pose.
poses : {Iterable of PoseStaped}
Target poses for multiple end effectors.
link : {String} Single end effector link.
links : {Iterable of strings} Multiple end effector links.
robot_state : {RobotState message}
Initial robot state, if ommited current robot state is used.
constraints : {Constrains message}
Constrains for iverse kinematic solver.
Note that either pose and link or poses and links must be defined.
Returns:
robot_state : {RobotState message}
Solution to the inverse kinematic problem.
"""
from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest
from std_msgs.msg import Header
# Check validity of input
assert ((pose is not None and link is not None)
or (poses is not None and links is not None))
if poses is None:
poses = [pose]
links = [link]
# Get IK service proxy
rospy.wait_for_service(SRV_GET_IK)
srv = rospy.ServiceProxy(SRV_GET_IK, GetPositionIK)
# Get robot state if neccesary
if robot_state is None:
robot_state = self.get_current_state()
for p, l in zip(poses, links):
if type(p) is Pose:
p = PoseStamped(Header(0, rospy.Time.now(), "base_link"), p)
# Convert to tip link if required
if self._ik_use_tip_link:
p_, l = self._fix_link(p.pose, l)
p.pose = p_
# Pepare request
req = GetPositionIKRequest()
req.ik_request.group_name = GROUP_FOR_EE[l]
# Allways avoid collisions
req.ik_request.avoid_collisions = True
req.ik_request.robot_state = robot_state
if constraints is not None:
req.ik_request.constraints = constraints
req.ik_request.ik_link_name = l
req.ik_request.pose_stamped = p
res = srv(req)
if res.error_code.val is res.error_code.SUCCESS:
robot_state = res.solution
else:
rospy.logwarn("Unable to compute IK error code: %d",
res.error_code.val)
return None
return robot_state
@staticmethod
def get_group_for_ee(ee_name):
"""Ger name of the group for end effector name."""
return GROUP_FOR_EE[ee_name]
def _convert_link(self, pose, source_link, target_link):
"""Convert link to another link.
Note: The transformation between source and target link should be
fixed, but it is not checked.
Arguments:
pose -- Pose to be transformed.
source_link -- Link of the pose.
target_link -- Link where the pose should be tranformed.
Returns:
Pose for the target_link so that the source_link is in the orignal
pose.
"""
Ts = pose2affine(self.robot.get_link(source_link).pose().pose)
Tt = pose2affine(self.robot.get_link(target_link).pose().pose)
Tp = pose2affine(pose)
T = np.dot(np.linalg.inv(Tt), Ts)
p = affine2pose(np.dot(Tp, np.linalg.inv(T)))
return p
def _fix_link(self, pose, link):
"""Helper to fix link if other than tip link.
Arguments:
pose -- Pose for given link
link -- Link
Returns:
pose, link -- Fixed pose and link
"""
target_link = link
if link.startswith("r1"):
target_link = "r1_tip_link"
elif link.startswith("r2"):
target_link = "r2_tip_link"
elif link.startswith("xtion1"):
target_link = "xtion1_link_ee"
elif link.startswith("xtion2"):
target_link = "xtion2_link_ee"
if link is target_link:
return pose, link
else:
return self._convert_link(pose, link, target_link), target_link
def set_start_state(self, msg):
self._start_state = msg
MoveGroupCommander.set_start_state(self, msg)
def set_start_state_to_current_state(self):
self._start_state = None
MoveGroupCommander.set_start_state_to_current_state(self)
def execute(self, msg):
if self.overwrite_time_parameterization:
start_state = self._start_state if self._start_state else self.get_current_state(
)
group = self.get_name()
msg = add_time_parametrisation(msg, start_state, group)
if not msg.success:
msg = None
raise Exception("Unsucesfull time parametrisation.")
else:
msg = msg.parametrized_trajectory
elif msg.joint_trajectory.points[-1].time_from_start == 0:
raise Exception("The time from start is not set!")
self._start_state = None
MoveGroupCommander.execute(self, msg)
def async_execute(self, msg):
if self.overwrite_time_parameterization:
start_state = self._start_state if self._start_state else self.get_current_state(
)
group = self.get_name()
msg = add_time_parametrisation(msg, start_state, group)
if not msg.success:
msg = None
else:
msg = msg.parametrized_trajectory
elif msg.joint_trajectory.points[-1].time_from_start == 0:
raise Exception("The time from start is not set!")
self._start_state = None
services.execute(msg, wait=False, persistent=True)
def go(self, joints=None, wait=True):
self._start_state = None
MoveGroupCommander.go(self, joints, wait)
def _get_start_state(self):
if self._start_state is None:
return self.get_current_state()
else:
return self._start_state
def set_joint_value_target(self, arg1, arg2=None, arg3=None):
"""HotFix: See MoveGroupCommander for info."""
if (type(arg1) is PoseStamped) or (type(arg1) is Pose):
approx = False
eef = ""
if arg2 is not None:
if type(arg2) is str:
eef = arg2
else:
if type(arg2) is bool:
approx = arg2
else:
raise MoveItCommanderException("Unexpected type")
if arg3 is not None:
if type(arg3) is str:
eef = arg3
else:
if type(arg3) is bool:
approx = arg3
else:
raise MoveItCommanderException("Unexpected type")
# There is a problem that when the r1_ee or r2_ee are specified as
# eef the end effector ends up 0.1 m below the target position to
# fix this we first transform the pose to the tip_link and then call
# MoveGroupCommander
if self._ik_use_tip_link:
if type(arg1) is PoseStamped:
p, eef = self._fix_link(arg1.pose, eef)
arg1.pose = p
else:
arg1, eef = self._fix_link(arg1, eef)
MoveGroupCommander.set_joint_value_target(self, arg1, eef, approx)
else:
MoveGroupCommander.set_joint_value_target(self, arg1, arg2, arg3)
def compute_cartesian_path(self,
waypoints,
eefs,
eef_step=0.01,
jump_threshold=1.2,
avoid_collisions=True):
"""Compute cartesian path for one or two arms.
Arguments:
waypoints -- List of poses or list tuples
eefs -- End effector link or tuple of links
eef_step --
jump_threshold --
"""
# Wait for service an make proxi
if type(eefs) is tuple and len(eefs) > 1:
rospy.wait_for_service(SRV_CARTESIAN_PATH_DUAL)
srv = rospy.ServiceProxy(SRV_CARTESIAN_PATH_DUAL,
GetCartesianPathDual)
req = GetCartesianPathDualRequest()
req.start_state = self._get_start_state()
# req.robot_state = self._get_start_state()
req.group_name = self.get_name()
req.header.frame_id = self.get_pose_reference_frame()
req.header.stamp = rospy.Time.now()
waypoints1, waypoints2 = zip(*waypoints)
req.waypoints_1 = waypoints1
req.link_name_1 = eefs[0]
req.link_name_2 = eefs[1]
req.waypoints_2 = waypoints2
req.max_step = eef_step
req.jump_threshold = jump_threshold
req.avoid_collisions = avoid_collisions
if not avoid_collisions:
rospy.logwarn("!!!!Collisions are disabled!!!!")
res = srv.call(req)
if res.error_code.val is res.error_code.SUCCESS:
return (res.solution, res.fraction)
else:
rospy.logwarn(
"Unable to compute collsion free cartesian path! error code: %d",
res.error_code.val)
return None
else:
waypoints2, links = zip(
*[self._fix_link(wp, eefs) for wp in waypoints])
rospy.wait_for_service(SRV_CARTESIAN_PATH_DUAL)
srv = rospy.ServiceProxy(SRV_CARTESIAN_PATH_DUAL,
GetCartesianPathDual)
req = GetCartesianPathDualRequest()
req.start_state = self._get_start_state()
# req.robot_state = self._get_start_state()
req.group_name = self.get_name()
req.header.frame_id = self.get_pose_reference_frame()
req.header.stamp = rospy.Time.now()
req.waypoints_1 = waypoints2
req.link_name_1 = links[0]
req.link_name_2 = links[0]
req.waypoints_2 = waypoints2
req.max_step = eef_step
req.jump_threshold = jump_threshold
req.avoid_collisions = avoid_collisions
if not avoid_collisions:
rospy.logwarn("!!!!Collisions are disabled!!!!")
res = srv.call(req)
if res.error_code.val is res.error_code.SUCCESS:
print res.error_code.val, req.group_name, req.header.frame_id
return (res.solution, res.fraction)
else:
rospy.logwarn(
"Unable to compute collsion free cartesian path! error code: %d",
res.error_code.val)
return None
# return MoveGroupCommander.compute_cartesian_path(self, waypoints2, eef_step, jump_threshold, True)
def plan_between_joint_states(self, js1, js2):
"""Plan path between two joint states.
Arguments:
js1,js2 : {JointState message}
Returns:
trajectory : {RobotTrajectory message}
"""
tmp_robot_state = self._get_start_state()
rs = RobotState(copy.deepcopy(tmp_robot_state))
rs.update_from_joint_state(js1)
self.set_start_state(rs.msg)
self.set_joint_value_target(js2)
traj = self.plan()
self.set_start_state(tmp_robot_state)
if len(traj.joint_trajectory.joint_names) <= 0:
return None
else:
return traj
def display_planned_path(self, path):
"""Sends the trajectory to apropriate topic to be displayed.
Arguments:
path : {RobotTrajectory message} Trajectory to be displayed
"""
self.pub = rospy.Publisher(TOPIC_DISPLAY_PLANNED_PATH,
DisplayTrajectory)
dsp = DisplayTrajectory()
dsp.trajectory = [path]
self.pub.publish(dsp)
def set_path_constraints(self, value):
# In order to force planning in joint coordinates we add dummy joint
# constrains.
empty_joint_constraints = JointConstraint()
empty_joint_constraints.joint_name = "r1_joint_grip"
empty_joint_constraints.position = 0
empty_joint_constraints.tolerance_above = 1.5
empty_joint_constraints.tolerance_below = 1.5
empty_joint_constraints.weight = 1
value.joint_constraints.append(empty_joint_constraints)
MoveGroupCommander.set_path_constraints(self, value)
def grasp_from_table_plan(self,
poses,
tip="r1_ee",
table_desk="t3_desk",
final_poses=[],
base_frame="base_link",
offset_minus=0.02,
offset_plus=0.02,
table_minus=0,
table_plus=0.1,
grasping_angle=math.pi / 6):
"""
Grasp from table.
Arguments:
poses= [pos x, pos y, pos z, orien x, orien y orien z orien w]
tip = "r1_ee"
table_desk="t3_desk"
final_poses= optinoal
base_frame="base_link"
offset_minus = 0.02
offset_plus = 0.02
table_minus = 0
table_plus = 0.1
grasping_angle = math.pi/ 6
Returns:
trajectory
"""
rospy.wait_for_service(SRV_GRASP_FROM_TABLE)
srv = rospy.ServiceProxy(SRV_GRASP_FROM_TABLE, ClopemaGraspFromTable)
req = ClopemaGraspFromTableRequest()
req.frame_id = base_frame
req.ik_link = tip
req.table_desk = table_desk
req.poses = poses
req.offset_minus = offset_minus
req.offset_plus = offset_plus
req.offset_table_minus = table_minus
req.offset_table_plus = table_plus
req.grasping_angle = grasping_angle
req.final_poses = final_poses
tmp_robot_state = self._get_start_state()
self.set_start_state(tmp_robot_state)
res = srv.call(req)
if res.error is "":
return (res.joint_trajectories)
else:
rospy.logwarn("Unable to compute path! error code: %d", res.error)
return None
def grasp_from_table_dual_plan(self,
poses_1,
poses_2,
tip1="r1_ee",
tip2="r2_ee",
table_desk="t3_desk",
final_poses_1=[],
final_poses_2=[],
base_frame="base_link",
offset_minus=0.02,
offset_plus=0.02,
offset_table_minus=0,
offset_table_plus=0.1,
grasping_angle=math.pi / 6):
"""
Grasp from table dual.
Arguments:
poses_1, poses_2= [pos x, pos y, pos z, orien x, orien y orien z orien w]
tip1 = "r1_ee"
tip2= "r2_ee"
table_desk="t3_desk"
final_poses_1, final_poses_2= optinoal
base_frame="base_link"
offset_minus = 0.02
offset_plus = 0.02
table_minus = 0
table_plus = 0.1
grasping_angle = math.pi/ 6
Returns:
trajectory
"""
rospy.wait_for_service(SRV_GRASP_FROM_TABLE_DUAL)
srv = rospy.ServiceProxy(SRV_GRASP_FROM_TABLE_DUAL,
ClopemaGraspFromTableDual)
req = ClopemaGraspFromTableDualRequest()
req.frame_id = base_frame
req.ik_link_1 = tip1
req.ik_link_2 = tip2
req.table_desk = table_desk
req.poses_1 = poses_1
req.poses_2 = poses_2
req.offset_minus = offset_minus
req.offset_plus = offset_plus
req.offset_table_minus = offset_table_minus
req.offset_table_plus = offset_table_plus
req.grasping_angle = grasping_angle
req.final_poses_1 = final_poses_1
req.final_poses_2 = final_poses_2
tmp_robot_state = self._get_start_state()
self.set_start_state(tmp_robot_state)
res = srv.call(req)
if res.error is "":
return (res.joint_trajectories)
else:
rospy.logwarn("Unable to compute path! error code: %s", res.error)
return None
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 constraint 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('right_arm_zero')
# 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('right_arm_zero')
# 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 main():
rospy.init_node(sys.argv[1])
total_envs = 10
total_targets = int(
sys.argv[2]) #total number of collision free targets to save
limb = baxter_interface.Limb('right')
def_angles = limb.joint_angles()
limb.set_joint_position_speed(0.65)
#robot_state_collision_pub = rospy.Publisher('/robot_collision_state', DisplayRobotState, queue_size=0)
#scene = PlanningSceneInterface()
#scene._scene_pub = rospy.Publisher('planning_scene',
# PlanningScene,
# queue_size=0)
robot = RobotCommander()
group = MoveGroupCommander("right_arm")
rs_man = RobotState()
sv = StateValidity()
#set_environment(robot, scene)
masterModifier = ShelfSceneModifier()
#sceneModifier = PlanningSceneModifier(masterModifier.obstacles)
#sceneModifier.setup_scene(scene, robot, group)
robot_state = robot.get_current_state()
print(robot_state)
rs_man = RobotState() # constructed manually for comparison
rs_man.joint_state.name = robot_state.joint_state.name
filler_robot_state = list(robot_state.joint_state.position)
done = False
masterEnvDict = {}
envFileName = "testEnvironments_test1.pkl"
x_bounds = [[0.89, 1.13], [
-0.2, 1.13
]] #0 is the right half of the right table, 1 is the right table
y_bounds = [[-0.66, 0], [-0.9, -0.66]]
x_ranges = [
max(x_bounds[0]) - min(x_bounds[0]),
max(x_bounds[1]) - min(x_bounds[1])
]
y_ranges = [
max(y_bounds[0]) - min(y_bounds[0]),
max(y_bounds[1]) - min(y_bounds[1])
]
xy_bounds_dict = {
'x': x_bounds,
'y': y_bounds,
'x_r': x_ranges,
'y_r': y_ranges
}
iter_num = 0
#sceneModifier.delete_obstacles()
while (not rospy.is_shutdown() and not done):
for i_env in range(total_envs):
new_pose = masterModifier.permute_obstacles()
key_name = 'TrainEnv_' + str(i_env)
#sceneModifier.permute_obstacles(new_pose)
masterEnvDict[key_name] = {}
masterEnvDict[key_name]['ObsPoses'] = {}
masterEnvDict[key_name]['Targets'] = {}
masterEnvDict[key_name]['Targets']['Pose'] = []
masterEnvDict[key_name]['Targets']['Joints'] = []
for i_target in range(total_targets):
pose = sample_from_boundary(xy_bounds_dict)
check_pose = group.get_random_pose()
check_pose.pose.position.x = pose[0]
check_pose.pose.position.y = pose[1]
check_pose.pose.position.z = pose[2]
joint_angles = ik_test(check_pose)[1]
filler_robot_state[10:17] = moveit_scrambler(
joint_angles.values())
rs_man.joint_state.position = tuple(filler_robot_state)
while (ik_test(check_pose)[0]
or not sv.getStateValidity(rs_man,
group_name="right_arm")):
pose = sample_from_boundary(xy_bounds_dict)
check_pose = group.get_random_pose()
check_pose.pose.position.x = pose[0]
check_pose.pose.position.y = pose[1]
check_pose.pose.position.z = pose[2]
if (not ik_test(check_pose)[0]):
joint_angles = ik_test(check_pose)[1]
filler_robot_state[10:17] = moveit_scrambler(
joint_angles.values())
rs_man.joint_state.position = tuple(filler_robot_state)
joint_angles = ik_test(check_pose)[1]
masterEnvDict[key_name]['Targets']['Pose'].append(check_pose)
masterEnvDict[key_name]['Targets']['Joints'].append(
joint_angles)
#sceneModifier.delete_obstacles()
masterEnvDict[key_name]['ObsPoses'] = new_pose
iter_num += 1
with open(envFileName, "wb") as env_f:
pickle.dump(masterEnvDict, env_f)
print("Done saving... exiting loop \n\n\n")
done = True
#
# Author: Ioan Sucan
import sys
import rospy
from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
robot = RobotCommander()
rospy.sleep(1)
print "Current state:"
print robot.get_current_state()
# plan to a random location
a = robot.right_arm
a.set_start_state(RobotState())
r = a.get_random_joint_values()
print "Planning to random joint position: "
print r
p = a.plan(r)
print "Solution:"
print p
roscpp_shutdown()
class Approach(object):
def __init__(self, name, takeoff_height):
self.robot_name = name
self.takeoff_height = takeoff_height
# Mutual exclusion
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
# Create trajectory server
self.trajectory_server = SimpleActionServer(
'approach_server', ExecuteDroneApproachAction, self.goCallback,
False)
self.server_feedback = ExecuteDroneApproachFeedback()
self.server_result = ExecuteDroneApproachResult()
# Get client from hector_quadrotor_actions
self.move_client = SimpleActionClient("/{}/action/pose".format(name),
PoseAction)
self.move_client.wait_for_server()
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry,
self.poseCallback)
# Subscribe to topic to receive the planned trajectory
rospy.Subscriber("/{}/move_group/display_planned_path".format(name),
DisplayTrajectory, self.planCallback)
#Auxiliary variables
self.trajectory = [] # Array with the trajectory to be executed
self.trajectory_received = False # Flag to signal trajectory received
self.odom_received = False # Flag to signal odom received
self.robot = RobotCommander(
robot_description="{}/robot_description".format(name),
ns="/{}".format(name))
self.display_trajectory_publisher = rospy.Publisher(
'/{}/move_group/display_planned_path'.format(name),
DisplayTrajectory,
queue_size=20)
# Variables for collision callback
self.validity_srv = rospy.ServiceProxy(
'/{}/check_state_validity'.format(name), GetStateValidity)
self.validity_srv.wait_for_service()
self.collision = False
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
#Start move_group
self.move_group = MoveGroup('earth', name)
self.move_group.set_planner(planner_id='RRTConnectkConfigDefault',
attempts=10,
allowed_time=2) #RRTConnectkConfigDefault
self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX,
ZMAX]) # Set the workspace size
# Get current robot position to define as start planning point
self.current_pose = self.robot.get_current_state()
#Start planningScenePublisher
self.scene_pub = PlanningScenePublisher(name, self.current_pose)
# Start trajectory server
self.trajectory_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.notify()
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
# print(self.odometry)
self.odometry_me.release()
self.odom_received = True
def goCallback(self, pose):
'''
Require a plan to go to the desired target and try to execute it 5 time or return erro
'''
self.target = pose.goal
####################################################################
# First takeoff if the drone is close to ground
self.sonar_me.acquire()
while not (self.current_height and self.odometry):
self.sonar_me.wait()
if self.current_height < self.takeoff_height:
self.odometry_me.acquire()
takeoff_pose = PoseGoal()
takeoff_pose.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
takeoff_pose.target_pose.pose.position.x = self.odometry.position.x
takeoff_pose.target_pose.pose.position.y = self.odometry.position.y
takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height
takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.odometry_me.release()
self.move_client.send_goal(takeoff_pose)
self.move_client.wait_for_result()
result = self.move_client.get_state()
if result == GoalStatus.ABORTED:
rospy.logerr("Abort approach! Unable to execute takeoff!")
self.trajectory_server.set_aborted()
return
self.sonar_me.release()
####################################################################
rospy.loginfo("Try to start from [{},{},{}]".format(
self.odometry.position.x, self.odometry.position.y,
self.odometry.position.z))
rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x,
self.target.position.y,
self.target.position.z))
self.trials = 0
while self.trials < 5:
rospy.logwarn("Attempt {}".format(self.trials + 1))
if (self.trials > 1):
self.target.position.z += 2 * rd() - 1
result = self.go(self.target)
if (result == 'replan') or (result == 'no_plan'):
self.trials += 1
else:
self.trials = 10
self.collision = False
if result == 'ok':
self.trajectory_server.set_succeeded()
elif (result == 'preempted'):
self.trajectory_server.set_preempted()
else:
self.trajectory_server.set_aborted()
def go(self, target_):
'''
Function to plan and execute the trajectory one time
'''
# Insert goal position on an array
target = []
target.append(target_.position.x)
target.append(target_.position.y)
target.append(target_.position.z)
target.append(target_.orientation.x)
target.append(target_.orientation.y)
target.append(target_.orientation.z)
target.append(target_.orientation.w)
#Define target for move_group
# self.move_group.set_joint_value_target(target)
self.move_group.set_target(target)
self.odometry_me.acquire()
self.current_pose.multi_dof_joint_state.transforms[
0].translation.x = self.odometry.position.x
self.current_pose.multi_dof_joint_state.transforms[
0].translation.y = self.odometry.position.y
self.current_pose.multi_dof_joint_state.transforms[
0].translation.z = self.odometry.position.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.x
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.y
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.w
self.odometry_me.release()
#Set start state
self.move_group.set_start_state(self.current_pose)
# Plan a trajectory till the desired target
plan = self.move_group.plan()
if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory
while (not self.trajectory_received):
rospy.loginfo("Waiting for trajectory!")
rospy.sleep(0.2)
# rospy.loginfo("TRAJECTORY: {}".format(self.trajectory))
#Execute trajectory with action_pose
last_pose = self.trajectory[0]
for pose in self.trajectory:
# Verify preempt call
if self.trajectory_server.is_preempt_requested():
self.move_client.send_goal(last_pose)
self.move_client.wait_for_result()
self.scene_pub.publishScene()
self.trajectory_received = False
self.odom_received = False
return 'preempted'
#Send next pose to move
self.next_pose = pose.target_pose.pose
self.move_client.send_goal(pose,
feedback_cb=self.collisionCallback)
self.move_client.wait_for_result()
result = self.move_client.get_state()
self.scene_pub.publishScene()
# Abort if the drone can not reach the position
if result == GoalStatus.ABORTED:
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'aborted'
elif result == GoalStatus.PREEMPTED:
# last_pose.target_pose.pose.position.z += rd() - 0.5
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'replan'
elif result == GoalStatus.SUCCEEDED:
self.trials = 0
last_pose = pose
self.server_feedback.current_pose = self.odometry
self.trajectory_server.publish_feedback(self.server_feedback)
# Reset control variables
self.trajectory_received = False
self.odom_received = False
rospy.loginfo("Trajectory is traversed!")
return 'ok'
else:
rospy.logerr("Trajectory is empty. Planning was unsuccessful.")
return 'no_plan'
def planCallback(self, msg):
'''
Receive planned trajectories and insert it into an array of waypoints
'''
if (not self.odom_received):
return
# Variable to calculate the distance difference between 2 consecutive points
last_pose = PoseGoal()
last_pose.target_pose.pose.position.x = self.odometry.position.x
last_pose.target_pose.pose.position.y = self.odometry.position.y
last_pose.target_pose.pose.position.z = self.odometry.position.z
last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.trajectory = []
last_motion_theta = 0
for t in msg.trajectory:
for point in t.multi_dof_joint_trajectory.points:
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
# Orientate the robot always to the motion direction
delta_x = point.transforms[
0].translation.x - last_pose.target_pose.pose.position.x
delta_y = point.transforms[
0].translation.y - last_pose.target_pose.pose.position.y
motion_theta = atan2(delta_y, delta_x)
last_motion_theta = motion_theta
# Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION
# if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION):
q = quaternion_from_euler(0, 0, motion_theta)
waypoint.target_pose.pose.orientation.x = q[0]
waypoint.target_pose.pose.orientation.y = q[1]
waypoint.target_pose.pose.orientation.z = q[2]
waypoint.target_pose.pose.orientation.w = q[3]
# else:
# waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x
# waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y
# waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z
# waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w
# Add a rotation inplace if next position has an angle difference bigger than 45
if abs(motion_theta - last_motion_theta) > 0.785:
last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation
self.trajectory.append(last_pose)
last_pose = copy.copy(
waypoint) # Save pose to calc the naxt delta
last_motion_theta = motion_theta
self.trajectory.append(waypoint)
#Insert a last point to ensure that the robot end at the right position
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
waypoint.target_pose.pose.orientation.x = point.transforms[
0].rotation.x
waypoint.target_pose.pose.orientation.y = point.transforms[
0].rotation.y
waypoint.target_pose.pose.orientation.z = point.transforms[
0].rotation.z
waypoint.target_pose.pose.orientation.w = point.transforms[
0].rotation.w
self.trajectory.append(waypoint)
self.trajectory_received = True
def collisionCallback(self, feedback):
'''
This callback runs on every feedback message received
'''
validity_msg = GetStateValidityRequest(
) # Build message to verify collision
validity_msg.group_name = PLANNING_GROUP
if self.next_pose and (not self.collision):
self.odometry_me.acquire()
x = self.odometry.position.x
y = self.odometry.position.y
z = self.odometry.position.z
# Distance between the robot and the next position
dist = sqrt((self.next_pose.position.x - x)**2 +
(self.next_pose.position.y - y)**2 +
(self.next_pose.position.z - z)**2)
# Pose to verify collision
pose = Transform()
pose.rotation.x = self.odometry.orientation.x
pose.rotation.y = self.odometry.orientation.y
pose.rotation.z = self.odometry.orientation.z
pose.rotation.w = self.odometry.orientation.w
self.odometry_me.release()
#Verify possible collisions on diferent points between the robot and the goal point
# rospy.logerr("\n\n\nCOLLISION CALLBACK: ")
# rospy.logerr(dist)
for d in arange(RESOLUTION, dist + 0.5, RESOLUTION):
pose.translation.x = (self.next_pose.position.x -
x) * (d / dist) + x
pose.translation.y = (self.next_pose.position.y -
y) * (d / dist) + y
pose.translation.z = (self.next_pose.position.z -
z) * (d / dist) + z
self.current_pose.multi_dof_joint_state.transforms[
0] = pose # Insert the correct odometry value
validity_msg.robot_state = self.current_pose
# Call service to verify collision
collision_res = self.validity_srv.call(validity_msg)
# print("\nCollision response:")
# print(collision_res)
# Check if robot is in collision
if not collision_res.valid:
# print(validity_msg)
rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format(
pose.translation.x, pose.translation.y,
pose.translation.z))
rospy.logerr('Current pose [x:{} y:{} z:{}]'.format(
x, y, z))
# print(collision_res)
self.move_client.cancel_goal()
self.collision = True
return
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)
import sys
import rospy
import moveit_commander
import moveit_msgs.msg
from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
robot = RobotCommander()
rospy.sleep(1)
print "Current state:"
print robot.get_current_state()
# plan to a random location
a = robot.arm
while not rospy.is_shutdown():
a.set_start_state(RobotState())
r = a.get_random_joint_values()
p = a.plan(r)
a.execute(p)
rospy.sleep(10)
# print "Planning to random joint position: "
# print r
# print "Solution:"
# print p
class Baxter(object):
def __init__(self):
# create a RobotCommander
self.robot = RobotCommander()
# create a PlanningScene Interface object
self.scene = PlanningSceneInterface()
# create left arm move group and gripper
self.left_arm = Arm("left_arm")
self.left_gripper = self.left_arm.gripper = Gripper("left")
# create right arm move group and gripper
self.right_arm = Arm("right_arm")
self.right_gripper = self.right_arm.gripper = Gripper("right")
self.gripper_offset = 0.0
# reset robot
self.reset()
def reset(self, ):
"""
Reset robot state and calibrate grippers
"""
# enable robot
self.enable()
# move to ready position
joint_angles = [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0]
self.move_to_joints("right_arm", joint_angles, blocking=False)
self.move_to_joints("left_arm", joint_angles, blocking=True)
# calibrate grippers
self.calibrate_grippers()
return
def enable(self):
"""
Enable robot
"""
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
def calibrate_grippers(self):
"""
Calibrate grippers
"""
if not self.left_gripper.calibrated():
self.left_gripper.calibrate()
if not self.right_gripper.calibrated():
self.right_gripper.calibrate()
return
def get_robot_state(self):
"""
Get robot state
Returns
ros msg (moveit_msgs.msg._RobotState.RobotState) - A message that contains
information about all of the joints, including fixed head pan,
revolute arm joints, and prismatic finger joints
"""
return self.robot.get_current_state()
def get_group_names(self):
"""
Get group names
Returns
group names (list): a list of move groups that can be used for planning
"""
return self.robot.get_group_names()
def pick(self):
return
def place(self):
return
class Arm():
default_planner = "BKPIECEkConfigDefault"
link_type_map = {
LinkType.INTERMEDIATE: IntermediateLink,
LinkType.TERMINAL: TerminalLink
}
def __init__(self,
planner=default_planner,
links={"ee": (LinkType.INTERMEDIATE, "manipulator")}):
# initialize interface node (if needed)
if rospy.get_node_uri() is None:
self.node_name = 'ur5_interface_node'
rospy.init_node(self.node_name)
ROS_INFO(
"The Arm interface was created outside a ROS node, a new node will be initialized!"
)
else:
ROS_INFO(
"The Arm interface was created within a ROS node, no need to create a new one!"
)
# store parameters
self._planner = planner
self._links = links
# create a RobotCommander
self._robot = RobotCommander()
# create semaphore
self._semaphore = BoundedSemaphore(value=1)
# default link (for utility functions only)
self._default_link = None
# create links objects
num_valid_links = 0
for link in self._links.items():
link_name, link_description = link
link_type, link_group_name = link_description
link_class = Arm.link_type_map[link_type]
link_obj = link_class(self, link_group_name, self._planner)
if not self._robot.has_group(link_group_name):
ROS_ERR("Move group `%s` not found!", link_group_name)
else:
self.__dict__[link_name] = link_obj
self._default_link = link_obj
num_valid_links += 1
if num_valid_links <= 0:
ROS_ERR("No valid links were found")
return None
# keep track of the status of the node
self._is_shutdown = False
self.verbose = False
def get_robot_state(self):
return self._robot.get_current_state()
def get_group_names(self):
return self._robot.get_group_names()
def get_planning_frame(self):
return self._default_link._group.get_planning_frame()
def get_joint_values(self):
return self._default_link._group.get_current_joint_values()
def plan_to_joint_config(self, joint_angles):
''' Plan to a given joint configuration '''
# Clear old pose targets
self._default_link._group.clear_pose_targets()
# Set Joint configuration target
self._default_link._group.set_joint_value_target(joint_angles)
numTries = 0
while numTries < 5:
success, plan, _, _ = self._default_link._group.plan()
numTries += 1
if success:
if self.verbose: print('succeeded in %d tries' % numTries)
return True, plan
if self.verbose: print("Planning failed")
return False, None
def execute_plan(self, plan):
''' Execute a given plan '''
self._semaphore.acquire()
out = self._default_link._group.execute(plan)
sleep(0.05)
self._semaphore.release()
return out
def _plan_to_pose(self, group, pose):
''' Plan to a given pose for the end-effector '''
# Clear old pose targets
group.clear_pose_targets()
constr = group.get_path_constraints()
if len(constr.orientation_constraints) > 0:
pose.orientation = constr.orientation_constraints[0].orientation
# Set target pose
group.set_pose_target(pose)
numTries = 0
while numTries < 5:
success, plan, _, _ = group.plan()
numTries += 1
# check that planning succeeded
if success:
if self.verbose: print('succeeded in %d tries' % numTries)
return True, plan
# if we get here, we couldn't find a plan in max number of tries
if self.verbose: print('Planning failed')
return False, None
def _plan_cartesian_path(self, group, waypoints):
numTries = 0
while numTries < 5:
success, plan, _, _ = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0 # jump_threshold
)
numTries += 1
# check that planning succeeded
if success:
if self.verbose: print('succeeded in %d tries' % numTries)
return True, plan
# if we get here, we couldn't find a plan in max number of tries
return False, None
def _execute_plan(self, group, plan):
''' Execute a given plan '''
self._semaphore.acquire()
out = group.execute(plan)
self._semaphore.release()
return out
def shutdown(self):
print('Shutting down Arm...')
self._is_shutdown = True
print('Arm released!')
return True
def main(args):
rospy.init_node('path_data_gen')
# sometimes takes a few tries to connect to robot arm
limb_init = False
while not limb_init:
try:
limb = baxter_interface.Limb('right')
limb_init = True
except OSError:
limb_init = False
neutral_start = limb.joint_angles()
min_goal_cost_threshold = 2.0
# set speed to make sure execution does not violate the speed constraint
limb.set_joint_position_speed(0.65)
# Set up planning scene and Move Group objects
robot = RobotCommander()
group = MoveGroupCommander("right_arm")
sv = StateValidity()
# Setup tables (geometry and location defined in moveit_functions.py, set_environment function)
# set_environment(robot, scene)
# Additional Move group setup
# group.set_goal_joint_tolerance(0.001)
# group.set_max_velocity_scaling_factor(0.7)
# group.set_max_acceleration_scaling_factor(0.1)
max_time = args.max_time
group.set_planning_time(max_time)
# Dictionary to save path data, and filename to save the data to in the end
pathsDict = {}
if not os.path.exists(args.path_data_path):
os.makedirs(args.path_data_path)
pathsFile = args.path_data_path + args.path_data_file
# load data from environment files for obstacle locations and collision free goal poses
# with open("env/environment_data/trainEnvironments_GazeboPatch.pkl", "rb") as env_f:
with open(args.env_data_path + args.env_data_file, "rb") as env_f:
envDict = pickle.load(env_f)
# with open("env/environment_data/trainEnvironments_testGoals.pkl", "rb") as goal_f:
#with open(args.targets_data_path+args.targets_data_file, "rb") as goal_f:
# goalDict = pickle.load(goal_f)
# Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt
robot_state = robot.get_current_state()
rs_man = RobotState()
rs_man.joint_state.name = robot_state.joint_state.name
filler_robot_state = list(robot_state.joint_state.position)
goal_sampler = GoalSampler(group, limb, robot_state, sv)
# Here we go
# slice this if you want only some environments
test_envs = envDict['poses'].keys()
done = False
iter_num = 0
print("Testing envs: ")
print(test_envs)
joint_state_topic = ['joint_states:=/robot/joint_states']
roscpp_initialize(joint_state_topic)
rospy.init_node("path_data_gen")
#roscpp_initialize(sys.argv)
# parameters for loading point cloud
executable = './pcd_to_pointcloud'
rootPath = '../data/pcd/'
rospy.wait_for_service('clear_octomap')
clear_octomap = rospy.ServiceProxy('clear_octomap', Empty)
respond = clear_octomap()
print(group.get_end_effector_link())
# try moving the robot to current joints
#group.go(joints=group.get_current_joint_values())
while (not rospy.is_shutdown() and not done):
for i_env, env_name in enumerate(test_envs):
print("env iteration number: " + str(i_env))
print("env name: " + str(env_name))
# load point cloud saved
name = ''
for file in sorted(
os.listdir(args.pcd_data_path),
key=lambda x: int(x.split('Env_')[1].split('_')[1][:-4])):
if (fnmatch.fnmatch(file, env_name + "*")):
# if found the first one that matches the name env+something, then record it
name = file
break
call = create_call(executable, rootPath, name)
### Printing the executable call and allowing user to manually cycle through environments for demonstration
print(call)
### Uncomment below to call pointcloud_to_pcd executable which takes snapshot of the streaming pointcloud data
### and saves it to a .pcd file in a desired file location (as specified by prefix in the command call)
print("Calling executable... \n\n\n")
t = time.time()
#stderr_f = open('log.txt', 'w')
# publishing topic of point cloud for planning
# wait for some time to make sure the point cloud is loaded corretly
p = subprocess.Popen(call) #, stderr=stderr_f)
rospy.sleep(10)
new_pose = envDict['poses'][env_name]
pathsDict[env_name] = {}
pathsDict[env_name]['paths'] = []
pathsDict[env_name]['costs'] = []
pathsDict[env_name]['times'] = []
pathsDict[env_name]['total'] = 0
pathsDict[env_name]['feasible'] = 0
#collision_free_goals = goalDict[env_name]['Joints']
total_paths = 0
feasible_paths = 0
i_path = 0
#group.go(joints=group.get_current_joint_values())
# run until either desired number of total or feasible paths has been found
while (total_paths < args.paths_to_save):
#do planning and save data
# some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table
# by only taking goals which have a straight path cost above a threshold
valid_goal = False
print("FP: " + str(feasible_paths))
print("TP: " + str(total_paths))
total_paths += 1
i_path += 1
# Uncomment below if using a start state different than the robot current state
# filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff
# filler_robot_state[10:17] = moveit_scrambler(start.values())
# rs_man.joint_state.position = tuple(filler_robot_state)
# group.set_start_state(rs_man) # set start
pos = []
while not valid_goal:
pose, joint = goal_sampler.sample()
goal = joint
optimal_path = [neutral_start.values(), goal.values()]
optimal_cost = compute_cost(optimal_path)
if optimal_cost > min_goal_cost_threshold:
valid_goal = True
group.set_start_state_to_current_state()
group.clear_pose_targets()
try:
group.set_joint_value_target(
moveit_scrambler(goal.values())) # set target
except MoveItCommanderException as e:
print(e)
continue
start_t = time.time()
plan = group.plan()
t = time.time() - start_t
#group.execute(plan)
pos = [
plan.joint_trajectory.points[i].positions
for i in range(len(plan.joint_trajectory.points))
]
if pos != []:
pos = np.asarray(pos)
cost = compute_cost(pos)
print("Time: " + str(t))
print("Cost: " + str(cost))
# Uncomment below if using max time as criteria for failure
if (t > (max_time * 0.99)):
print("Reached max time...")
continue
feasible_paths += 1
pathsDict[env_name]['paths'].append(pos)
pathsDict[env_name]['costs'].append(cost)
pathsDict[env_name]['times'].append(t)
pathsDict[env_name]['feasible'] = feasible_paths
pathsDict[env_name]['total'] = total_paths
# Uncomment below if you want to overwrite data on each new feasible path
with open(pathsFile + "_" + env_name + ".pkl",
"wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("\n")
p.terminate()
p.wait()
# rosservice call to clear octomap
rospy.wait_for_service('clear_octomap')
clear_octomap = rospy.ServiceProxy('clear_octomap', Empty)
respond = clear_octomap()
iter_num += 1
print("Env: " + str(env_name))
print("Feasible Paths: " + str(feasible_paths))
print("Total Paths: " + str(total_paths))
print("\n")
pathsDict[env_name]['total'] = total_paths
pathsDict[env_name]['feasible'] = feasible_paths
with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("Done iterating, saving all data and exiting...\n\n\n")
with open(pathsFile + ".pkl", "wb") as path_f:
pickle.dump(pathsDict, path_f)
done = True
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.target_ar_id = 9
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
"""
move to initial pose of the UR3
"""
#self.clear_octomap()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
-0.535054565144069, -2.009213503260451, 1.8350906250920112,
-0.7794355413099039, -0.7980899690645948, 0.7782740454087982
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def move_moveit_setting_pose(self, pose_name):
"""
move to one of the moveit_setup pose: "home", "zeros", "table"
"""
if pose_name == "home":
self.robot_arm.set_named_target("home")
elif pose_name == "zeros":
self.robot_arm.set_named_target("zeros")
elif pose_name == "table":
self.robot_arm.set_named_target("table")
#print "Press the Enter"
#raw_input()
self.robot_arm.go(wait=True)
def go_to_move(self, scale=1.0):
"""
manipulator is moving to the target ar_marker. \n
the start ar_marker id is 9, goal ar_marker id is 10.
"""
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
coke_offset = [0, -0.35, -0.1] #x y z
# gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품.
# linear offset = abs([0, 0.0625, 0.13])
robot_base_offset = 0.873
base_wrist2_offset = 0.1 #for avoiding link contact error
if self.target_ar_id == 9:
print ">> robot arm plannig frame: \n", planning_frame
print ">> move mode id: ", self.target_ar_id
self.calculed_coke_pose.position.x = (
scale * self.goal_x) # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale *
self.goal_y) + coke_offset[1]
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 1.57))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
elif self.target_ar_id == 10:
print ">> robot arm plannig frame: \n", planning_frame
print ">> move mode id: ", self.target_ar_id
self.calculed_coke_pose.position.x = (scale *
self.goal_x) + coke_offset[1]
self.calculed_coke_pose.position.y = (scale * self.goal_y) + 0
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 0))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [
self.calculed_coke_pose.position.x,
self.calculed_coke_pose.position.y,
self.calculed_coke_pose.position.z
]
tf_display_orientation = [
self.calculed_coke_pose.orientation.x,
self.calculed_coke_pose.orientation.y,
self.calculed_coke_pose.orientation.z,
self.calculed_coke_pose.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
coke_waypoints = []
coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose))
(coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(
coke_waypoints, 0.01, 0.0)
self.display_trajectory(coke_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch,
yaw):
"""
Manipulator is moving to the desired coordinate. \n
Now move to the ar_10 marker.
"""
calculed_ar_id_10 = Pose()
#desired_goal_pose = [0.171, -0.113, 1.039]
#desired_goal_euler = [3.14, 0.17, 0]
desired_goal_pose = [pose_x, pose_y, pose_z]
desired_goal_euler = [roll, pitch, yaw]
Cplanning_frame = self.robot_arm.get_planning_frame()
print ">> current planning frame: \n", Cplanning_frame
calculed_ar_id_10.position.x = desired_goal_pose[0] + 0.1
calculed_ar_id_10.position.y = desired_goal_pose[1]
calculed_ar_id_10.position.z = desired_goal_pose[2]
calculed_ar_id_10.orientation = Quaternion(*quaternion_from_euler(
desired_goal_euler[0], desired_goal_euler[1],
desired_goal_euler[2]))
print ">>> ar id 10 goal frame: ", desired_goal_pose
self.robot_arm.set_pose_target(calculed_ar_id_10)
tf_display_position = [
calculed_ar_id_10.position.x, calculed_ar_id_10.position.y,
calculed_ar_id_10.position.z
]
tf_display_orientation = [
calculed_ar_id_10.orientation.x, calculed_ar_id_10.orientation.y,
calculed_ar_id_10.orientation.z, calculed_ar_id_10.orientation.w
]
jj = 0
while jj < 5:
jj += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
ar_id_10_waypoints = []
ar_id_10_waypoints.append(copy.deepcopy(calculed_ar_id_10))
(ar_id_10_plan,
ar_id_10_fraction) = self.robot_arm.compute_cartesian_path(
ar_id_10_waypoints, 0.01, 0.0)
self.display_trajectory(ar_id_10_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
"""
Visualized trajectory of the manipulator
"""
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0):
"""
Working on the "linear move": x-axis -> y-axis -> z-axis
"""
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
#print "waypoints:", waypoints
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
"""
Working on the "linear move": x-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
"""
Working on the "linear move": y-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
"""
Working on the "linear move": z-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.z = (scale *
self.wpose.position.z) + z_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
"""
execute planned "plan" trajectory.
"""
group = self.robot_arm
group.execute(plan, wait=True)
def ar_pose_subscriber(self):
"""
ar_maker subscriber
"""
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
#print "======= pos(meter): ", self.position_list
#print "======= orientation: ", self.orientation_list
def ar_tf_listener(self, msg):
"""
Listening the ar_marker pose coordinate.
"""
try:
self.marker = msg.markers
ml = len(self.marker)
target_start_point_id = self.target_ar_id
#target_id = target_ar_id
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_start_point_id:
pass
#target_id_flage = False
elif self.m_idd == target_start_point_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[
target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[
target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[
target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[
target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[
target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[
target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[
target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.ar_pose.orientation.x, self.ar_pose.orientation.y,
self.ar_pose.orientation.z, self.ar_pose.orientation.w
]
(self.goal_roll,
self.goal_pitch, self.goal_yaw) = euler_from_quaternion(
self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
try:
rospy.init_node("ik_node")
robot = RobotCommander()
group_name = "manipulator"
group = MoveGroupCommander(group_name)
connection = rospy.ServiceProxy("/compute_ik", GetPositionIK)
rospy.loginfo("waiting for ik server")
connection.wait_for_service()
rospy.loginfo("server found")
request = GetPositionIKRequest()
request.ik_request.group_name = group_name
request.ik_request.ik_link_name = group.get_end_effector_link()
request.ik_request.ik_link_names = robot.get_link_names(group_name)
request.ik_request.robot_state = robot.get_current_state()
# get cartesian state
cartesian = raw_input("enter cartesian state x y z r p y \n")
[x, y, z, roll, pitch,
yaw] = [float(idx) for idx in cartesian.split(' ')]
request.ik_request.pose_stamped.pose.position.x = x
request.ik_request.pose_stamped.pose.position.y = y
request.ik_request.pose_stamped.pose.position.z = z
quat = quaternion_from_euler(roll, pitch, yaw)
request.ik_request.pose_stamped.pose.orientation.x = quat[0]
request.ik_request.pose_stamped.pose.orientation.y = quat[1]
request.ik_request.pose_stamped.pose.orientation.z = quat[2]
request.ik_request.pose_stamped.pose.orientation.w = quat[3]
def main():
rospy.init_node(sys.argv[1])
# sometimes takes a few tries to connect to robot arm
limb_init = False
while not limb_init:
try:
limb = baxter_interface.Limb('right')
limb_init = True
except OSError:
limb_init = False
neutral_start = limb.joint_angles()
min_goal_cost_threshold = 2.0
# Set up planning scene and Move Group objects
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher(
'planning_scene', PlanningScene, queue_size=0)
robot = RobotCommander()
group = MoveGroupCommander("right_arm")
sv = StateValidity()
set_environment(robot, scene)
# Setup tables (geometry and location defined in moveit_functions.py, set_environment function)
# set_environment(robot, scene)
# Additional Move group setup
# group.set_goal_joint_tolerance(0.001)
# group.set_max_velocity_scaling_factor(0.7)
# group.set_max_acceleration_scaling_factor(0.1)
max_time = 300
group.set_planning_time(max_time)
# Dictionary to save path data, and filename to save the data to in the end
pathsDict = {}
pathsFile = "data/path_data_example"
# load data from environment files for obstacle locations and collision free goal poses
with open("env/trainEnvironments.pkl", "rb") as env_f:
envDict = pickle.load(env_f)
with open("env/trainEnvironments_testGoals.pkl", "rb") as goal_f:
goalDict = pickle.load(goal_f)
# Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt
sceneModifier = PlanningSceneModifier(envDict['obsData'])
sceneModifier.setup_scene(scene, robot, group)
robot_state = robot.get_current_state()
rs_man = RobotState()
rs_man.joint_state.name = robot_state.joint_state.name
# Here we go
test_envs = envDict['poses'].keys() #slice this if you want only some environments
done = False
iter_num = 0
print("Testing envs: ")
print(test_envs)
while(not rospy.is_shutdown() and not done):
for i_env, env_name in enumerate(test_envs):
print("env iteration number: " + str(i_env))
print("env name: " + str(env_name))
sceneModifier.delete_obstacles()
new_pose = envDict['poses'][env_name]
sceneModifier.permute_obstacles(new_pose)
print("Loaded new pose and permuted obstacles\n")
pathsDict[env_name] = {}
pathsDict[env_name]['paths'] = []
pathsDict[env_name]['costs'] = []
pathsDict[env_name]['times'] = []
pathsDict[env_name]['total'] = 0
pathsDict[env_name]['feasible'] = 0
collision_free_goals = goalDict[env_name]['Joints']
total_paths = 0
feasible_paths = 0
i_path = 0
while (total_paths < 30): #run until either desired number of total or feasible paths has been found
#do planning and save data
# some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table
# by only taking goals which have a straight path cost above a threshold
valid_goal = False
while not valid_goal:
goal = collision_free_goals[np.random.randint(0, len(collision_free_goals))]
optimal_path = [neutral_start.values(), goal.values()]
optimal_cost = compute_cost(optimal_path)
if optimal_cost > min_goal_cost_threshold:
valid_goal = True
print("FP: " + str(feasible_paths))
print("TP: " + str(total_paths))
total_paths += 1
i_path += 1
# Uncomment below if using a start state different than the robot current state
# filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff
# filler_robot_state[10:17] = moveit_scrambler(start.values())
# rs_man.joint_state.position = tuple(filler_robot_state)
# group.set_start_state(rs_man) # set start
group.set_start_state_to_current_state()
group.clear_pose_targets()
try:
group.set_joint_value_target(moveit_scrambler(goal.values())) # set target
except MoveItCommanderException as e:
print(e)
continue
start_t = time.time()
plan = group.plan()
pos = [plan.joint_trajectory.points[i].positions for i in range(len(plan.joint_trajectory.points))]
if pos != []:
pos = np.asarray(pos)
cost = compute_cost(pos)
t = time.time() - start_t
print("Time: " + str(t))
print("Cost: " + str(cost))
# Uncomment below if using max time as criteria for failure
if (t > (max_time*0.99)):
print("Reached max time...")
continue
feasible_paths += 1
pathsDict[env_name]['paths'].append(pos)
pathsDict[env_name]['costs'].append(cost)
pathsDict[env_name]['times'].append(t)
pathsDict[env_name]['feasible'] = feasible_paths
pathsDict[env_name]['total'] = total_paths
# Uncomment below if you want to overwrite data on each new feasible path
with open (pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("\n")
sceneModifier.delete_obstacles()
iter_num += 1
print("Env: " + str(env_name))
print("Feasible Paths: " + str(feasible_paths))
print("Total Paths: " + str(total_paths))
print("\n")
pathsDict[env_name]['total'] = total_paths
pathsDict[env_name]['feasible'] = feasible_paths
with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f:
pickle.dump(pathsDict[env_name], path_f)
print("Done iterating, saving all data and exiting...\n\n\n")
with open(pathsFile + ".pkl", "wb") as path_f:
pickle.dump(pathsDict, path_f)
done = True
class trajectoryConstructor():
def __init__(self):
rospy.loginfo("Waiting for service " + IK_SERVICE_NAME)
rospy.wait_for_service(IK_SERVICE_NAME)
self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK)
self.joint_list = ['torso_1_joint', 'torso_2_joint',
'head_1_joint', 'head_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint',
'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.left_arm = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint']
self.right_arm = ['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.right_arm_torso = ['torso_1_joint', 'torso_2_joint',
'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.left_arm_torso = ['torso_1_joint', 'torso_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint']
# # Store answer so we ask only one time
# self.joint_state = JointState()
# self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names
# self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names)
# self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0]
self.r_commander = RobotCommander()
self.initial_robot_state = self.r_commander.get_current_state()
if DEBUG_MODE:
self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list', MarkerArray, latch=True)
self.ok_markers = MarkerArray()
self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list', MarkerArray, latch=True)
self.fail_markers = MarkerArray()
self.markers_id = 5
def getIkPose(self, pose, group="right_arm", previous_state=None):
"""Get IK of the pose specified, for the group specified, optionally using
the robot_state of previous_state (if not, current robot state will be requested) """
# point point to test if there is ik
# returns the answer of the service
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = group
rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now())
rqst.ik_request.pose_stamped.header.frame_id = 'base_link'
# Set point to check IK for
rqst.ik_request.pose_stamped.pose.position = pose.position
rqst.ik_request.pose_stamped.pose.orientation = pose.orientation
if previous_state == None:
cs = self.r_commander.get_current_state()
rqst.ik_request.robot_state = cs
else:
rqst.ik_request.robot_state = previous_state
ik_answer = GetPositionIKResponse()
if DEBUG_MODE:
timeStart = rospy.Time.now()
ik_answer = self.ik_serv.call(rqst)
if DEBUG_MODE:
durationCall= rospy.Time.now() - timeStart
rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s")
return ik_answer
def computeJointTrajFromCartesian(self, points, arm="right_arm_torso"):
#fjt_goal = FollowJointTrajectoryGoal()
poselist = []
for point in points:
qt = quaternion_from_euler(point.positions[3], point.positions[4], point.positions[5])
pose = Pose(Point(point.positions[0], point.positions[1], point.positions[2]),
Quaternion(*qt.tolist()))
poselist.append(pose)
fjt_goal = self.computeIKsPose(poselist, arm)
return fjt_goal
def computeIKsPose(self, poselist, arm="right_arm"):
rospy.loginfo("Computing " + str(len(poselist)) + " IKs" )
fjt_goal = FollowJointTrajectoryGoal()
if arm == 'right_arm_torso':
fjt_goal.trajectory.joint_names = self.right_arm_torso
elif arm == 'left_arm_torso':
fjt_goal.trajectory.joint_names = self.left_arm_torso
elif arm == 'right_arm':
fjt_goal.trajectory.joint_names = self.right_arm
elif arm == 'left_arm':
fjt_goal.trajectory.joint_names = self.left_arm
# TODO: add other options
ik_answer = None
for pose in poselist:
if ik_answer != None:
ik_answer = self.getIkPose(pose,"right_arm", previous_state=ik_answer.solution)
else:
ik_answer = self.getIkPose(pose)
if DEBUG_MODE:
rospy.loginfo("Got error_code: " + str(ik_answer.error_code.val) + " which means: " + moveit_error_dict[ik_answer.error_code.val])
if moveit_error_dict[ik_answer.error_code.val] == 'SUCCESS':
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(0,1,0,1))
self.ok_markers.markers.append(arrow)
jtp = JointTrajectoryPoint()
#ik_answer = GetConstraintAwarePositionIKResponse()
# sort positions and add only the ones of the joints we are interested in
positions = self.sortOutJointList(fjt_goal.trajectory.joint_names, ik_answer.solution.joint_state)
jtp.positions = positions
# TODO: add velocities | WILL BE DONE OUTSIDE
# TODO: add acc? | DUNNO
fjt_goal.trajectory.points.append(jtp)
if DEBUG_MODE:
self.pub_ok_markers.publish(self.ok_markers)
else:
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(1,0,0,1))
self.fail_markers.markers.append(arrow)
self.pub_fail_markers.publish(self.fail_markers)
return fjt_goal
def sortOutJointList(self, joint_name_list, joint_state):
""" Get only the joints we are interested in and it's values and return it in
joint_state.name and joint_state.points format"""
if DEBUG_MODE:
rospy.loginfo("Sorting jointlist...")
list_to_iterate = joint_name_list
curr_j_s = joint_state
ids_list = []
position_list = []
for joint in list_to_iterate:
idx_in_message = curr_j_s.name.index(joint)
ids_list.append(idx_in_message)
position_list.append(curr_j_s.position[idx_in_message])
return position_list
def adaptTimesAndVelocitiesOfMsg(self, trajectory, plan, desired_final_time):
"""Adapt the times and velocities of the message for the controller
from the times computed in the DMP and velocities 0.0, controller will take care of it"""
rospy.loginfo("Adapting times and velocities...")
traj = trajectory #FollowJointTrajectoryGoal()
p = plan #GetDMPPlanResponse()
# we should have the same number of points on each, NOPE, as we can have points that IK fails
# if len(traj.trajectory.points) != len(p.plan.points):
# rospy.logerr("Oops, something went wrong, different number of points")
# rospy.logerr("generated trajectory: " + str(len(traj.trajectory.points)) + " plan: " + str(len(p.plan.points)))
# exit(0)
point = JointTrajectoryPoint()
counter = 0
for point in traj.trajectory.points:
#rospy.loginfo("Step " + str(counter) + " / " + str(len(traj.trajectory.points)))
counter += 1
point.velocities.extend(len(point.positions) * [0.0]) # adding 0.0 as speeds
point.time_from_start = rospy.Duration( counter * (desired_final_time / len(traj.trajectory.points)) )
return traj
def publish_markers(self):
while True:
self.pub_ok_markers.publish(self.ok_markers)
self.pub_fail_markers.publish(self.fail_markers)
rospy.sleep(0.1)
def createArrowMarker(self, pose, color):
marker = Marker()
marker.header.frame_id = 'base_link'
marker.type = marker.ARROW
marker.action = marker.ADD
general_scale = 0.01
marker.scale.x = general_scale
marker.scale.y = general_scale / 3.0
marker.scale.z = general_scale / 10.0
marker.color = color
marker.pose.orientation = pose.orientation
marker.pose.position = pose.position
marker.id = self.markers_id
self.markers_id += 1
return marker
def main(argv):
rospy.init_node("PickPlaceDemo")
rospy.on_shutdown(onshutdownhook)
rospy.wait_for_service("descartes_generate_motion_plan")
print "Planning service availabe"
global scene
scene = PlanningSceneInterface()
robot = RobotCommander()
path = JointTrajectory()
rospy.sleep(2)
discretization = math.pi / 30.0
#scene.is_diff = True
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.700
p.pose.position.y = 0.000
p.pose.position.z = 0.060
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
scene.add_box("box1", p, (0.15, 0.15, 0.15))
p.pose.position.x = 0.700
p.pose.position.y = 0.250
p.pose.position.z = 0.060
scene.add_box("box2", p, (0.15, 0.15, 0.15))
p.pose.position.x = 0.700
p.pose.position.y = 0.500
p.pose.position.z = 0.060
scene.add_box("box3", p, (0.15, 0.15, 0.15))
rospy.sleep(2)
del quaternion
#Pick the first box
quaternion = tf.transformations.quaternion_from_euler(
0.0, (math.pi / 2.0), (math.pi / 2.0))
poses = PoseArray()
pose_target = Pose()
pose_target.position.x = 0.700
pose_target.position.y = 0.000
pose_target.position.z = 0.155
pose_target.orientation.x = quaternion[0]
pose_target.orientation.y = quaternion[1]
pose_target.orientation.z = quaternion[2]
pose_target.orientation.w = quaternion[3]
poses.poses.append(pose_target)
try:
planningrequest = rospy.ServiceProxy('descartes_generate_motion_plan',
generate_motion_plan)
plan = planningrequest(poses, "world", "ee_link",
rospy.get_param("/controller_joint_names"), 5,
"Y_AXIS", discretization,
robot.get_current_state().joint_state)
path = plan.plan
print "Plan generated"
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.position_list = []
self.orientation_list = []
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.5)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
marker_joint_goal = [4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
def execute_plan(self,plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
class Phantomx_Pincher():
def __init__(self):
roscpp_initialize([])
rospy.sleep(5) #wait for moveit. TO-DO
self.robot = RobotCommander()
self.init_planner()
def init_planner(self):
# initialize the planner parameters
self.robot.pincher_arm.set_start_state(RobotState())
self.robot.pincher_arm.set_num_planning_attempts(10000)
self.robot.pincher_arm.set_planning_time(5)
self.robot.pincher_arm.set_goal_position_tolerance(0.01)
self.robot.pincher_arm.set_goal_orientation_tolerance(0.5)
def set_start_state_to_current_state(self):
self.robot.pincher_arm.set_start_state_to_current_state()
self.robot.pincher_gripper.set_start_state_to_current_state()
def openGripper(self):
#open gripper
posture = dict()
posture["PhantomXPincher_gripperClose_joint"] = 0.030
self.robot.pincher_gripper.set_joint_value_target(posture)
gplan = self.robot.pincher_gripper.plan()
if len(gplan.joint_trajectory.points) == 0:
return None
return gplan
def closeGripper(self):
# close gripper
posture = dict()
posture["PhantomXPincher_gripperClose_joint"] = 0.015
self.robot.pincher_gripper.set_joint_value_target(posture)
gplan = self.robot.pincher_gripper.plan()
if len(gplan.joint_trajectory.points) == 0:
return None
return gplan
def ef_pose(self, target, attemps=10):
# Here we try to verify if the target is in the arm range. Also, we
# try to orient the end-effector(ef) to nice hard-coded orientation
# Returns: planned trajectory
self.robot.get_current_state()
d = pow(pow(target[0], 2) + pow(target[1], 2), 0.5)
if d > 3.0:
rospy.loginfo("Too far. Out of reach")
return None
rp = np.pi / 2.0 - np.arcsin((d - 0.1) / .205)
ry = np.arctan2(target[1], target[0])
attemp = 0
again = True
while (again and attemp < 10):
again = False
rp_a = attemp * ((0.05 + 0.05) * np.random.ranf() - 0.05) + rp
ry_a = attemp * ((0.05 + 0.05) * np.random.ranf() - 0.05) + ry
q = tf.transformations.quaternion_from_euler(0, rp_a, ry_a)
print[0, rp_a, ry_a]
p = geometry_msgs.msg.Pose()
p.position.x = target[0]
p.position.y = target[1]
p.position.z = target[2]
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
target[2] += np.abs(np.cos(rp)) / 250.0
self.robot.pincher_arm.set_pose_target(p)
planed = self.robot.pincher_arm.plan()
if len(planed.joint_trajectory.points) == 0:
rospy.loginfo("Motion plan failed, trying again")
attemp += 1
again = True
if again:
return None
return planed
def arm_execute(self, plan):
self.robot.pincher_arm.execute(plan)
def gripper_execute(self, plan):
self.robot.pincher_gripper.execute(plan)
class Phantomx_Pincher():
def __init__(self):
roscpp_initialize([])
rospy.sleep(8)
# TO-DO: wait for moveit.
self.listener = tf.TransformListener()
self.robot = RobotCommander()
self.init_planner()
self.gripper_dimensions = [0.02, 0.02, 0.02]
def init_planner(self):
# initialize the planner parameters
self.robot.pincher_arm.set_start_state(RobotState())
self.robot.pincher_arm.set_num_planning_attempts(10000)
self.robot.pincher_arm.set_planning_time(7)
self.robot.pincher_arm.set_goal_position_tolerance(0.01)
self.robot.pincher_arm.set_goal_orientation_tolerance(0.1)
def set_start_state_to_current_state(self):
self.robot.pincher_arm.set_start_state_to_current_state()
self.robot.pincher_gripper.set_start_state_to_current_state()
def openGripper(self):
# open gripper
posture = dict()
posture["PhantomXPincher_gripperClose_joint"] = 0.030
self.robot.pincher_gripper.set_joint_value_target(posture)
gplan = self.robot.pincher_gripper.plan()
if len(gplan.joint_trajectory.points) == 0:
return None
return gplan
def closeGripper(self):
# close gripper
posture = dict()
posture["PhantomXPincher_gripperClose_joint"] = 0.010
self.robot.pincher_gripper.set_joint_value_target(posture)
gplan = self.robot.pincher_gripper.plan()
if len(gplan.joint_trajectory.points) == 0:
return None
return gplan
def to_pose(self, waypoint, frame="/map"):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = frame
p.header.stamp = rospy.Time.now()
p.pose.position.x = waypoint[0]
p.pose.position.y = waypoint[1]
p.pose.position.z = waypoint[2]
p.pose.orientation.w = 1
if len(waypoint) == 7:
p.pose.orientation.x = waypoint[3]
p.pose.orientation.y = waypoint[4]
p.pose.orientation.z = waypoint[5]
p.pose.orientation.w = waypoint[6]
elif len(waypoint) == 6:
q = tf.transformations.quaternion_from_euler(
waypoint[3] / 180.0 * np.pi, waypoint[4] / 180.0 * np.pi,
waypoint[5] / 180.0 * np.pi)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
return p
def target_to_frame(self,
target,
frame_to="/arm_base_link",
frame_from="/map",
orientation=False):
pose = self.to_pose(target, frame=frame_from)
self.listener.waitForTransform(frame_from, frame_to, rospy.Time.now(),
rospy.Duration(4))
self.listener.getLatestCommonTime(frame_from, frame_to)
pose = self.listener.transformPose(frame_to, pose)
if orientation:
return [
pose.pose.position.x, pose.pose.position.y,
pose.pose.position.z, pose.pose.orientation.x,
pose.pose.orientation.y, pose.pose.orientation.z,
pose.pose.orientation.w
]
return [
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z
]
def correct_grasp_position(self, obj_dimension, gripper_orientation):
'''
This function corrects the grasp position accordantly to the object
dimensions, the gripper size (available size for the gripper grasp) and
the gripper orientation.
Input:
- obj_dimension: list with object dimension in meters along x,y,z
- gripper_orientation: [roll, pitch, yaw] in radians
'''
if len(obj_dimension) < 3:
return np.asarray([0, 0, 0])
obj_dimension = np.asarray(obj_dimension)
# rospy.loginfo("Obj Dimension [%s]", obj_dimension)
delta = (obj_dimension - self.gripper_dimensions) / 2.
delta = list(delta)
delta[2] = 0
delta[1] = 0.01 # gripper_link
# Tranformation Matrix from gripper to arm_base
T_G_B = tf.transformations.euler_matrix(gripper_orientation[0],
gripper_orientation[1],
gripper_orientation[2])
gripper_delta = T_G_B * np.asmatrix(delta + [1]).T
gripper_delta = gripper_delta[:3]
rospy.loginfo("Delta [arm_base_link] [%s]", gripper_delta)
return np.asarray(gripper_delta).flatten()
def ef_pose(self,
target,
dimension=[],
orientation=[],
attemps=10,
frame='/map'):
'''
Here we try to verify if the target is in the arm range. Also, we
try to orient the end-effector(ef) to nice hard-coded orientation
Returns: planned trajectory
'''
self.robot.get_current_state()
target = self.target_to_frame(target, frame_from=frame)
# rospy.loginfo("Planing to [%s] on arm_frame", target)
d = pow(pow(target[0], 2) + pow(target[1], 2), 0.5)
if d > 0.3:
rospy.loginfo("Too far. Out of reach")
return None
if len(orientation) > 0:
rpy = tf.transformations.euler_from_quaternion(orientation)
# rospy.loginfo('Robot target quaternion [%s]',
# np.rad2deg(orientation))
rp = rpy[1]
else:
rp = np.pi / 2.0 - np.arcsin((d - 0.1) / .205)
ry = np.arctan2(target[1], target[0])
rospy.loginfo("Original target %s %s", target, np.rad2deg([0, rp, ry]))
delta = self.correct_grasp_position(dimension, [0, rp, ry])
target -= delta
rospy.loginfo(target)
ry = np.arctan2(target[1], target[0])
rospy.loginfo(ry)
rospy.loginfo("Target after correction %s %s", target,
np.rad2deg([0, rp, ry]))
attemp = 0
again = True
while (again and attemp < 10):
again = False
rp_a = attemp * ((0.05 + 0.05) * np.random.ranf() - 0.05) + rp
ry_a = ry
# attemp * ((0.05 + 0.05)*np.random.ranf() - 0.05) + ry
q = tf.transformations.quaternion_from_euler(0, rp_a, ry_a)
rospy.loginfo([0, rp_a, ry_a])
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = '/arm_base_link'
p.pose.position.x = target[0]
p.pose.position.y = target[1]
p.pose.position.z = target[2]
# + np.abs(np.cos(rp))/50.0
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
rospy.loginfo(
"New target pose [%.4f, %.4f, %.4f] [d: %.4f, p: %.4f, y: %.4f]",
p.pose.position.x, p.pose.position.y, p.pose.position.z, d,
rp_a, ry_a)
self.robot.pincher_arm.set_pose_target(p)
planed = self.robot.pincher_arm.plan()
if len(planed.joint_trajectory.points) == 0:
rospy.loginfo("Motion plan failed, trying again")
attemp += 1
again = True
if again:
return None
return planed
def arm_execute(self, plan):
return self.robot.pincher_arm.execute(plan)
def gripper_execute(self, plan):
return self.robot.pincher_gripper.execute(plan)
#
# Author: Ioan Sucan
import sys
import rospy
from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState
if __name__ == "__main__":
roscpp_initialize(sys.argv)
rospy.init_node("moveit_py_demo", anonymous=True)
robot = RobotCommander()
rospy.sleep(1)
print("Current state:")
print(robot.get_current_state())
# plan to a random location
a = robot.right_arm
a.set_start_state(RobotState())
r = a.get_random_joint_values()
print("Planning to random joint position: ")
print(r)
p = a.plan(r)
print("Solution:")
print(p)
roscpp_shutdown()
# ------------------------------
# Configure the planner
# -----------------------------
robot = RobotCommander()
robot.pincher_arm.set_start_state(RobotState())
#robot.pincher_arm.set_planner_id('RRTConnectkConfigDefault')
robot.pincher_arm.set_num_planning_attempts(10000)
robot.pincher_arm.set_planning_time(5)
robot.pincher_arm.set_goal_position_tolerance(0.01)
robot.pincher_arm.set_goal_orientation_tolerance(0.1)
robot.pincher_gripper.set_goal_position_tolerance(0.01)
robot.pincher_gripper.set_goal_orientation_tolerance(0.1)
robot.get_current_state()
robot.pincher_arm.set_start_state(RobotState())
'''#------------------------------
# The Grasp
#-----------------------------
the_grasp = Grasp()
the_grasp.id = "Por cima"
# Gripper Posture before the grasp (opened griper)
the_grasp.pre_grasp_posture.joint_names = robot.pincher_gripper.get_active_joints()
the_grasp.pre_grasp_posture.points.append(JointTrajectoryPoint())
the_grasp.pre_grasp_posture.points[0].positions = [0.030]
# Gripper posture while grasping (closed gripper)
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)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
# self.listener = tf.TransformListener()
#self.goal_x = 0
#self.goal_y = 0
#self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.t_idd = 0
self.t_pose_x = []
self.t_pose_y = []
self.t_pose_z = []
self.t_ori_w = []
self.t_ori_x = []
self.t_ori_y = []
self.t_ori_z = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
-0.535054565144069, -2.009213503260451, 1.8350906250920112,
-0.7794355413099039, -0.7980899690645948, 0.7782740454087982
]
print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_move(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
tomato_offset = [0, -0.35, -0.1]
robot_base_offset = 0.873
base_wrist2_offset = 0.1
print(">> robot arm planning frame: \n", planning_frame)
'''
self.calculated_tomato.position.x = (scale*self.goal_x)
self.calculated_tomato.position.y = (scale*self.goal_y)
self.calculated_tomato.position.z = (scale*self.goal_z)
'''
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation = Quaternion(
*quaternion_from_euler(3.14, 0.1, 1.57))
print("=== robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("=== Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch,
yaw):
'''
Manipulator is moving to the desired coordinate
Now move to the calculated_tomato_id_goal
'''
calculated_tomato_id_goal = Pose()
desired_goal_pose = [pose_x, pose_y, pose_z]
desired_goal_euler = [roll, pitch, yaw]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n", Cplanning_frame)
calculated_tomato_id_goal.position.x = desired_goal_pose[0]
calculated_tomato_id_goal.position.y = desired_goal_pose[1]
calculated_tomato_id_goal.position.z = desired_goal_pose[2]
calculated_tomato_id_goal.orientation = Quaternion(
*quaternion_from_euler(desired_goal_euler[0],
desired_goal_euler[1],
desired_goal_euler[2]))
print(">> tomato_id_goal frame: ", desired_goal_pose)
self.robot_arm.set_pose_target(calculated_tomato_id_goal)
tf_display_position = [
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.z
]
tf_display_orientation = [
calculated_tomato_id_goal.orientation.x,
calculated_tomato_id_goal.orientation.y,
calculated_tomato_id_goal.orientation.z,
calculated_tomato_id_goal.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(
copy.deepcopy(calculated_tomato_id_goal))
(tomato_id_goal_plan,
tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(
tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale *
self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
self.distance = 0
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.calculated_tomato_coor = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
2.6482303142547607, -0.3752959410296839, -2.1118043104754847,
-0.4801228682147425, -1.4944542090045374, -4.647655431424276
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
def go_to_goal_point(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
print(">> robot arm planning frame: \n", planning_frame)
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation.w = (scale * ori_w)
self.calculated_tomato.orientation.x = (scale * ori_x)
self.calculated_tomato.orientation.y = (scale * ori_y)
self.calculated_tomato.orientation.z = (scale * ori_z)
print(">> robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("======= Press `Enter` to if plan in correct!======")
raw_input()
self.robot_arm.go(True)
'''
def go_to_designated_coordinate(self):
desired_goal_pose = [-0.537,0.166, 0.248]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n",Cplanning_frame)
self.calculated_tomato_coor.position.x = desired_goal_pose[0]
self.calculated_tomato_coor.position.y = desired_goal_pose[1]
self.calculated_tomato_coor.position.z = desired_goal_pose[2]
self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2]))
print(">> goal frame", self.calculated_tomato_coor)
self.robot_arm.set_pose_target(self.calculated_tomato_coor)
tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z]
tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w]
jj = 0
while jj < 5:
jj += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor))
(plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0)
self.display_trajectory(plan)
print("=========== Press `Enter` to if plan is correct!!...")
raw_input()
self.robot_arm.go(True)
'''
def plan_cartesian_path(self, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x -= scale * 0.1
self.wpose.position.y += scale * 0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
self.wpose.position.x -= scale*0.2
waypoints.append(copy.deepcopy(self.wpose))
'''
'''
self.wpose.position.x -= scale*0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
try:
(trans,
rot) = self.listener.lookupTransform('base_link', 'yolo_output00',
rospy.Time(0))
goal_x = trans[0]
goal_y = trans[1]
goal_z = trans[2]
ori_w = rot[0]
ori_x = rot[1]
ori_y = rot[2]
ori_z = rot[3]
print("==== trans[x,y,z]: ", trans)
print("==== rot[x,y,z,w]: ", rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn('there is no tf')
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)
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), plan_time, total_joint_rotation, comp_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
class BaseEnvironment():
def __init__(self, limb_name):
rospy.init_node('Baxter_Env')
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self.scene._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=0)
self.limb_name = limb_name
self.limb = baxter_interface.Limb(limb_name)
self.group = moveit_commander.MoveGroupCommander(limb_name + "_arm")
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
DisplayTrajectory,
queue_size=0)
rospy.sleep(2)
print("Initialized Base environment.")
def construct_robot_state(self, joint):
rs = RobotState()
robot_state = self.robot.get_current_state()
rs.joint_state.name = robot_state.joint_state.name
rs.joint_state.position = list(
robot_state.joint_state.position
) # filler for rest of the joint angles not found in waypoint
joint_name_indices = [
rs.joint_state.name.index(n) for n in waypoint.keys()
]
for i, idx in enumerate(joint_name_indices):
rs.joint_state.position[idx] = waypoint.values()[i]
return rs
def move_to_random_state(self):
waypoint = sample_loc(1, self.limb_name)
self.limb.move_to_joint_positions(waypoint) # Baxter SDK;
print("Moving robot arms to waypoint: %s" % (waypoint, ))
def move_to_goal(self, goal):
# Move the limb to goal;
self.group.set_joint_value_target(goal)
result = self.group.go()
print "============ Waiting while moving robot to goal."
print "============ Printing result after moving: %s" % result
def plan_trajectory_in_cspace(self, goal, visualization=True):
## Then, we will get the current set of joint values for the group
self.group.set_joint_value_target(goal)
plan = self.group.plan()
if visualization:
print "============ Visualizing trajectory_plan"
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state(
)
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while plan is visualized (again)..."
rospy.sleep(5)
return plan
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
self.clear_octomap()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose : ", self.wpose
marker_joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def look_object(self):
look_object = self.robot_arm.get_current_joint_values()
## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함
print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5]
#look_object[5] = math.radians(90)
look_object[5] = 0
'''
if look_object[5] > abs(math.radians(180)):
if look_object[5] < 0:
look_object[5] = look_object[5] + math.radians(90) # wrist3
elif look_object[5] > 0:
look_object[5] = look_object[5] - math.radians(90)
else:
look_object[5] = look_object[5] - math.radians(90) # wrist3
'''
print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5]
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap... Please wait...."
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
print "waypoints:", waypoints
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
print "======= pos(meter): ", self.position_list
print "======= orientation: ", self.orientation_list
def go_to_move(self, scale = 1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
coke_offset = [0, -0.35, -0.1] #x y z
# gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품.
# linear offset = abs([0, 0.0625, 0.13])
robot_base_offset = 0.873
base_wrist2_offset = 0.1 #for avoiding link contact error
print "========== robot arm plannig frame: \n", planning_frame
self.calculed_coke_pose.position.x = (scale * self.goal_x) + base_wrist2_offset # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1]
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset
self.calculed_coke_pose.position.z = (scale * self.goal_z) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z]
tf_display_orientation = [self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
rate.sleep()
## ## ## show how to move on the Rviz
coke_waypoints = []
coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose))
(coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(coke_waypoints, 0.01, 0.0)
self.display_trajectory(coke_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def ar_tf_listener(self, msg):
try:
self.marker = msg.markers
ml = len(self.marker)
target_id = 9
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_id:
pass
#target_id_flage = False
elif self.m_idd == target_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w]
(self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
) # initializes rospy node, creates a test environment with collision objects
""" Preliminaries, initialize limb, set up publisher for collision checking """
limb_name = 'right'
limb = baxter_interface.Limb(limb_name)
robot_state_collision_pub = rospy.Publisher('/robot_collision_state',
DisplayRobotState,
queue_size=0)
sv = StateValidity()
# Sample a waypoint
waypoint = sample_loc(num_samples=1, name=limb_name)
# CONSTRUCT A ROBOTSTATE MSG FROM SAMPLED JOINT FOR COLLISION CHECKING
rs = RobotState()
robot = RobotCommander()
robot_state = robot.get_current_state()
rs.joint_state.name = robot_state.joint_state.name
rs.joint_state.position = list(
robot_state.joint_state.position
) # filler for rest of the joint angles not found in waypoint
joint_name_indices = [
rs.joint_state.name.index(n) for n in waypoint.keys()
]
for i, idx in enumerate(joint_name_indices):
rs.joint_state.position[idx] = waypoint.values()[i]
collision_flag = sv.getStateValidity(rs, group_name=limb_name + '_arm')
print(collision_flag) # Boolean
limb.move_to_joint_positions(
waypoint) # moves to waypoint for visual confirmation
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
self.listener = tf.TransformListener()
# self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
self.tomatoboundingBox = BoundingBox()
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
self.distance = 0
self.trans = [0.0, 0.0, 0.0]
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print ("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print ("====== current pose : ", self.wpose)
joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982]
print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait = True)
def go_to_desired_coordinate(self):
self.calculated_tomato.position.x = goal_x
self.calculated_tomato.position.y = goal_y
self.calculated_tomato.position.z = goal_z
self.calculated_tomato.orientation.x = goal_roll
self.calculated_tomato.orientation.y = goal_pitch
self.calculated_tomato.orientation.z = goal_yaw
tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z]
tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato))
(tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print ("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
# rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
try:
(trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0))
goal_x = round(trans[0],2)
goal_y = round(trans[1],2)
goal_z = round(trans[2],2)
print("====== trans [x, y, z]: ", trans)
print("====== rotat [x, y, z, w]: ", rot)
orientation = euler_from_quaternion(rot)
goal_roll = orientation[0]
goal_pitch = orientation[1]
goal_yaw = orientation[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('there is no tf')
class TestPlanners(object):
def __init__(self, group_id, planner):
rospy.init_node('moveit_test_planners', anonymous=True)
self.pass_list = []
self.fail_list = []
self.planner = planner
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(group_id)
rospy.sleep(1)
self.group.set_planner_id(self.planner)
self.test_trajectories_empty_environment()
self.test_waypoints()
self.test_trajectories_with_walls_and_ground()
for pass_test in self.pass_list:
print(pass_test)
for fail_test in self.fail_list:
print(fail_test)
def _add_walls_and_ground(self):
# publish a scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below ground (to prevent collision with
# the robot itself)
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.1))
p.pose.position.x = 0.4
p.pose.position.y = 0.85
p.pose.position.z = 0.4
p.pose.orientation.x = 0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
self.scene.add_box("wall_front", p, (0.8, 2, 0.01))
p.pose.position.x = 1.33
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707388
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.706825
self.scene.add_box("wall_right", p, (0.8, 2, 0.01))
p.pose.position.x = -0.5
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707107
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.707107
self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
# rospy.sleep(1)
def _check_plan(self, plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
def _plan_joints(self, joints):
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
return self._check_plan(plan)
def test_trajectories_rotating_each_joint(self):
# test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
test_joint_values = [numpy.pi / 2.0]
joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
# Joint 4th is colliding with the hand
# for joint in range(6):
for joint in [0, 1, 2, 3, 5]:
for value in test_joint_values:
joints[joint] = value
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
def test_trajectories_empty_environment(self):
# Up - Does not work with sbpl but it does with ompl
joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# All joints up
joints = [
-1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Down
joints = [
-0.000348431194526, 0.397651011661, 0.0766181197394,
-0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# left
joints = [
0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
0.146075718452, 0.00420656698366, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Front
joints = [
1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
-1.42700242769, 3.1415450794, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Behind
joints = [
1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
0.694689321451, -0.390769365032, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Should fail because it is in self-collision
joints = [
-0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
1.04235601797, -1.69730925867, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
def test_waypoints(self):
# Start planning in a given joint position
joints = [
-0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
0.344227622185, -3.03250264451, 0.0, 0.0
]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:8] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
waypoints = []
initial_pose = self.group.get_current_pose().pose
initial_pose.position.x = -0.301185959729
initial_pose.position.y = 0.517069787724
initial_pose.position.z = 1.20681710541
initial_pose.orientation.x = 0.0207499700474
initial_pose.orientation.y = -0.723943002716
initial_pose.orientation.z = -0.689528413407
initial_pose.orientation.w = 0.00515118111221
# start with a specific position
waypoints.append(initial_pose)
# first move it down
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[0].orientation
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.20
waypoints.append(copy.deepcopy(wpose))
# second front
wpose.position.y += 0.20
waypoints.append(copy.deepcopy(wpose))
# third side
wpose.position.x -= 0.20
waypoints.append(copy.deepcopy(wpose))
# fourth return to initial pose
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
(plan3,
fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
self.pass_list.append("Passed: test_waypoints, " + self.planner)
def test_trajectories_with_walls_and_ground(self):
self._add_walls_and_ground()
# Should fail to plan: Goal is in collision with the wall_front
joints = [
0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
0.302143499777, 0.00130280337897, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Should fail to plan: Goal is in collision with the ground
joints = [
3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
0.000133883149666, -0.594498939239, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Goal close to right corner
joints = [
0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
-1.3516255551, 2.8225061435, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed, test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
self.scene.remove_world_object("ground")
self.scene.remove_world_object("wall_left")
self.scene.remove_world_object("wall_right")
self.scene.remove_world_object("wall_front")
class BaxterMoveit(BaxterRobot):
LEFT = -1
RIGHT = 1
def __init__(self, node, limb=+1):
super(BaxterMoveit, self).__init__(node, limb=+1)
self.group = MoveGroupCommander(self.lns + "_arm")
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self._info()
def _info(self):
'''Getting Basic Information'''
# name of the reference frame for this robot:
print("@@@@@@@@@@@@ Reference frame: %s" %
self.group.get_planning_frame())
# We can also print the name of the end-effector link for this group:
print("@@@@@@@@@@@@ End effector: %s" %
self.group.get_end_effector_link())
# We can get a list of all the groups in the robot:
print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@",
self.robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@")
print(self.robot.get_current_state())
def obstacle(self, name, position, size):
planning_frame = self.robot.get_planning_frame()
pose = PoseStamped()
pose.header.frame_id = planning_frame
pose.pose.position.x = position[0]
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
self.scene.add_box(name, pose, tuple(size))
def movej(self, q, raw=False):
''' move in joint space by giving a joint configuration as dictionary'''
if raw:
print("in moveit 'raw' motion is not avaiable")
# succ = False
# while succ is False:
succ = self.group.go(q, wait=True)
# self.group.stop() # ensures that there is no residual movement
def showplan(self, target):
if type(target) is PyKDL.Frame or type(target) is Pose:
q = self.ik(target)
elif type(target) is dict:
q = target
else:
print("Target format error")
return
self.group.set_joint_value_target(q)
self.group.plan()
def movep(self, pose, raw=False):
''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame'''
if type(pose) is PyKDL.Frame:
pose = transformations.KDLToPose(pose)
q = self.ik(pose)
if q is not None:
self.movej(q, raw=raw)
else:
print("\nNO SOLUTION TO IK\n" * 20)
def simple_function():
rc = RobotCommander()
mgc = MoveGroupCommander("manipulator")
# print(rc.get_group_names())
# print(rc.get_group('manipulator'))
# exit()
eef_step = 0.01
jump_threshold = 2
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rc.get_current_state()
rospy.logwarn(rc.get_current_state())
sss.wait_for_input()
#rate = rospy.Rate(0.1) # 10hz
rate = rospy.Rate(1) # 10hz
rospy.sleep(1)
theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
while not rospy.is_shutdown():
approach_pose = PoseStamped()
approach_pose.header.frame_id = "table"
approach_pose.header.stamp = rospy.Time(0)
approach_pose.pose.position.x = 0.146
approach_pose.pose.position.y = 0.622
approach_pose.pose.position.z = 0.01
quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
approach_pose.pose.orientation.x = quat[0]
approach_pose.pose.orientation.y = quat[1]
approach_pose.pose.orientation.z = quat[2]
approach_pose.pose.orientation.w = quat[3]
# mgc.set_start_state_to_current_state()
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed before adding collision objects")
# else:
# rospy.logwarn("Planning succeeded before adding collision objects")
#
# rospy.logwarn("waiting for input to add dummy box")
# sss.wait_for_input()
#
box_dummy_pose = PoseStamped()
box_dummy_pose.header.frame_id = "table"
box_dummy_pose.pose.position.x = 0.147
box_dummy_pose.pose.position.y = 0.636
box_dummy_pose.pose.position.z = 0
psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC
# rospy.logwarn("I have added the box")
# rospy.sleep(1)
# rospy.logwarn("waiting for input to try planning with dummy box")
# sss.wait_for_input()
#
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
# else:
# rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#
rospy.logwarn("waiting for input to add end effector box box")
sss.wait_for_input()
# end effector collision object
link_attached_to_ef = "ee_link"
mb_ef_collisonobj = "metal_bracket"
mb_ef_pose = PoseStamped()
mb_ef_pose.header.frame_id = link_attached_to_ef
mb_ef_pose.pose.position.x = 0.065/2.0
mb_ef_pose.pose.position.y = 0.0
mb_ef_pose.pose.position.z = 0.0
mb_ef_size = (0.065,0.06,0.06)
psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
raw_input("0 hi")
mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
rospy.logwarn("I have added the attached box to the end effector")
rospy.sleep(1)
raw_input("1 hi")
rospy.logwarn(rc.get_current_state())
# mgc.attach_object(object_name, link_name, touch_links)
mgc.set_start_state_to_current_state()
rospy.logwarn(rc.get_current_state())
raw_input("2 hi")
rospy.logwarn("waiting for input to try planning with end effector box")
sss.wait_for_input()
(traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
if(frac_approach != 1):
rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
else:
rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
rospy.logwarn("waiting for input to try planning next loop")
sss.wait_for_input()
rate.sleep()
