Commit 3f0911aa authored by Jayant Khatkar's avatar Jayant Khatkar

using two separate moveit instances, control arm from python commander (#5)

parent 7bd4597f
......@@ -13,7 +13,7 @@
<arg name="reverse_port" value="50001"/>
<arg name="script_sender_port" value="50002"/>
</include>
<!--include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/-->
<include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
</group>
<!-- ROBOT 2 -->
......@@ -24,7 +24,7 @@
<arg name="reverse_port" value="50003"/>
<arg name="script_sender_port" value="50004"/>
</include>
<!--include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/-->
<include file="$(find ur5_e_moveit_config)/launch/ur5_e_moveit_planning_execution.launch"/>
</group>
</launch>
<?xml version="1.0"?>
<launch>
<!-- ROBOT 1 -->
<group ns="r1">
<include file="$(find ur5_e_moveit_config)/launch/demo.launch"/>
</group>
<!-- ROBOT 2 -->
<group ns="r2">
<include file="$(find ur5_e_moveit_config)/launch/demo.launch"/>
</group>
</launch>
......@@ -62,12 +62,15 @@ class Controller(object):
hotend_srv='hotend',
retractionDist_mm=12.0,
retractionSpeed_mmpmin=4800.0,
disable_extruder=False):
disable_extruder=False,
robot=0):
"""
Creates a controller to multiple robot arms. (TODO)
close_deg: Euclidean distance of configurations within which two configurations are considered "close"
timeScale: Scaling factor of time
robot can be 0 (if only one robot is turned on (not using 2xbringup), 1 for r1, 2 for r2
"""
## Initialise ROS things
self.node = rospy.init_node('py_controller',
......@@ -93,9 +96,22 @@ class Controller(object):
# initialise MoveIt commander
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('manipulator')
if robot == 0:
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('manipulator')
else:
self.robot = moveit_commander.RobotCommander(
"/r" + str(robot) + "/robot_description",
ns="r" + str(robot)
)
self.scene = moveit_commander.PlanningSceneInterface(ns="r"+ str(robot))
self.group = moveit_commander.MoveGroupCommander(
'manipulator',
robot_description="r" + str(robot) + "/robot_description",
ns="r" + str(robot)
)
self.group.set_max_velocity_scaling_factor(timeScale)
self.group.set_max_acceleration_scaling_factor(timeScale**2)
......
......@@ -37,7 +37,8 @@ class KeyboardController(Controller):
homeConf_deg=np.array([-90,-90,45,-45,-90,-45]),
waitForExtruder=True,
timeScale=1.0,
disableExtruder=False):
disableExtruder=False,
robot=0):
"""
"""
if DEBUG:
......@@ -46,7 +47,8 @@ class KeyboardController(Controller):
super(KeyboardController,self).__init__(
timeScale=timeScale,
blocking=waitForExtruder,
disable_extruder=disableExtruder)
disable_extruder=disableExtruder,
robot=robot)
# 1x6
homeConf_rad = np.deg2rad(homeConf_deg)
......@@ -593,6 +595,7 @@ class KeyboardController(Controller):
if __name__=="__main__":
con = KeyboardController(disableExtruder=True)
#con = KeyboardController(disableExtruder=True, robot=2)
#con = KeyboardController()
print('Collecting configrations...')
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment