def test_ompl_freespace_joint_cart():
env, manip = get_environment()
fwd_kin = env.getManipulatorManager().getFwdKinematicSolver(
manip.manipulator)
inv_kin = env.getManipulatorManager().getInvKinematicSolver(
manip.manipulator)
joint_names = fwd_kin.getJointNames()
cur_state = env.getCurrentState()
wp1 = JointWaypoint(joint_names,
np.array([0, 0, 0, -1.57, 0, 0, 0], dtype=np.float64))
wp2 = CartesianWaypoint(Isometry3d.Identity() *
Translation3d(-.2, .4, 0.2) *
Quaterniond(0, 0, 1.0, 0))
start_instruction = PlanInstruction(Waypoint(wp1),
PlanInstructionType_START,
"TEST_PROFILE")
plan_f1 = PlanInstruction(Waypoint(wp2), PlanInstructionType_FREESPACE,
"TEST_PROFILE")
program = CompositeInstruction("TEST_PROFILE")
program.setStartInstruction(Instruction(start_instruction))
program.setManipulatorInfo(manip)
program.append(Instruction(plan_f1))
seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10)
plan_profile = OMPLDefaultPlanProfile()
test_planner = OMPLMotionPlanner()
test_planner.plan_profiles["TEST_PROFILE"] = plan_profile
problem_generator_fn = OMPLProblemGeneratorFn(DefaultOMPLProblemGenerator)
test_planner.problem_generator = problem_generator_fn
request = PlannerRequest()
request.seed = seed
request.instructions = program
request.env = env
request.env_state = env.getCurrentState()
response = PlannerResponse()
assert test_planner.solve(request, response)
assert response.status.value(
) == OMPLMotionPlannerStatusCategory.SolutionFound
results = flatten(response.results)
assert len(results) == 11
for instr in results:
assert isMoveInstruction(instr)
wp1 = instr.cast_MoveInstruction().getWaypoint()
assert isStateWaypoint(wp1)
wp = wp1.cast_StateWaypoint()
assert len(wp.joint_names) == 7
assert isinstance(wp.position, np.ndarray)
assert len(wp.position) == 7
def main():
locator = GazeboModelResourceLocator()
env = Environment()
urdf_path = FilesystemPath("../urdf/combined.urdf")
srdf_path = FilesystemPath("../urdf/combined.srdf")
assert env.init(urdf_path, srdf_path, locator)
checker = env.getDiscreteContactManager()
contact_distance = 0.2
monitored_link_names = env.getLinkNames()
checker.setActiveCollisionObjects(monitored_link_names)
checker.setContactDistanceThreshold(contact_distance)
#load calibration parameters
with open('../calibration/sawyer.yaml') as file:
H_Sawyer = np.array(yaml.load(file)['H'], dtype=np.float64)
with open('../calibration/ur.yaml') as file:
H_UR = np.array(yaml.load(file)['H'], dtype=np.float64)
with open('../calibration/abb.yaml') as file:
H_ABB = np.array(yaml.load(file)['H'], dtype=np.float64)
env.changeJointOrigin("ur_pose", Isometry3d(H_UR))
env.changeJointOrigin("sawyer_pose", Isometry3d(H_Sawyer))
env.changeJointOrigin("abb_pose", Isometry3d(H_ABB))
joint_angles = np.ones((6, ), dtype=np.float64) * 0.1
joint_angles[1] = 2
joint_angles[2] = 2
env.setState(ABB_joint_names, joint_angles)
env_state = env.getCurrentState()
checker.setCollisionObjectsTransform(env_state.link_transforms)
result = ContactResultMap()
checker.contactTest(result, ContactRequest(ContactTestType_ALL))
result_vector = ContactResultVector()
collisionFlattenResults(result, result_vector)
for r in result_vector:
print(r.link_names[0] + "," + r.link_names[1] + " " + str(r.distance))
def test_planning_server_freespace():
env, manip = get_environment()
joint_names = [
"joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6",
"joint_a7"
]
wp1 = JointWaypoint(joint_names,
np.array([0, 0, 0, -1.57, 0, 0, 0], dtype=np.float64))
wp2 = CartesianWaypoint(Isometry3d.Identity() *
Translation3d(-.2, .4, 0.2) *
Quaterniond(0, 0, 1.0, 0))
start_instruction = PlanInstruction(Waypoint(wp1),
PlanInstructionType_START, "DEFAULT")
plan_f1 = PlanInstruction(Waypoint(wp2), PlanInstructionType_FREESPACE,
"DEFAULT")
program = CompositeInstruction("DEFAULT")
program.setStartInstruction(Instruction(start_instruction))
program.setManipulatorInfo(manip)
program.append(Instruction(plan_f1))
planning_server = ProcessPlanningServer(env, 1)
planning_server.loadDefaultProcessPlanners()
request = ProcessPlanningRequest()
request.name = FREESPACE_PLANNER_NAME
request.instructions = Instruction(program)
response = planning_server.run(request)
planning_server.waitForAll()
assert response.interface.isSuccessful()
results = flatten(response.getResults().cast_CompositeInstruction())
assert len(results) == 37
for instr in results:
assert isMoveInstruction(instr)
wp1 = instr.cast_MoveInstruction().getWaypoint()
assert isStateWaypoint(wp1)
wp = wp1.cast_StateWaypoint()
assert len(wp.joint_names) == 7
assert isinstance(wp.position, np.ndarray)
assert len(wp.position) == 7
manip_info = ManipulatorInfo()
manip_info.manipulator = "manipulator"
viewer = TesseractViewer()
viewer.update_environment(t_env, [0, 0, 0])
joint_names = ["joint_%d" % (i + 1) for i in range(6)]
viewer.update_joint_positions(joint_names, np.array([1, -.2, .01, .3, -.5, 1]))
viewer.start_serve_background()
t_env.setState(joint_names, np.ones(6) * 0.1)
wp1 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(.6, -.8, 0.6) *
Quaterniond(0, 0, 1.0, 0))
wp2 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(.4, .8, 1.5) *
Quaterniond(0.7071, 0, 0.7071, 0))
start_instruction = PlanInstruction(Waypoint(wp1), PlanInstructionType_START,
"DEFAULT")
plan_f1 = PlanInstruction(Waypoint(wp2), PlanInstructionType_FREESPACE,
"DEFAULT")
program = CompositeInstruction("DEFAULT")
program.setStartInstruction(Instruction(start_instruction))
program.setManipulatorInfo(manip_info)
program.append(Instruction(plan_f1))
planning_server = ProcessPlanningServer(t_env, 1)
checker = env.getDiscreteContactManager()
contact_distance=0.2
monitored_link_names = env.getLinkNames()
checker.setActiveCollisionObjects(monitored_link_names)
checker.setContactDistanceThreshold(contact_distance)
#load calibration parameters
with open(workspace_path+'calibration/Sawyer2.yaml') as file:
H_Sawyer = np.array(yaml.load(file)['H'],dtype=np.float64)
with open(workspace_path+'calibration/UR2.yaml') as file:
H_UR = np.array(yaml.load(file)['H'],dtype=np.float64)
with open(workspace_path+'calibration/ABB2.yaml') as file:
H_ABB = np.array(yaml.load(file)['H'],dtype=np.float64)
env.changeJointOrigin("ur_pose", Isometry3d(H_UR))
env.changeJointOrigin("sawyer_pose", Isometry3d(H_Sawyer))
env.changeJointOrigin("abb_pose", Isometry3d(H_ABB))
viewer = TesseractViewer()
viewer.update_environment(env, [0,0,0])
viewer.start_serve_background()
def check():
try:
#update robot joints
for i in range(num_robot):
wire_packet=robot_state_list[i].TryGetInValue()
#only update the ones online
def __init__(self):
self._lock=threading.RLock()
self._running=False
#load calibration parameters
with open('calibration/Sawyer.yaml') as file:
H_Sawyer = np.array(yaml.load(file)['H'],dtype=np.float64)
with open('calibration/UR.yaml') as file:
H_UR = np.array(yaml.load(file)['H'],dtype=np.float64)
with open('calibration/ABB.yaml') as file:
H_ABB = np.array(yaml.load(file)['H'],dtype=np.float64)
with open('calibration/tx60.yaml') as file:
H_tx60 = np.array(yaml.load(file)['H'],dtype=np.float64)
self.H_UR=H42H3(H_UR)
self.H_Sawyer=H42H3(H_Sawyer)
self.H_ABB=H42H3(H_ABB)
self.H_tx60=H42H3(H_tx60)
self.H_robot={'ur':self.H_UR,'sawyer':self.H_Sawyer,'abb':self.H_ABB,'staubli':self.H_tx60}
self.distance_report=RRN.GetStructureType("edu.rpi.robotics.distance.distance_report")
self.dict={'ur':0,'sawyer':1,'abb':2,'staubli':3}
self.distance_report_dict={}
for robot_name,robot_idx in self.dict.items():
self.distance_report_dict[robot_name]=self.distance_report()
#connect to RR gazebo plugin service
server=RRN.ConnectService('rr+tcp://localhost:11346/?service=GazeboServer')
self.w=server.get_worlds(str(server.world_names[0]))
#create RR pose type
pose_dtype = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Pose", server)
self.model_pose = np.zeros((1,), dtype = pose_dtype)
#form H into RR transformation struct
self.transformations={}
self.transformation=RRN.GetStructureType("edu.rpi.robotics.distance.transformation")
transformation1=self.transformation()
transformation1.name="UR"
transformation1.row=len(self.H_UR)
transformation1.column=len(self.H_UR[0])
transformation1.H=np.float16(self.H_UR).flatten().tolist()
self.transformations['ur']=transformation1
transformation2=self.transformation()
transformation2.name="Sawyer"
transformation2.row=len(self.H_Sawyer)
transformation2.column=len(self.H_Sawyer[0])
transformation2.H=np.float16(self.H_Sawyer).flatten().tolist()
self.transformations['sawyer']=transformation2
transformation3=self.transformation()
transformation3.name="ABB"
transformation3.row=len(self.H_ABB)
transformation3.column=len(self.H_ABB[0])
transformation3.H=np.float16(self.H_ABB).flatten().tolist()
self.transformations['abb']=transformation3
transformation4=self.transformation()
transformation4.name="Staubli"
transformation4.row=len(self.H_tx60)
transformation4.column=len(self.H_tx60[0])
transformation4.H=np.float16(self.H_tx60).flatten().tolist()
self.transformations['staubli']=transformation4
#Connect to robot service
with open('client_yaml/client_ur.yaml') as file:
self.url_ur= yaml.load(file)['url']
with open('client_yaml/client_sawyer.yaml') as file:
self.url_sawyer= yaml.load(file)['url']
with open('client_yaml/client_abb.yaml') as file:
self.url_abb= yaml.load(file)['url']
with open('client_yaml/client_staubli.yaml') as file:
self.url_tx60= yaml.load(file)['url']
self.ur_sub=RRN.SubscribeService(self.url_ur)
self.sawyer_sub=RRN.SubscribeService(self.url_sawyer)
self.abb_sub=RRN.SubscribeService(self.url_abb)
self.tx60_sub=RRN.SubscribeService(self.url_tx60)
UR_state=self.ur_sub.SubscribeWire("robot_state")
Sawyer_state=self.sawyer_sub.SubscribeWire("robot_state")
ABB_state=self.abb_sub.SubscribeWire("robot_state")
tx60_state=self.tx60_sub.SubscribeWire("robot_state")
#link and joint names in urdf
Sawyer_joint_names=["right_j0","right_j1","right_j2","right_j3","right_j4","right_j5","right_j6"]
UR_joint_names=["shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"]
Sawyer_link_names=["right_l0","right_l1","right_l2","right_l3","right_l4","right_l5","right_l6","right_l1_2","right_l2_2","right_l4_2","right_hand"]
UR_link_names=['UR_base_link',"shoulder_link","upper_arm_link","forearm_link","wrist_1_link","wrist_2_link","wrist_3_link"]
ABB_joint_names=['ABB1200_joint_1','ABB1200_joint_2','ABB1200_joint_3','ABB1200_joint_4','ABB1200_joint_5','ABB1200_joint_6']
ABB_link_names=['ABB1200_base_link','ABB1200_link_1','ABB1200_link_2','ABB1200_link_3','ABB1200_link_4','ABB1200_link_5','ABB1200_link_6']
tx60_joint_names=['tx60_joint_1','tx60_joint_2','tx60_joint_3','tx60_joint_4','tx60_joint_5','tx60_joint_6']
tx60_link_names=['tx60_base_link','tx60_link_1','tx60_link_2','tx60_link_3','tx60_link_4','tx60_link_5','tx60_link_6']
self.robot_state_list=[UR_state,Sawyer_state,ABB_state,tx60_state]
self.robot_link_list=[UR_link_names,Sawyer_link_names,ABB_link_names,tx60_link_names]
self.robot_joint_list=[UR_joint_names,Sawyer_joint_names,ABB_joint_names,tx60_joint_names]
self.num_robot=len(self.robot_state_list)
######tesseract environment setup:
self.t_env = Environment()
urdf_path = FilesystemPath("urdf/combined.urdf")
srdf_path = FilesystemPath("urdf/combined.srdf")
assert self.t_env.init(urdf_path, srdf_path, GazeboModelResourceLocator())
#update robot poses based on calibration file
self.t_env.changeJointOrigin("ur_pose", Isometry3d(H_UR))
self.t_env.changeJointOrigin("sawyer_pose", Isometry3d(H_Sawyer))
self.t_env.changeJointOrigin("abb_pose", Isometry3d(H_ABB))
self.t_env.changeJointOrigin("staubli_pose", Isometry3d(H_tx60))
contact_distance=0.2
monitored_link_names = self.t_env.getLinkNames()
self.manager = self.t_env.getDiscreteContactManager()
self.manager.setActiveCollisionObjects(monitored_link_names)
self.manager.setContactDistanceThreshold(contact_distance)
# viewer update
self.viewer = TesseractViewer()
self.viewer.update_environment(self.t_env, [0,0,0])
self.viewer.start_serve_background()
self.robot_def_dict={}
try:
UR=self.ur_sub.GetDefaultClientWait(1)
self.robot_def_dict['ur']=Robot(np.transpose(np.array(UR.robot_info.chains[0].H.tolist())),np.transpose(np.array(UR.robot_info.chains[0].P.tolist())),np.zeros(len(UR.robot_info.joint_info)))
except:
pass
try:
Sawyer=self.sawyer_sub.GetDefaultClientWait(1)
self.robot_def_dict['sawyer']=Robot(np.transpose(np.array(Sawyer.robot_info.chains[0].H.tolist())),np.transpose(np.array(Sawyer.robot_info.chains[0].P.tolist())),np.zeros(len(Sawyer.robot_info.joint_info)))
except:
pass
try:
ABB=self.abb_sub.GetDefaultClientWait(1)
self.robot_def_dict['abb']=Robot(np.transpose(np.array(ABB.robot_info.chains[0].H.tolist())),np.transpose(np.array(ABB.robot_info.chains[0].P.tolist())),np.zeros(len(ABB.robot_info.joint_info)))
except:
pass
try:
Staubli=self.tx60_sub.GetDefaultClientWait(1)
self.robot_def_dict['staubli']=Robot(np.transpose(np.array(Staubli.robot_info.chains[0].H.tolist())),np.transpose(np.array(Staubli.robot_info.chains[0].P.tolist())),np.zeros(len(Staubli.robot_info.joint_info)))
except:
pass
#trajectories
self.traj_change=False
self.traj_change_name=None
self.steps=300
self.plan_time=0.15
self.execution_delay=0.03
self.trajectory={'ur':np.zeros((self.steps,7)),'sawyer':np.zeros((self.steps,8)),'abb':np.zeros((self.steps,7)),'staubli':np.zeros((self.steps,7))}
self.traj_joint_names={'ur':['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'],
'sawyer':['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'],
'abb':['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'],
'staubli':['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
}
self.time_step=0.03
#initialize static trajectories
for key, value in self.trajectory.items():
for i in range(self.steps):
try:
value[i]=np.append([0],self.robot_state_list[self.dict[key]].InValue.joint_position)
except:
traceback.print_exc()
value[i]=np.append([0],[0,0,0,0,0,0])
self.inv={'ur':inv_ur,'sawyer':inv_sawyer,'abb':inv_abb,'staubli':inv_staubli}
self.joint_names_traj={'ur':inv_ur,'sawyer':inv_sawyer,'abb':inv_abb,'staubli':inv_staubli}
#register service constant
self.JointTrajectoryWaypoint = RRN.GetStructureType("com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint")
self.JointTrajectory = RRN.GetStructureType("com.robotraconteur.robotics.trajectory.JointTrajectory")