Beispiel #1
0
    def __init__(self, torso_center, lower_body_flag):
        self._body_center = torso_center
        self._lower_body_flag = lower_body_flag
        self.head = Head(Point(torso_center.x, torso_center.y+15, torso_center.z))
        self.torso = Torso(Point(torso_center.x, torso_center.y, torso_center.z))
        self.left_arm = Arm(Point(torso_center.x+6.6, torso_center.y+8, torso_center.z))
        self.right_arm = Arm(Point(torso_center.x-6.6, torso_center.y+8, torso_center.z))

        self.right_arm.set_slocal_rotate_angle(180, 0, 0)


        if self._lower_body_flag:
            self.left_leg = Leg(Point(torso_center.x+4, torso_center.y-10.6, torso_center.z))
            self.right_leg = Leg(Point(torso_center.x-4, torso_center.y-10.6, torso_center.z))
            self.right_leg.set_hlocal_rotate_angle(180, 0, 0)
    def __init__(self):
	self.arm = Arm(self.ARM_BASE, 100, 160, 200, 1, -20)	# TODO: Initialze arm object
	#self.paperPlot = PaperPlot()
	#self.forcePlot = ForcePlot()
	self.paper = Paper(self.PAPER_BASE)	# Initialize paper
	self.armPlot = ArmPlot() # Initialize the 3D plot
	self.armPlot.plotPaper(self.paper)
	self.armPlot.plotArm(self.arm)
	self.controller = DeltaController(self.arm, self.paper)
Beispiel #3
0
class Actuators:
	def __init__(self,tamp):
		self.tamp=tamp
				
		#motor left
		self.motorL = Motor(self.tamp, 2, 4)
		self.motorL.write(1,0)

		#motor right
		self.motorR = Motor(self.tamp,1, 3)
		self.motorR.write(1,0)

		#arm servo
		self.arm = Arm(self.tamp)

		#sorting servo
		self.sorter = Sorter(self.tamp)

	def update(self):
		self.arm.update()
		self.sorter.update()
Beispiel #4
0
	def __init__(self,tamp):
		self.tamp=tamp
				
		#motor left
		self.motorL = Motor(self.tamp, 2, 4)
		self.motorL.write(1,0)

		#motor right
		self.motorR = Motor(self.tamp,1, 3)
		self.motorR.write(1,0)

		#arm servo
		self.arm = Arm(self.tamp)

		#sorting servo
		self.sorter = Sorter(self.tamp)
    def __init__(self,obs_size,n_actions,n_hidden_channels=50):
        super().__init__()
        with self.init_scope():
            self.l0=L.Linear(obs_size,n_hidden_channels)
            self.l1=L.Linear(n_hidden_channels,n_hidden_channels*2)
            self.l2=L.Linear(n_hidden_channels*2,n_hidden_channels*2)
            self.l3=L.Linear(n_hidden_channels*2,n_actions)

    def __call__(self,x,test=False):
        h=F.tanh(self.l0(x))
        h=F.tanh(self.l1(h))
        h=F.tanh(self.l2(h))
        return chainerrl.action_value.DiscreteActionValue(self.l3(h))

if __name__ == "__main__":
    env=Arm(4,[1.,1.,1.,1.])
    print('observation space:',env.state)
    print('action space:',env.actions)
    obs=env.reset()

    #env.render()
    print('initial observation:',obs)

    action=env.random_action()
    print(action)
    state,r,done=env.step(action)
    print('next observation:',state)
    print('reward:',r)
    env.actions.shape[0]

    obs_size=env.state.shape[0]
    def __init__(self,
                 robot_config_file,
                 initRobotConfig,
                 timeStep,
                 odometryTimer=0.1,
                 defaultLogging=30):
        """
        Creates the main bot class.
        """
        if not os.path.exists(robot_config_file):
            raise ValueError(f'Cannot find configuration file '
                             f'"{robot_config_file}"')

        with open(robot_config_file, 'r') as f:
            self.robotConfiguration = json.load(f)

        # Adjust JSON just read
        self.__adjustConfigDict(self.robotConfiguration)

        # Exception Queue for inter threads exception communications
        self.exceptionQueue = queue.Queue()
        self.mqttMoveQueue = queue.Queue()
        self.baseMovementLock = asyncio.locks.Lock()
        self.timeStep = timeStep
        self.powerSupply = PowerSupply()

        # region sensor Init
        self.sensors = {}
        for sensor in self.robotConfiguration["sensors"]:
            sensorDict = self.robotConfiguration["sensors"][sensor]
            legoPort = LegoPort(sensorDict["input"])
            # Applies to both I2C and SDK controled sensors
            if legoPort.mode != sensorDict["mode"]:
                legoPort.mode = sensorDict["mode"]
            # Only applies to sensors controled by the python SDK
            if "set_device" in sensorDict:
                legoPort.set_device = sensorDict["set_device"]
                time.sleep(1)
            address = sensorDict["input"]
            self.sensors[sensor] = sensorDict["sensorType"](address=address)
        # endregion

        # base Init
        self.base = DiffDriveBase(self.robotConfiguration["base"],
                                  initRobotConfig, odometryTimer)

        # arm/endeffector Init
        self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"])

        # Other matrix init
        self.M0e = mr.RpToTrans(
            R=np.array(
                self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T,   # noqa F501
            p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"])   # noqa F501
        self.Toe_grip = mr.RpToTrans(
            R=np.array(
                self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T,   # noqa F501
            p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"])    # noqa F501
        self.Tbc = mr.RpToTrans(
            R=np.array(
                self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T,   # noqa F501
            p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"])
        self.Tcb = mr.TransInv(self.Tbc)

        # Reset all motors at init
        self.urgentStop = False
        log.info(LOGGER_DDR_MAIN, 'Done main robot init.')
Beispiel #7
0
from time import sleep
import SocketServer
# from OmegaArmHandler import OmegaArmHandler # creates an Arm instance in the handler script

# some constants for testing
reference_position = [-75, 150, 300]
reference_vec56 = [0, 1, 0]
reference_vec67 = [1, 0, 0]
reference_end_effector = 90  # open servo gripper
upright = [0, 160, 200]  # some upright position

# main function
# most of the stuff is done in the individual files
if __name__ == '__main__':
    # hard coded test values
    arm = Arm('COM6', 115200)  # initialize the arm

    # test setServoAngles
    print "Controlling the robot via angles."
    servoAngles = [90, 115, 65, 90, 90, 90, 90]  # robot reaches up to its left
    arm.setServoAngles(servoAngles)
    print "Angle command sent."
    sleep(2)

    print "Controlling the robot via angles."
    servoAngles = [90, 115, 110, 90, 90, 0, 0]  # robot reaches up to its left
    arm.setServoAngles(servoAngles)
    print "Angle command sent."
    sleep(2)

    print "Controlling the robot via angles."
Beispiel #8
0
sim.timeoflastreset = 0  # time when arm was last reseted

# train/test params
sim.trainTime = 1 * 1e3
sim.testTime = 1 * 1e3
sim.cfg.duration = sim.trainTime + sim.testTime
sim.numTrials = ceil(sim.cfg.duration / 1e3)
sim.numTargets = 1
sim.targetid = 3  # target to train+test
sim.trialTargets = [
    sim.targetid
] * sim.numTrials  #[i%sim.numTargets for i in range(int(sim.numTrials+1))] # set target for each trial

# create Arm class and setup
if sim.useArm:
    sim.arm = Arm(sim.animArm, sim.graphsArm)
    sim.arm.targetid = 0
    sim.arm.setup(sim)  # pass framework as argument


# Function to run at intervals during simulation
def runArm(t):
    # turn off RL and explor movs for last testing trial
    if t >= sim.trainTime:
        sim.useRL = False
        sim.explorMovs = False

    if sim.useArm:
        sim.arm.run(
            t, sim
        )  # run virtual arm apparatus (calculate command, move arm, feedback)
Beispiel #9
0
    def recluster(self, cluster_positions, cluster_fitnesses, labels):

        k = max(labels) + 1

        # Compare ranks to find unchanged arms
        old_arms_ranks = self.get_ranks()
        new_arms_ranks = self.get_ranks(cluster_fitnesses)
        if self.verbose:
            print('\nold ranks:')
            for ranks in old_arms_ranks:
                print(ranks)
            print('new ranks:')
            for ranks in new_arms_ranks:
                print(ranks)

        # Generate new arms
        new_arms = []
        matrices = [
            Matrix(positions, fitnesses, self.min_bounds,
                   self.max_bounds) for positions, fitnesses in zip(
                       cluster_positions, cluster_fitnesses)
        ]
        trans_samples = np.random.uniform(0,
                                          1,
                                          size=(self.n_samples,
                                                self.dimension))

        for new_index, new_ranks in enumerate(new_arms_ranks):

            new_set = set(new_ranks)
            arm = None

            # Get best positions for matrix optimization
            argmax = np.argmax(cluster_fitnesses[new_index])
            best = cluster_positions[new_index][argmax]

            # Get exclude positions for matrix optimization
            exclude = []
            for j in range(k):
                if new_index != j:
                    samples = matrices[j].inverse_transform(trans_samples)
                    exclude.extend(samples)
            exclude = np.array(exclude)

            # Case 1: Copy old arm with more than 50% similarities
            for old_index, old_ranks in enumerate(old_arms_ranks):

                intersection = new_set.intersection(old_ranks)
                duplicate_ratio = len(intersection) / max(
                    len(old_ranks), len(new_ranks))

                #if len(intersection) > max( len(old_ranks)/2, len(new_ranks)/2 ):
                if duplicate_ratio > 0.5:

                    # Optimize matrix
                    if self.verbose:
                        print('\nOptimizing matrix for new arm %d' % new_index)
                    matrices[new_index].optimize(cluster_positions[new_index],
                                                 cluster_fitnesses[new_index],
                                                 exclude)

                    # Copy unchanged arm to new arms
                    arm = self.arms[old_index]

                    # Transform Algorithm to new subspace
                    arm.transform(matrices[new_index])

                    # Find update particles
                    insert_ranks = new_set.difference(intersection)
                    remove = set(old_ranks).difference(new_set)

                    resize_ranks = sorted(deepcopy(new_ranks))
                    resize_ranks = resize_ranks[:self.n_points]

                    if self.verbose:
                        print('Copy arm %d -> %d: %.1f%% duplicate' %
                              (old_index, new_index, 100 * duplicate_ratio))
                        #print('intersection:', list(intersection))
                        #print('insert_ranks:', list(insert_ranks))
                        #print('remove_ranks:', list(remove))
                        #print('resize_ranks:', resize_ranks)

                        #print('\noriginal:')
                        #print(self.arms[old_index].get_positions())
                        #print(self.arms[old_index].get_fitnesses())
                        #print(old_ranks)

                        #print('\nnew:')
                        #print(cluster_positions[new_index])
                        #print(cluster_fitnesses[new_index])
                        #print(new_ranks)


                    replace_indices, replace_positions, replace_fitnesses = [], [], []
                    for i, rank in enumerate(old_ranks):
                        if rank not in resize_ranks:

                            # insert set is not empty
                            if insert_ranks:
                                insert_rank = insert_ranks.pop()
                                index = new_ranks.index(insert_rank)
                                replace_position = cluster_positions[
                                    new_index][index]
                                subspace_position = arm.matrix.transform(
                                    [replace_position])[0]
                                replace_fitness = cluster_fitnesses[new_index][
                                    index]
                                #if self.verbose:
                                #if self.verbose:
                                #    print('old_rank[%d] = %d is replaced by rank %d, f = %f, pos='
                                #          % (i, rank, insert_rank, replace_fitness), replace_position)

                            # Replace particle with a random sample
                            else:
                                subspace_position = np.random.uniform(
                                    0, 1, size=(self.dimension))
                                replace_position = arm.matrix.inverse_transform(
                                    [subspace_position])[0]
                                replace_fitness = self.obj(replace_position)
                                #if self.verbose:
                                #    print('old_rank[%d] = %d is replaced by sample point f = %f, pos='
                                #          % (i, rank, replace_fitness), replace_position)

                            replace_indices.append(i)
                            replace_positions.append(subspace_position)
                            replace_fitnesses.append(replace_fitness)

                    try:
                        arm.algo.replace(replace_indices, replace_positions,
                                         replace_fitnesses)
                    except AssertionError as e:
                        arm = None

                        for i, p, f in zip(replace_indices, replace_positions,
                                           replace_fitnesses):
                            print(i, p, f)
                        input()
                        break
                    arm.update_model()
                    break

            # Case 2: Create new arm
            if arm is None:
                arm = Arm(self.obj,
                          self.n_points,
                          cluster_positions[new_index],
                          cluster_fitnesses[new_index],
                          algo_type=self.algo_type,
                          exclude=exclude,
                          min_bounds=self.min_bounds,
                          max_bounds=self.max_bounds)

            # Append new_arm and update matrix
            new_arms.append(arm)
            matrices[new_index] = new_arms[new_index].matrix

        self.arms = new_arms
Beispiel #10
0
		self.config_ax.set_ylim(-0.1, 2*pi + 0.1)
		self.op_ax.set_xlim(-10.1, 10.1)
		self.op_ax.set_ylim(-5.1, 15.1)
		self.config_ax.axvline(self.arm.control_links[0].min_angle)
		self.config_ax.axvline(self.arm.control_links[0].max_angle)
		self.config_ax.axhline(self.arm.control_links[1].min_angle)
		self.config_ax.axhline(self.arm.control_links[1].max_angle)
		self.config_ax.set_xlabel('$\Theta_1$')
		self.config_ax.set_ylabel('$\Theta_2$')
		self.op_ax.set_xlabel('X')
		self.op_ax.set_ylabel('Y')

		self.fig.canvas.draw()


if __name__ == '__main__':
	l1 = FixedLink(length = 5, angle = pi/2)
	l2 = FixedLink(length = 0, angle = -pi/2)
	l3 = Link(length = 6, max_angle=2*pi)
	l4 = FixedLink(length = 0, angle = -pi/2)
	l5 = Link(length = 4, max_angle=2*pi)
	l6 = FixedLink(length = 0, angle = -pi/2)
	l7 = Link(length = 4, max_angle=2*pi)
	arm = Arm([l1, l3, l5])
	print(arm)
	gui = GUI(arm)




Beispiel #11
0
 def __init__(self):
     self.arm = Arm()
     self.hand = Hand()
Beispiel #12
0
io_reader.Start()
arm_config = ArmConfig()
arm_config.ApplyTares(bank, io_reader)
#   while True:
#       s = "\n------\n"
#       for i in range(22, 32, 2):
#           s += " PIN %d = %d\n" % (i, io_reader.GetPin(i))

#       logging.info("RPS = %f %s", io_reader.ReadsPerSec(), s)
#       time.sleep(0.2)
bank.Release()
bank.Rezero()
time.sleep(5.0)

hand = Hand(bank, io_reader, arm_config)
arm = Arm(bank, io_reader, arm_config)
#hand.Dispense()
#   if True:
#       hand.Grab()
#       bank.WaitDone()
#       time.sleep(5)
#       logging.info("Release now.")
#       hand.Release()
#       bank.WaitDone()
#   while True:
#       logging.info("Wait here forever.")
#       time.sleep(10)
hand.Calibrate()
#bank.WaitDone()
bank.base_motor.SetDisableAfterMoving(False)
arm.Rotate(-math.pi / 2)
Beispiel #13
0
PORT = 8765

RIGHT_ARM_INDEX = 0
LEFT_ARM_INDEX = 50

decoder = json.JSONDecoder()

robot = piarm.PiArm()
robot.connect("/dev/ttyAMA0")

DEFAULT_RESPONSE = "ack".encode()
CLIENT_ERROR = "CLIENT error".encode()
SERVER_ERROR = "SERVER error".encode()

ARMS = {
    'Right': Arm(robot, RIGHT_ARM_INDEX),
    'Left': Arm(robot, LEFT_ARM_INDEX)
}


def client_thread(conn, robot_arm):
    # conn.send(":smile:")
    while True:
        try:
            data = conn.recv(1024)  # buffer size is 1024 bytes
        except socket.timeout:
            print('Socket timed out')
            break

        # This check will ensure that empty streams, generally caused by
        # unorderly terminating a socket connection at the client side
class ArmSim(object):
    # ===== Paper location input for simulation =====
    Rp = Rrpy(pi/2,0,0) # Roll-pitch-yaw rotation parameterization
    PAPER_BASE = np.identity(4)		      # Paper frame rigid body transform
    PAPER_BASE[0:3,0:3] = Rp		      # Paper rotation matrix
    PAPER_BASE[0:3,3]  = np.asfarray([-Paper.X_SIZE/2,-50,0]) # Paper origin
    
    # Arm Location
    Ra = Rrpy(-pi/2-0.05,pi/2,0)		# Arm Roll-Pitch-Yaw base orientation parameterization
    ARM_BASE = np.identity(4)			# Arm fixed-base rigid body transform
    ARM_BASE[0:3,3] = np.asfarray([250,-50,0])	# Arm origin 
    ARM_BASE[0:3,0:3] = Ra			# Set rotation
		 
    INITIAL_CONFIG = np.asfarray([pi/4,pi/4,0])	# Initial arm joint configuration
    
   # ===== Set of waypoints on paper =====
    WAYPOINTS = [[[10,0],[10,30]],[[10,15],[20,15]],[[20,0],[20,30]]]

    def __init__(self):
	self.arm = Arm(self.ARM_BASE, 100, 160, 200, 1, -20)	# TODO: Initialze arm object
	#self.paperPlot = PaperPlot()
	#self.forcePlot = ForcePlot()
	self.paper = Paper(self.PAPER_BASE)	# Initialize paper
	self.armPlot = ArmPlot() # Initialize the 3D plot
	self.armPlot.plotPaper(self.paper)
	self.armPlot.plotArm(self.arm)
	self.controller = DeltaController(self.arm, self.paper)
    
    # Run the simulation
    def run(self, strokes, initialConfig, minStep=100):
	initialConfig = np.asfarray(initialConfig)
	current = initialConfig	# Current configuration
	self.worldWaypoints = self.paper.strokesToWorld(strokes)
	
	# Loop over waypoints
	#for i in range(0,len(strokes)):
	'''
	ikConfig = np.append(self.arm.planarIK(strokes[i][0]),[50])# Compute IK
	print str(ikConfig)
	nsteps = minStep
	configs = interpolateLinear(current, ikConfig, nsteps)  # Interpolate trajectory
	print configs.shape'''
	    
	configs = self.controller.generateTrajectory(strokes)
	
	# Wait for replay
	# Loop over interpolated configurations
	while(True):
	    for k in range(0, len(configs)):
		print 'Step', k
		#print str(configs[k])
		self.arm.setConfiguration(self.rx64RoundConfig(configs[k])) # Update arm position
		self.armPlot.clear()
		self.armPlot.plotArm(self.arm) 	# Plot
		self.armPlot.plotPaper(self.paper)	# Plot paper again
		self.armPlot.plotIdealPath(self.worldWaypoints)
		#print 'Arm Position', str(self.arm.eePosition())
		self.arm.printEEPosition()
		draw()
		self.armPlot.fig.show()
		sleep(0.0001)
	    c = input('Enter some string to continue')
	    #current = ikConfig
    
    # Round the configuration to RX64 angles
    def rx64RoundConfig(self, config, randomness=0):
	nbits = 10
	rx64Range = 5.0*pi/3.0	
	dConfig = (config/rx64Range) * pow(2.0, nbits)
	#print 'Discrete configuration:', str(dConfig)
	dConfig = np.round(dConfig)
	roundedConfig = dConfig/pow(2.0, nbits) * rx64Range
	#print 'Rounded Configuration', str(roundedConfig)
	# Generate Gaussian
	return roundedConfig
class Robot_Interface(object):
    """For usage with the Fetch robot."""

    def __init__(self, simulation=True):
        """Initializes various aspects of the Fetch.
        
        TODOs: get things working, also use `simulation` flag to change ROS
        topic names if necessary (especially for the cameras!). UPDATE: actually
        I don't think this is necessary now, they have the same topic names.
        """
        rospy.init_node("fetch")
        self.arm = Arm()
        self.arm_joints = ArmJoints()
        self.base = Base()
        self.camera = RGBD()
        self.head = Head()
        self.gripper = Gripper(self.camera)
        self.torso = Torso()
        self.joint_reader = JointStateReader()

        # Tucked arm starting joint angle configuration
        self.names = ArmJoints().names()
        self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0]
        self.tucked_list = [(x,y) for (x,y) in zip(self.names, self.tucked)]

        # Initial (x,y,yaw) position of the robot wrt map origin. We keep this
        # fixed so that we can reset to this position as needed. The HSR's
        # `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is
        # the rotation about that axis (intuitively, the z axis). For the base,
        # `base.odom` supplies both `position` and `orientation` attributes.
        start = copy.deepcopy(self.base.odom.position)
        yaw = Base._yaw_from_quaternion(self.base.odom.orientation)
        self.start_pose = np.array([start.x, start.y, yaw])
        self.TURN_SPEED = 0.3

        self.num_restarts = 0


    def body_start_pose(self, start_height=0.10, end_height=0.10, velocity_factor=None):
        """Sets the robot's body to some initial configuration.
        
        The HSR uses `whole_body.move_to_go()` which initializes an appropriate
        posture so that the hand doesn't collide with movement. For the Fetch,
        we should probably make the torso extend, so the arms can extend more
        easily without collisions. Use `move_to_joint_goal` since that uses
        motion planning. Do NOT directly set the joints without planning!!
        """
        self.torso.set_height(start_height)
        self.arm.move_to_joint_goal(self.tucked_list, velocity_factor=velocity_factor)
        self.torso.set_height(end_height)
        # Specific to the siemens challenge (actually a lot of this stuff is ...)
        if self.num_restarts == 0:
            self.base.turn(angular_distance=45*DEG_TO_RAD)
            self.num_restarts += 1


    def head_start_pose(self):
        """Hard-coded starting pose for the robot's head.
        
        These values are from the HSR. The Fetch needs a different pan and tilt.
        Positive pan means rotating counterclockwise when looking at robot from
        an aerial view.
        """
        self.head.pan_tilt(pan=0.0, tilt=0.8)


    def position_start_pose(self, offsets=None, do_something=False):
        """Assigns the robot's base to some starting pose.

        Mainly to "reset" the robot to the original starting position (and also,
        rotation about base axis) after it has moved, usually w/no offsets.
        
        Ugly workaround: we have target (x,y), and compute the distance to the
        point and the angle. We turn the Fetch according to that angle, and go
        forward. Finally, we do a second turn which corresponds to the target
        yaw at the end. This turns w.r.t. the current angle, so we undo the
        effect of the first turn.  See `examples/test_position_start_pose.py`
        for tests.
        
        Args:
            offsets: a list of length 3, indicating offsets in the x, y, and
            yaws, respectively, to be added onto the starting pose.
        """
        # Causing problems during my tests of the Siemens demo.
        if not do_something:
            return


        current_pos = copy.deepcopy(self.base.odom.position)
        current_theta = Base._yaw_from_quaternion(self.base.odom.orientation) # [-pi, pi]
        ss = np.array([current_pos.x, current_pos.y, current_theta])

        # Absolute target position and orientation specified with `pp`.
        pp = np.copy(self.start_pose)
        if offsets:
            pp += np.array(offsets)

        # Get distance to travel, critically assumes `pp` is starting position.
        dist = np.sqrt( (ss[0]-pp[0])**2 + (ss[1]-pp[1])**2 )
        rel_x = ss[0] - pp[0]
        rel_y = ss[1] - pp[1]
        assert -1 <= rel_x / dist <= 1
        assert -1 <= rel_y / dist <= 1

        # But we also need to be *facing* the correct direction, w/input [-1,1].
        # First, get the opposite view (facing "outwards"), then flip by 180.
        desired_facing = np.arctan2(rel_y, rel_x) # [-pi, pi], facing outward
        desired_theta  = math.pi + desired_facing # [0, 2*pi], flip by 180
        if desired_theta > math.pi:
            desired_theta -= 2*math.pi # [-pi, pi]
 
        # Reconcile with the current theta. Got this by basically trial/error
        angle = desired_theta - current_theta  # [-2*pi, 2*pi]
        if angle > math.pi:
            angle -= 2*math.pi 
        elif angle < -math.pi:
            angle += 2*math.pi 

        self.base.turn(angular_distance=angle, speed=self.TURN_SPEED)
        self.base.go_forward(distance=dist, speed=0.2)

        # Back at the start x, y, but now need to consider the _second_ turn.
        # The robot is facing at `desired_theta` rads, but wants `pp[2]` rads.
        final_angle = pp[2] - desired_theta
        if final_angle > math.pi:
            final_angle -= 2*math.pi 
        elif final_angle < -math.pi:
            final_angle += 2*math.pi 
        self.base.turn(angular_distance=final_angle, speed=self.TURN_SPEED)


    def get_img_data(self):
        """Obtain camera and depth image.
        
        Returns:
            Tuple containing RGB camera image and corresponding depth image.
        """
        c_img = self.camera.read_color_data()
        d_img = self.camera.read_depth_data()
        return (c_img, d_img)


    def get_depth(self, point, d_img):
        """Compute mean depth near grasp point.

        NOTE: assumes that we have a simlar `cfg.ZRANGE` as with the HSR. I'm
        not sure where exactly this comes from.
        """
        y, x = int(point[0]), int(point[1])
        z_box = d_img[y-ZRANGE:y+ZRANGE, x-ZRANGE:x+ZRANGE]
        indx = np.nonzero(z_box)
        z = np.mean(z_box[indx])
        return z


    def get_rot(self, direction):
        """Compute rotation of gripper such that given vector is grasped.

        Currently this directly follows the HSR code as there's nothing
        Fetch-dependent.
        """
        dy, dx = direction[0], direction[1]
        dx *= -1
        if dy < 0:
            dx *= -1
            dy *= -1
        rot = np.arctan2(dy, dx)
        rot = np.pi - rot
        return rot


    def create_grasp_pose(self, x, y, z, rot):
        """ If `intuitive=True` then x,y,z,rot interpreted wrt some link in the
        world, e.g., 'odom' for the Fetch. It's False by default to maintain
        backwards compatibility w/Siemens-based code.
        """
        pose_name = self.gripper.create_grasp_pose(x, y, z, rot)
        return pose_name

        
    def open_gripper(self):
        self.gripper.open()


    def close_gripper(self):
        self.gripper.close()


    def move_to_pose(self, pose_name, z_offset, velocity_factor=None):
        """Moves to a pose.
 
        In the HSR, moved the `hand_palm_link` to the frame named `pose_name` at
        the correct pose. For the Fetch we should be able to extract the pose
        from `pose_name` and then call the Arm's `move_to_pose` method.
        
        Args:
            pose_name: A string name for the pose to go 
            z_offset: Scalar offset in z-direction, offset is wrt the pose
                specified by `pose_name`.
            velocity_factor: controls the speed, closer to 0 means slower,
                closer to 1 means faster. (If 0.0, then it turns into 1.0 for
                some reason.) Values greater than 1.0 are cut to 1.0.
        """
        # See: 
        #   http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29
        #   https://answers.ros.org/question/256354/does-tftransformlistenerlookuptransform-return-quaternion-position-or-translation-and-rotation/
        # First frame should be the reference frame, use `base_link`, not `odom`.
        point, quat = self.gripper.tl.lookupTransform('base_link', pose_name, rospy.Time(0))
        z_point = point[2] + z_offset

        # See:
        #   https://github.com/cse481wi18/cse481wi18/blob/indigo-devel/applications/scripts/cart_arm_demo.py
        #   https://github.com/cse481wi18/cse481wi18/wiki/Lab-19%3A-Cartesian-space-manipulation
        ps = PoseStamped()
        ps.header.frame_id = 'base_link'
        ps.pose = Pose(
                Point(point[0], point[1], z_point), 
                Quaternion(quat[0], quat[1], quat[2], quat[3])
        )

        # See `arm.py` written by Justin Huang
        error = self.arm.move_to_pose(pose_stamped=ps, velocity_factor=velocity_factor)
        if error is not None:
            rospy.logerr(error)


    def find_ar(self, ar_number, velocity_factor=None):
        try:
            ar_name = 'ar_marker/' + str(ar_number)

            # HSR code, with two hard-coded offsets?
            #self.whole_body.move_end_effector_pose(geometry.pose(y=0.08, z=-0.3), ar_name)

            # Fetch 'translation'. Note the `ar_name` for pose name.
            point, quat = self.gripper.tl.lookupTransform('base_link', ar_name, rospy.Time(0))
            y_point = point[1] + 0.08
            z_point = point[2] - 0.3

            ps = PoseStamped()
            ps.header.frame_id = 'base_link'
            ps.pose = Pose(
                    Point(point[0], y_point, z_point), 
                    Quaternion(quat[0], quat[1], quat[2], quat[3])
            )

            error = self.arm.move_to_pose(pose_stamped=ps, velocity_factor=velocity_factor)
            if error is not None:
                rospy.logerr(error)

            return True
        except:
            return False


    def pan_head(self, tilt):
        """Adjusts tilt of the robot, AND set pan at zero.
        
        Args: 
            tilt: Value in radians, positive means looking downwards.
        """
        self.head.pan_tilt(pan=0, tilt=tilt)
Beispiel #16
0
class EyeInHand:
    def __init__(self, limb_name):
        self.limb_name = limb_name
        self.arm = Arm(limb_name)
        self.available_tread = True
        if limb_name == 'left':
            self.other_limb_name = 'right'
        else:
            self.other_limb_name = 'left'
        self.otherArm = Arm(self.other_limb_name)
        #self.otherArm.tuck_arm()
        self.cards = np.array([])
        self.bridge = CvBridge()
        self.cards = Cards()
        #rospy.init_node(limb_name, anonymous=True)
        self.display_pub = rospy.Publisher('/robot/xdisplay', Image)
        head_camera = baxter_interface.CameraController('head_camera')
        head_camera.close()
        self.other_camera = baxter_interface.CameraController(
            self.other_limb_name + '_hand_camera')
        self.other_camera.close()
        self.camera = baxter_interface.CameraController(self.limb_name +
                                                        '_hand_camera')
        self.camera.open()
        self.arm.calibrate()
        resp = 'n'
        while resp is not 'y':
            resp = str(
                raw_input(
                    'Please place the gripper arm at the center of the card holder, press "y" to continue: '
                ))
        self.arm.go_home()
        if self.available_tread:
            cam_thread = threading.Thread(target=self.get_camera)
            choices_thread = threading.Thread(target=self.pick_choices)
            cam_thread.start()
            choices_thread.start()
        rospy.spin()
        cv2.destroyAllWindows()

    def get_camera(self):
        self.available_tread = False
        sub = rospy.Subscriber(
            '/cameras/' + self.limb_name + '_hand_camera/image', Image,
            self.callback, None, 1)

    def callback(self, msg):
        self.cards.img = self.bridge.imgmsg_to_cv2(msg)
        cv2.imshow('main', self.cards.img)
        cv2.waitKey(1)
        self.corners, self.contours, self.hierarchy = self.cards.get_features(
            (5, 5))
        for iterations, corner in enumerate(self.corners):
            card_img = self.cards.extract_card(corner)
            self.cards.append(card_img)
            cv2.imshow(str(iterations), card_img)

    def pick_choices(self):
        while not rospy.is_shutdown():
            card_response = 100
            while (card_response < 0
                   or card_response > 3) and not rospy.is_shutdown():
                card_response = int(
                    input(
                        'please pick a card index, where the left most corner is 0 and the right most corner is 3: '
                    ))
                self.arm.choose_card(card_response)
                if card_response < 0 or card_response > 3:
                    print(
                        'index out of range! please choose a number between 0 and 3!'
                    )
Beispiel #17
0
from math import pi

from stepper_motors.stepper import Stepper

from arm import Arm
from link import Link, FixedLink
from arm_astar import ArmAStar
from arm_gui import ArmGUI

if __name__ == '__main__':
    """
    Script to run the A* code on the physical workshop arm.
    """
    lf1 = FixedLink(length=0, angle=pi / 2)
    l1 = Link(length=11.8, angle=0, min_angle=-1e4, max_angle=1e4)
    lf2 = FixedLink(length=.12, angle=pi / 2)
    lf3 = FixedLink(length=0, angle=-pi / 2)
    l2 = Link(length=11.8, angle=0, min_angle=-1e4, max_angle=1e4)

    s2 = Stepper(2, 3, 4, 17, delay=1e-2)
    s1 = Stepper(27, 22, 10, 9, delay=1e-2)
    s1.reverse()

    arm = Arm([lf1, l1, lf2, lf3, l2], [s1, s2])

    print(arm)
    astar = ArmAStar(arm, discretization=3 * pi / 180, min_dist=0.5)
    gui = ArmGUI(arm, astar)
xboxCont = XboxController(controlCallBack, deadzone = 30, scale = 100, invertYAxis = True)

#start the xbox controller
xboxCont.start()
#=================================================

kin = Kinematic()

#Clear Log File
open('logs/log.txt', 'w').close()

with open('config/config.json') as json_file:  
    config = json.load(json_file)

#Create instance of Arm class for each arm
arm1 = Arm(config)
arm2 = Arm(config, "192.168.0.106")

arm = []
arm.append(arm1)
arm.append(arm2)

arm[0].resetEStop()
arm[1].resetEStop()

#calibrate both arms
arm[0].calibrateArm()
arm[1].calibrateArm()

while(not (arm[0].armCalibrated() and arm[1].armCalibrated())):
    time.sleep(1)
Beispiel #19
0
# -*- coding: utf-8 -*
from car import Car
from arm import Arm
import time

if __name__ == "__main__":
    #    car = Car()
    #    car.capture()
    #    time.sleep(1)
    #    car.unload()
    arm = Arm()
    #	for i in range(2500, 0, -10):
    arm.servo_angle(arm.s1, 2500)
    time.sleep(0.01)
Beispiel #20
0
import numpy as np

from arm.arm_directions import ArmDirections
from arm.move_arm import move_arm
import arm.Arm

default_arm_positions = [np.pi / 2, np.pi / 2, 0]

## create the robot arm
arm = Arm.Arm3Link(q=default_arm_positions, L=np.array([90, 60, 48]))

print('the old angles are:')
print(arm.q)
print(arm.get_xy())

old_angles = default_arm_positions

# x, y = -100, 80

x, y = -70, 0

arm.q = arm.inv_kin([x, y])

new_angles = arm.q

print('new angle is:')
print(arm.q)
print(arm.get_xy())

move_arm(old_angles[0] - new_angles[0], ArmDirections.up(),
         ArmDirections.down())
Beispiel #21
0
from room2 import Room
from rotplate import RotationPlate
from connection_cylinder import ConnectionCylinder
from topcylinder import TopCylinder
from arm import Arm
from arm2 import Arm2
from camerabody import CameraRoom
from topmotor_holder import TopMotorHolder
from wire_conductor import WireConductor

if __name__ == "__main__":
    room = Room()
    plate = RotationPlate()
    concyl = ConnectionCylinder()
    topcylinder = TopCylinder()
    arm0 = Arm()
    arm1 = Arm2()
    camera = CameraRoom()
    wire_conductor = WireConductor()

    topmotor_holder = TopMotorHolder()

    concyl.set_color(0, 1, 0, 0)

    topcylinder.rotateZ(deg(90))

    room.socket.link(plate)
    plate.socket.link(concyl)
    concyl.socket.link(topcylinder)

    topcylinder.socket0.link(arm0)
Beispiel #22
0
class Application:
    def __init__(self, configfile, map_name, human=True, fps=DEFAULT_FPS):
        self.running = False
        self.displaySurface = None
        self.config = configparser.ConfigParser()
        self.config.read(configfile)
        self.fps = fps
        self.__human = human
        self.clock = pygame.time.Clock()
        self.trajectory = []

        # Parse config file
        self.windowTitle = "CS440 MP2 Robotic Arm"
        self.window = eval(self.config.get(map_name, 'Window'))

        armBase = eval(self.config.get(map_name, 'ArmBase'))
        armLinks = eval(self.config.get(map_name, 'ArmLinks'))
        self.armLimits = [(0, 0), (0, 0), (0, 0)]
        for i in range(len(armLinks)):
            self.armLimits[i] = armLinks[i][-1]
        self.arm = Arm(armBase, armLinks)

        self.obstacles = eval(self.config.get(map_name, 'Obstacles'))
        self.goals = eval(self.config.get(map_name, 'Goals'))

    # Initializes the pygame context and certain properties of the maze
    def initialize(self):

        pygame.init()
        self.displaySurface = pygame.display.set_mode(
            (self.window[0], self.window[1]), pygame.HWSURFACE)
        self.displaySurface.fill(WHITE)
        pygame.display.flip()
        pygame.display.set_caption(self.windowTitle)
        self.running = True

    # Once the application is initiated, execute is in charge of drawing the game and dealing with the game loop
    def execute(self, searchMethod, granularity, trajectory, saveImage,
                saveMaze):
        self.initialize()
        if not self.running:
            print("Program init failed")
            raise SystemExit

        currAngle = [0, 0, 0]
        for i in range(len(self.arm.getArmAngle())):
            currAngle[i] = self.arm.getArmAngle()[i]
        self.gameLoop()

        if not self.__human:
            print("Transforming a map configuration to a maze...")
            maze = transformToMaze(self.arm, self.goals, self.obstacles,
                                   self.window, granularity)
            #Save testfile
            maze.saveToFile('TestTransform.txt')
            print("Done!")
            print("Searching the path...")
            path = search(maze, searchMethod)
            if path is None:
                print("No path found!")
            else:
                for i in range(len(path)):
                    self.arm.setArmAngle(path[i])
                    if (trajectory > 0) and (i % trajectory == 0):
                        self.trajectory.append(self.arm.getArmPos())
                    self.gameLoop()
                print("Done!")
                self.drawTrajectory()

        while self.running:
            pygame.event.pump()
            keys = pygame.key.get_pressed()

            if (keys[K_ESCAPE]):
                self.running = False

            if self.__human:
                alpha, beta, gamma = currAngle
                if (keys[K_z]):
                    alpha += granularity if isValueInBetween(
                        self.armLimits[ALPHA], alpha + granularity) else 0

                if (keys[K_x]):
                    alpha -= granularity if isValueInBetween(
                        self.armLimits[ALPHA], alpha - granularity) else 0

                if (keys[K_a]):
                    beta += granularity if isValueInBetween(
                        self.armLimits[BETA], beta + granularity) else 0

                if (keys[K_s]):
                    beta -= granularity if isValueInBetween(
                        self.armLimits[BETA], beta - granularity) else 0

                if (keys[K_q]):
                    gamma += granularity if isValueInBetween(
                        self.armLimits[GAMMA], gamma + granularity) else 0

                if (keys[K_w]):
                    gamma -= granularity if isValueInBetween(
                        self.armLimits[GAMMA], gamma - granularity) else 0

                newAngle = (alpha, beta, gamma)
                tempArm = copy.deepcopy(self.arm)
                tempArm.setArmAngle(newAngle)
                armEnd = tempArm.getEnd()
                armPos = tempArm.getArmPos()
                armPosDist = tempArm.getArmPosDist()

                print("doesArmTouchObjects",
                      doesArmTouchObjects(armPosDist, self.obstacles))

                # print(armPosDist)

                if doesArmTouchObjects(
                        armPosDist, self.obstacles) or not isArmWithinWindow(
                            armPos, self.window):
                    continue

                if not doesArmTipTouchGoals(
                        armEnd, self.goals) and doesArmTouchObjects(
                            armPosDist, self.goals, isGoal=True):
                    continue

                self.arm.setArmAngle(newAngle)
                self.gameLoop()
                currAngle = copy.deepcopy(newAngle)

                if doesArmTipTouchGoals(armEnd, self.goals):
                    self.gameLoop()
                    print("SUCCESS")
                    raise SystemExit

        if saveImage:
            pygame.image.save(self.displaySurface, saveImage)

        if saveMaze and not self.__human:
            maze.saveToFile(saveMaze)

    def gameLoop(self):
        self.clock.tick(self.fps)
        self.displaySurface.fill(WHITE)
        self.drawTrajectory()
        self.drawArm()
        self.drawObstacles()
        self.drawGoal()
        pygame.display.flip()

    def drawTrajectory(self):
        cnt = 1
        for armPos in self.trajectory:
            x = (255 - 255 / len(self.trajectory) * cnt)
            color = (x, x, x)
            cnt += 1
            for i in range(len(armPos)):
                pygame.draw.line(self.displaySurface, color, armPos[i][0],
                                 armPos[i][1], ARM_LINKS_WIDTH[i])

    def drawArm(self):
        armPos = self.arm.getArmPos()
        for i in range(len(armPos)):
            pygame.draw.line(self.displaySurface, BLACK, armPos[i][0],
                             armPos[i][1], ARM_LINKS_WIDTH[i])

    def drawObstacles(self):
        for obstacle in self.obstacles:
            pygame.draw.circle(self.displaySurface, RED,
                               (obstacle[0], obstacle[1]), obstacle[2])

    def drawGoal(self):
        for goal in self.goals:
            pygame.draw.circle(self.displaySurface, BLUE, (goal[0], goal[1]),
                               goal[2])
Beispiel #23
0
from __future__ import division
import numpy as np
from arm import Arm
import math

###############################################################################
# CONSTANTS AND FUNCTIONS
num_arms = 3
time_horizon = 50
###############################################################################

all_arms = [Arm(0, 1), Arm(1, 1), Arm(1.1, 1)]
valid_indices = set(range(num_arms))
delta = 1
num_rounds = int(math.floor(0.5 * math.log(time_horizon / math.exp(1), 2)))
print num_rounds

iteration = 0
reward = 0
for m in range(num_rounds):
    counts = [0] * num_arms
    n_m = int(math.ceil(2 * math.log(time_horizon * delta**2) / (delta**2)))
    goal = [n_m if i in valid_indices else 0 for i in range(num_arms)]
    while counts != goal:
        i = np.random.choice(list(valid_indices))
        if counts[i] >= n_m:
            continue
        new_reward = all_arms[i].sample()
        counts[i] += 1
        reward += new_reward
        print "\nIteration: %d" % iteration
Beispiel #24
0
class Body:
    def __init__(self, torso_center, lower_body_flag):
        self._body_center = torso_center
        self._lower_body_flag = lower_body_flag
        self.head = Head(Point(torso_center.x, torso_center.y+15, torso_center.z))
        self.torso = Torso(Point(torso_center.x, torso_center.y, torso_center.z))
        self.left_arm = Arm(Point(torso_center.x+6.6, torso_center.y+8, torso_center.z))
        self.right_arm = Arm(Point(torso_center.x-6.6, torso_center.y+8, torso_center.z))

        self.right_arm.set_slocal_rotate_angle(180, 0, 0)


        if self._lower_body_flag:
            self.left_leg = Leg(Point(torso_center.x+4, torso_center.y-10.6, torso_center.z))
            self.right_leg = Leg(Point(torso_center.x-4, torso_center.y-10.6, torso_center.z))
            self.right_leg.set_hlocal_rotate_angle(180, 0, 0)

    def horiz_move(self, gx, gy, gz):
        movement = tf.translate(Vector(gx, gy, gz)) # Affine matrix for horizontal movement.
        self._body_center = movement * self._init_torso_center

        self.head.horiz_move(x, y, z)
        self.torso.horiz_move(x, y, z)
        self.left_arm.horiz_move(x, y, z)
        self.right_arm.horiz_move(x, y, z)

        if self._lower_body_flag:
            self.left_leg.horiz_move(x, y, z)
            self.right_leg.horiz_move(x, y, z)

    def calc_body_center(self):
        return self._body_center

    def get_property_list(self):
        property_list = []
        property_list.extend(self.head.get_property_list())
        property_list.extend(self.torso.get_property_list())
        property_list.extend(self.left_arm.get_property_list())
        property_list.extend(self.right_arm.get_property_list())

        if self._lower_body_flag:
            property_list.extend(self.left_leg.get_property_list())
            property_list.extend(self.right_leg.get_property_list())

        return property_list

    def constr_json_data(self):
        # Construcat JSON data
        json_data_head = self.head.constr_json_data()
        json_data_torso = self.torso.constr_json_data()
        json_data_left_arm = self.left_arm.constr_json_data()
        json_data_right_arm = self.right_arm.constr_json_data()
        
        if self._lower_body_flag:
            json_data_left_leg = self.left_leg.constr_json_data()
            json_data_right_leg= self.right_leg.constr_json_data()

        json_data = {
            'joint_angle': {
                'left_shoulder': json_data_left_arm['joint_angle']['shoulder'],
                'left_elbow': json_data_left_arm['joint_angle']['elbow'],
                'right_shoulder': json_data_right_arm['joint_angle']['shoulder'],
                'right_elbow': json_data_right_arm['joint_angle']['elbow']

            },
            'true_position': {
                'head': json_data_head['true_position']['head'],
                'torso': json_data_torso['true_position']['torso'],
                'left_shouler': json_data_left_arm['true_position']['shoulder'],
                'left_elbow': json_data_left_arm['true_position']['elbow'],
                'left_hand': json_data_left_arm['true_position']['hand'],
                'right_shoulder': json_data_right_arm['true_position']['shoulder'],
                'right_elbow': json_data_right_arm['true_position']['elbow'],
                'right_hand': json_data_right_arm['true_position']['hand']
            }
        }

        if self._lower_body_flag:
            json_data['joint_angle']['left_hip'] = json_data_left_leg['joint_angle']['hip']
            json_data['joint_angle']['left_knee'] = json_data_left_leg['joint_angle']['knee']
            json_data['joint_angle']['right_hip'] = json_data_right_leg['joint_angle']['hip']
            json_data['joint_angle']['right_knee'] = json_data_right_leg['joint_angle']['knee']
            json_data['true_position']['left_hip'] = json_data_left_leg['true_position']['hip']
            json_data['true_position']['left_knee'] = json_data_left_leg['true_position']['knee']
            json_data['true_position']['left_foot'] = json_data_left_leg['true_position']['foot']
            json_data['true_position']['right_hip'] = json_data_right_leg['true_position']['hip']
            json_data['true_position']['right_knee'] = json_data_right_leg['true_position']['knee']
            json_data['true_position']['right_foot'] = json_data_right_leg['true_position']['foot']

        return json_data

    def set_from_json_data(self, json_data):
        # Set angle parameters from json data (not json file!)
        self.left_arm.set_from_json_data(json_data['joint_angle']['left_shoulder'], json_data['joint_angle']['left_elbow'])
        self.right_arm.set_from_json_data(json_data['joint_angle']['right_shoulder'], json_data['joint_angle']['right_elbow'])

        if self._lower_body_flag:
            self.left_leg.set_from_json_data(json_data['joint_angle']['left_hip'], json_data['joint_angle']['left_knee'])
            self.right_leg.set_from_json_data(json_data['joint_angle']['right_hip'], json_data['joint_angle']['right_knee'])

    def is_collision_body_parts(self):

        def collision_detection_generator():
            # left arm vs. cranicm
            yield self.left_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())

            # left arm vs. torso
            yield self.left_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius())

            # left arm vs. right upperarm
            yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_shoulder_point(), self.right_arm.calc_upperarm_lower_point(), self.right_arm.get_upperarm_radius())

            # left arm vs. right elbow
            yield self.left_arm.is_collision_to_sphere(self.right_arm.calc_elbow_point(), self.right_arm.get_elbow_radius())

            # left arm vs. right forearm
            yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_forearm_upper_point(), self.right_arm.calc_hand_point(), self.right_arm.get_forearm_radius())

            # right arm vs. cranicm
            yield self.right_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())

            # right arm vs. torso
            yield self.right_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius())

            if self._lower_body_flag:
                # left arm vs. left thigh
                yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius())

                # left arm vs. left knee
                yield self.left_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius())

                # left arm vs. left calf
                yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius())

                # left arm vs. right thigh
                yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius())

                # left arm vs. right knee
                yield self.left_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())

                # left arm vs. right calf
                yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())

                # right arm vs. left thigh
                yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius())

                # right arm vs. left thigh
                yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius())

                # right arm vs. left knee
                yield self.right_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius())

                # right arm vs. left calf
                yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius())

                # right arm vs. right thigh
                yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius())

                # right arm vs. right knee
                yield self.right_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())

                # right arm vs. right calf
                yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())

                # left leg vs. cranicm
                yield self.left_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())

                # left leg vs. right thigh
                yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius())

                # left leg vs. right knee
                yield self.left_leg.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())

                # left leg vs. right calf
                yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())

                # right leg vs. cranicm
                yield self.right_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())

                # torso vs. left knee
                yield self.torso.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius())

                # torso vs. left calf
                yield self.torso.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius())

                # torso vs. right knee
                yield self.torso.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())

                # torso vs. right calf
                yield self.torso.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())

        for detection in collision_detection_generator():
            if detection:
                return True

        return False
Beispiel #25
0
import sys
sys.path.append("..");
from arm import Arm;

if __name__=="__main__":
    arm = Arm();
    angles = {1:95, 2:65};
    arm.setAngle(angles);
    del arm

Beispiel #26
0
    def init_arms(self, cluster_positions, cluster_fitnesses):

        # Default matrix that translate and shrink search space to [0,1]^D
        matrices = [
            Matrix(positions, fitnesses, self.min_bounds,
                   self.max_bounds) for positions, fitnesses in zip(
                       cluster_positions, cluster_fitnesses)
        ]

        if self.plot > 0:
            draw_optimization(function_id - 1,
                              cluster_positions,
                              matrices,
                              fig_name='initial.png',
                              **self.fig_config)

        # Random sample n_samples to represent search space for non-overlapped optimization
        trans_samples = np.random.uniform(0,
                                          1,
                                          size=(self.n_samples,
                                                self.dimension))

        k = len(cluster_positions)
        for i in range(k):
            exclude = []
            for j in range(k):
                if i != j:
                    samples = matrices[j].inverse_transform(trans_samples)
                    exclude.extend(samples)

            exclude = np.array(exclude)

            if self.verbose: print('optimizing matrix for arm %d' % i)

            arm = Arm(self.obj,
                      self.n_points,
                      cluster_positions[i],
                      cluster_fitnesses[i],
                      algo_type=self.algo_type,
                      exclude=exclude,
                      min_bounds=self.min_bounds,
                      max_bounds=self.max_bounds)
            self.arms.append(arm)
            matrices[i] = self.arms[i].matrix

            if self.plot > 0:
                opt_points = []
                for j in range(k):
                    if i == j:
                        opt_points.append(cluster_positions[i])
                    else:
                        samples = matrices[j].inverse_transform(trans_samples)
                        opt_points.append(samples)
                draw_optimization(function_id - 1,
                                  opt_points,
                                  matrices,
                                  fig_name='initial_optimize_%d.png' % i,
                                  **self.fig_config)

        if self.plot > 0:
            opt_points = [arm.get_positions() for arm in self.arms]
            opt_matrices = [arm.matrix for arm in self.arms]
            draw_optimization(function_id - 1,
                              opt_points,
                              opt_matrices,
                              fig_name='initial_optimized.png',
                              **self.fig_config)
Beispiel #27
0
from arm import Arm
import rpyc
from rpyc.utils.server import ThreadedServer

ARM = Arm() # thread-safe instance

class ArmService(rpyc.Service):
    def _rpyc_getattr(self, name):
        # expose everything of the ARM instance
        return getattr(ARM, name)


def main():
    ARM.open()

    RPC_PORT = 18861
    t = ThreadedServer(ArmService, port=RPC_PORT)
    t.start()

if __name__ == '__main__':
    main()
Beispiel #28
0
def train_network_scenario1(prototypes1, prototypes2, origin, train_data='train_data.p', val_data='validation_data.p'):
    """
    Combines the networks for the arm and the eye. Arm is dominant over the eye, as in scenario 2, so the eye recieves its input from the arm and its target.
    Also plots a lot of information about loss and accuracy.
    Saves the weights of the network
    Input: prototypes for the arm, prototypes for the eye, point of origin of both models, trainingdata for the arm, validationdata for the eye
    Output: -
    """

    epochs = 150  # number of epochs

    print 'network1'
    network1, train_fn1, val_fn1 = create_network(prototypes1)
    print 'network2'
    network2, train_fn2, val_fn2 = create_network(prototypes2, n_inputs=4)

    print 'Networks done'
    eyes = Eyes(origin=origin, visualize=False)
    arm = Arm(origin=origin, visualize=False)

    print 'moare stuff'
    # Arrays for saving performance after each epoch
    means_arm = np.zeros(epochs)
    stds_arm = np.zeros(epochs)
    train_losses_arm = np.zeros(epochs)
    val_losses_arm = np.zeros(epochs)
    means_eye = np.zeros(epochs)
    stds_eye = np.zeros(epochs)
    train_losses_eye = np.zeros(epochs)
    val_losses_eye = np.zeros(epochs)
    dists_eye = np.zeros(epochs)
    dists_arm = np.zeros(epochs)

    print "Train network"

    for e in tqdm(range(epochs)):

        total_mean_arm = 0
        total_std_arm = 0
        total_mean_eye = 0
        total_std_eye = 0
        total_error_arm = 0
        total_error_eye = 0
        train_loss_arm = 0
        val_loss_arm = 0
        train_loss_eye = 0
        val_loss_eye = 0

        # training epoch
        i = 0
        for input_batch, output_batch in iterate_data(data_file=train_data):
            pred1, train_loss1 = train_fn1(input_batch, output_batch)
            arm_angles = np.array([arm.calculate_angles(x, y) for [x, y] in input_batch], dtype='float32')  # same targets as arm
            eye_positions = [calc_intersect(left, right) for [left, right] in pred1]  # get x,y from predicted eye angels
            arm_input = np.hstack((input_batch, eye_positions)).astype('float32')  # first the eye coordinates, take care when combining prototypes
            pred2, train_loss2 = train_fn2(arm_input, arm_angles)

            train_loss_arm += train_loss2
            train_loss_eye += train_loss1
            i += 1
        # Take average loss of this epoch
        train_loss_arm = train_loss_arm / i
        train_loss_eye = train_loss_eye / i

        n = 0
        # Validation Epoch
        for inp_val, out_val in iterate_data(data_file=val_data):
            predictions_eye, loss_eye = val_fn1(inp_val, out_val)
            dist_eye, mean_eye, std_eye = evaluate(predictions_eye, out_val)  # dist_arm is for debugging

            arm_angles = np.array([arm.calculate_angles(x, y) for [x, y] in inp_val], dtype='float32')
            eye_positions = [calc_intersect(left, right) for [left, right] in predictions_eye]

            arm_input = np.hstack((inp_val, eye_positions)).astype('float32')
            prediction_arm, loss_arm = val_fn2(arm_input, arm_angles)
            dist_arm, mean_arm, std_arm = evaluate(prediction_arm, inp_val)
            arm_positions = np.array([arm.move_arm(shoulder, shoulder) for [shoulder, elbow] in prediction_arm])

            arm_error_dist, mean_arm_error, std_arm_error = evaluate(arm_positions, inp_val)
            eye_error_dist, mean_eye_error, std_eye_error = evaluate(eye_positions, inp_val)

            total_error_arm += mean_arm_error
            total_error_eye += mean_eye_error

            n += 1
            total_mean_arm += mean_arm
            total_std_arm += std_arm
            total_mean_eye += mean_eye
            total_std_eye += std_eye
            val_loss_arm += loss_arm
            val_loss_eye += loss_eye


        # Save epoch data
        means_arm[e] = total_mean_arm / n
        stds_arm[e] = total_std_arm / n
        train_losses_arm[e] = train_loss_arm
        val_losses_arm[e] = val_loss_arm / n
        means_eye[e] = total_mean_eye / n
        stds_eye[e] = total_std_eye / n
        train_losses_eye[e] = train_loss_eye
        val_losses_eye[e] = val_loss_eye / n
        dists_eye[e] = total_error_eye / n
        dists_arm[e] = total_error_arm / n

    # Plots
    # Plot mean and std
    plt.figure()
    meanplot_arm, = plt.plot(means_arm, label='mean arm')
    stdplot_arm, = plt.plot(stds_arm, label='std arm')
    meanplot_eye, = plt.plot(means_eye, label='mean eye')
    stdplot_eye, = plt.plot(stds_eye, label='std eye')

    plt.legend(handles=[meanplot_arm, stdplot_arm, meanplot_eye, stdplot_eye])
    plt.savefig('../images/scenario1/accuracy_combined.png')
    plt.show()

    # Plot just the means
    plt.figure()
    meanplot_arm, = plt.plot(means_arm, label='mean arm')
    meanplot_eye, = plt.plot(means_eye, label='mean eye')
    plt.legend(handles=[meanplot_arm, meanplot_eye])
    plt.savefig('../images/scenario1/accuracy_combined_arm.png')

    # Ploot the train and validations losses
    plt.figure()
    trainplot_arm, = plt.plot(train_losses_arm, label='train loss arm')
    valplot_arm, = plt.plot(val_losses_arm, label='val loss arm')
    trainplot_eye, = plt.plot(train_losses_eye, label='train loss eye')
    valplot_eye, = plt.plot(val_losses_eye, label='val loss eye')

    plt.legend(handles=[trainplot_arm, valplot_arm, trainplot_eye, valplot_eye])
    plt.savefig('../images/scenario1/loss_combined.png')
    plt.show()

    # Plot distance errors
    plt.figure()
    distsplot_arm, = plt.plot(dists_arm, label='Distance Error arm')
    plt.legend(handles=[distsplot_arm])
    plt.savefig('../images/scenario1/distance_error_arm.png')
    plt.show()
    np.save('../images/scenario1/distance_arm', dists_arm)

    # Plot Distance error ot the eye
    plt.figure()
    distsplot_eye, = plt.plot(dists_eye, label='Distance Error eye')
    plt.legend(handles=[distsplot_eye])
    plt.savefig('../images/scenario1/distance_error_eye.png')
    plt.show()
    np.save('../images/scenario1/distance_eye', dists_eye)

    # Save the weights
    np.save('network_arm_s1', layers.get_all_param_values(network1))
    np.save('network_eye_s1', layers.get_all_param_values(network2))

    return  # network, predictions
Beispiel #29
0
        #eyes.attended_points = np.array()


if __name__ == '__main__':
    """
    Create arm model
    Create arm prototypes
    Create dataset
    Create eye model
    Create eye prototypes
    Combine prototypes (for the eye network)
    Train the network
    """

    origin = 0 #make sure the eye and the arm have the same origin!
    arm = Arm(visualize = False, origin = origin)
    print 'Create arm proto'
    proto_arm = arm.create_prototypes(shape=(10, 10))

    eyes = Eyes(visualize = False, origin = origin)
   # eyes.create_dataset(n_datapoints=100000, train_file='train_data_eyes.p', val_file='validation_data_eyes.p', test_file= 'test_data_eyes.p')
    print 'create eye proto'
    proto_eye = eyes.create_prototypes(shape = (10, 10))
    print 'combine prototypes'
    proto_arm = combine_prototypes(proto_arm, proto_eye)
    print 'To the network!'
    train_network_scenario1(proto_eye, proto_arm, origin = origin,  train_data = 'train_data_eyes.p', val_data = 'validation_data_eyes.p' )

    """
    origin = 0
    arm = Arm(visualize= False, origin = origin)
class DiffDriveManipulatorRobot(object):
    """
    DiffDrive main robot class. Launching a few threads to handle mechanics
    and odometry.
    """

    __slots__ = [
        '_urgentStop',
        'timeStep',
        'logger',
        '_current_V',
        'baseMovementLock',
        'armMovementLock',
        'powerSupply',
        'eventLoop',
        'eventLoopExecutors',
        'eventLoopArmExecutors',
        'eventLoopMainExecutors',
        'eventLoopBaseExecutors',
        'sensors',
        'gyroSensorConfiguration',
        'robotConfiguration',
        'arm',
        'base',
        'mqtt_topics',
        'mqtt_connect_mid',
        'threadOdometry',
        'eventLoopThread',
        'threadMQTT',
        'mqttMoveQueue',
        'mqttClient',
        'exceptionQueue',
        'asyncVoltageCheckLoopTask',
        'asyncOdometryReportingTask',
        'M0e',
        'Toe_grip',
        'Tbc',
        'Tcb',
        'asyncImplMoveLoopTask'
    ]

    def __init__(self,
                 robot_config_file,
                 initRobotConfig,
                 timeStep,
                 odometryTimer=0.1,
                 defaultLogging=30):
        """
        Creates the main bot class.
        """
        if not os.path.exists(robot_config_file):
            raise ValueError(f'Cannot find configuration file '
                             f'"{robot_config_file}"')

        with open(robot_config_file, 'r') as f:
            self.robotConfiguration = json.load(f)

        # Adjust JSON just read
        self.__adjustConfigDict(self.robotConfiguration)

        # Exception Queue for inter threads exception communications
        self.exceptionQueue = queue.Queue()
        self.mqttMoveQueue = queue.Queue()
        self.baseMovementLock = asyncio.locks.Lock()
        self.timeStep = timeStep
        self.powerSupply = PowerSupply()

        # region sensor Init
        self.sensors = {}
        for sensor in self.robotConfiguration["sensors"]:
            sensorDict = self.robotConfiguration["sensors"][sensor]
            legoPort = LegoPort(sensorDict["input"])
            # Applies to both I2C and SDK controled sensors
            if legoPort.mode != sensorDict["mode"]:
                legoPort.mode = sensorDict["mode"]
            # Only applies to sensors controled by the python SDK
            if "set_device" in sensorDict:
                legoPort.set_device = sensorDict["set_device"]
                time.sleep(1)
            address = sensorDict["input"]
            self.sensors[sensor] = sensorDict["sensorType"](address=address)
        # endregion

        # base Init
        self.base = DiffDriveBase(self.robotConfiguration["base"],
                                  initRobotConfig, odometryTimer)

        # arm/endeffector Init
        self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"])

        # Other matrix init
        self.M0e = mr.RpToTrans(
            R=np.array(
                self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T,   # noqa F501
            p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"])   # noqa F501
        self.Toe_grip = mr.RpToTrans(
            R=np.array(
                self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T,   # noqa F501
            p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"])    # noqa F501
        self.Tbc = mr.RpToTrans(
            R=np.array(
                self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T,   # noqa F501
            p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"])
        self.Tcb = mr.TransInv(self.Tbc)

        # Reset all motors at init
        self.urgentStop = False
        log.info(LOGGER_DDR_MAIN, 'Done main robot init.')
        # log.setLevel(LOGGER_DDR_MAIN)

    def __del__(self):
        """ Destructor """
        loggerName = 'ddrmain'
        log.info(loggerName,
                 msg=f'Deleting robot ; opening gripper '
                     f'and shutting down motors.')
        # Open the gripper
        self.arm.openEndEffector()
        # Shut down the motors
        self.urgentStop = True
        # Clear the event loop and shut it down
        self.asyncVoltageCheckLoopTask.cancel()
        self.asyncImplMoveLoopTask.cancel()
        # Cancel running tasks in the event loop
        if self.eventLoop.is_running():
            self.eventLoop.stop()
        # Stop threads
        self.base.stopThreadOdometry()
        # Will release motors that are on hold before shutting down
        self.base.resetBodyConfiguration()
        # Stop the mqtt client loop
        self.mqttClient.loop_stop()
        self.arm.killThread = True
        self.arm.openEndEffector()
        # Unload the drivers for sensors
        for sensor in self.robotConfiguration["sensors"]:
            LegoPort(
                self.robotConfiguration["sensors"][sensor]['input']).mode = \
                'none'
        log.debug(loggerName, 'Done the __del__ function')
        # Unload the sensor drivers

    # region properties
    @property
    def currentVoltage(self):
        return self.powerSupply.measured_volts

    @property
    def urgentStop(self):
        return self._urgentStop

    @urgentStop.setter
    def urgentStop(self, value):
        if value is True:
            log.info(LOGGER_DDR_MAIN, "Need to immediately stop the bot.")
            self.mqttMoveQueue = queue.Queue()
        self._urgentStop = value
        if self.base:
            self.base.urgentStop = value
        if self.arm:
            self.arm.urgentStop = value

    @property           # should stay on main robot
    def currentConfigVector(self):
        return np.r_[self.base.currentConfigVector,
                     self.arm.currentConfigVector,
                     self.arm.endEffectorStatus]
    # endregion

    def run(self):
        """
        run the main event loop for the robot and the required threads
        """
        try:
            # region Start Base Odometry Thread Init
            self.base.run()
            # endregion

            # region MQTTConnect Thread Init
            self.threadMQTT = threading.Thread(
                target=self.threadImplMQTT,
                name="threadImplMQTT")
            self.threadMQTT.start()
            # endregion

            # Launch main event loop in a separate thread
            self.eventLoopThread = threading.Thread(
                target=self.threadImplEventLoop,
                name="threadImplEventLoop")
            self.eventLoopThread.start()
            # Error handling for threads
            while True:
                try:
                    exc = self.exceptionQueue.get(block=True)
                except queue.Empty:
                    pass
                else:
                    log.error(LOGGER_DDR_RUN,
                              msg=f'Exception handled in one of the '
                                  f'spawned process : {exc}')
                    raise exc
                self.eventLoopThread.join(0.2)
                if self.eventLoopThread.isAlive():
                    continue
                else:
                    break
        except SystemExit:
            # raise the exception up the stack
            raise
        except:
            log.error(LOGGER_DDR_RUN, f'Error : {traceback.print_exc()}')

    def threadImplEventLoop(self):
        """
        Main event asyncio eventloop launched in a thread
        """
        try:
            log.warning(LOGGER_DDR_THREADIMPLEVENTLOOP,
                        msg=f'Launching main event loop.')
            self.eventLoop = asyncio.new_event_loop()
            self.eventLoopArmExecutors = \
                concurrent.futures.ThreadPoolExecutor(
                    max_workers=MAX_WORKER_THREADS,
                    thread_name_prefix='ArmThreadPool')
            self.eventLoopMainExecutors = \
                concurrent.futures.ThreadPoolExecutor(
                    max_workers=MAX_WORKER_THREADS,
                    thread_name_prefix='MainThreadPool')
            self.eventLoopBaseExecutors = \
                concurrent.futures.ThreadPoolExecutor(
                    max_workers=MAX_WORKER_THREADS,
                    thread_name_prefix='BaseThreadPool')
            self.asyncImplMoveLoopTask = \
                self.eventLoop.create_task(
                    self.asyncProcessMQTTMessages(0.25))
            self.asyncVoltageCheckLoopTask = \
                self.eventLoop.create_task(self.asyncVoltageCheckLoop(10))
            self.asyncOdometryReportingTask = \
                self.eventLoop.create_task(self.asyncOdometryReporting(1))
            self.eventLoop.run_in_executor(
                self.eventLoopMainExecutors,
                self.asyncExecutorImplKillSwitch)
            self.eventLoop.run_forever()
        except:
            log.error(LOGGER_DDR_THREADIMPLEVENTLOOP,
                      msg=f'Error : {traceback.print_exc()}')
        finally:
            self.eventLoop.stop()
            self.eventLoop.close()

    async def asyncProcessMQTTMessages(self, loopDelay):
        """
        This function receives the messages from MQTT to move the robot.
        It is implemented in another thread so that the killswitch can kill
        the thread without knowing which specific
        """
        while True:
            try:
                await asyncio.sleep(loopDelay)
                if self.mqttMoveQueue.empty() is False:
                    try:
                        if self.currentVoltage < LOW_VOLTAGE_THRESHOLD:
                            raise LowVoltageException
                    except LowVoltageException:
                        log.warning(LOGGER_DDR_THREADIMPLMQTT,
                                    msg=f'Low voltage threshold '
                                        f'{float(LOW_VOLTAGE_THRESHOLD):.2} '
                                        f'reached. Current voltage = '
                                        f'{self.currentVoltage:.2f} .'
                                        f'Robot will not move, but MQTT '
                                        f'message will attempt to process. '
                                        f'Consider charging the battery, '
                                        f'or find some kind of power supply')
                    # Remove the first in the list, will pause until there
                    # is something
                    currentMQTTMoveMessage = self.mqttMoveQueue.get()
                    # Decode message received
                    msgdict = json.loads(
                        currentMQTTMoveMessage.payload.decode('utf-8'))
                    # Verify if need to urgently overried current movement
                    # and queue
                    if 'override' in msgdict:
                        if msgdict['override']:
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Overriding all motion - calling '
                                          f'killswitch to empty queue')
                            # Urgently stop the current movement (and empty
                            # movement queue - done in killSwitch)
                            self.killSwitch()

                    # Default motion configuration
                    pid = False if 'pid' not in msgdict else msgdict["pid"]
                    velocity_factor = 0.1 if 'velocity_factor' not \
                        in msgdict else msgdict["velocity_factor"]
                    dryrun = False if 'dryrun' not in msgdict else \
                        msgdict["dryrun"]
                    inter_wheel_distance = None if 'inter_wheel_distance' not\
                        in msgdict else msgdict["inter_wheel_distance"]
                    wheel_radius = None if 'wheel_radius' not in msgdict \
                        else msgdict["wheel_radius"]
                    time_scaling_method = MOVEMENT_TIME_SCALING_METHOD_DEFAULT

                    if currentMQTTMoveMessage.topic == 'bot/killSwitch':
                        self.killSwitch()

                    elif currentMQTTMoveMessage.topic == \
                            'bot/base/reset/position':
                        log.warning(
                            LOGGER_DDR_IMPLMOVELOOP,
                            msg=f'Invoking reset body configuration')
                        self.base.resetBodyConfiguration()

                    elif currentMQTTMoveMessage.topic == \
                            'bot/base/move/xyphi':
                        self.eventLoop.run_in_executor(
                            self.eventLoopBaseExecutors,
                            functools.partial(self.base.moveBaseXYPhi,
                                              endBaseConfig=(msgdict["x"],
                                                             msgdict["y"],
                                                             msgdict["phi"]),
                                              pid=pid,
                                              velocity_factor=velocity_factor,
                                              dryrun=dryrun))

                    elif currentMQTTMoveMessage.topic == \
                            'bot/base/move/x':
                        self.eventLoop.run_in_executor(
                            self.eventLoopBaseExecutors,
                            functools.partial(self.base.moveBaseX,
                                              distance=msgdict["distance"],
                                              pid=pid,
                                              velocity_factor=velocity_factor,
                                              wheel_radius=wheel_radius,
                                              dryrun=dryrun))

                    elif currentMQTTMoveMessage.topic == \
                            'bot/base/turn/phi':
                        self.eventLoop.run_in_executor(
                            self.eventLoopBaseExecutors,
                            functools.partial(
                                self.base.rotateBasePhi,
                                phi=msgdict["phi"],
                                pid=pid,
                                velocity_factor=velocity_factor,
                                inter_wheel_distance=inter_wheel_distance,
                                dryrun=dryrun))

                    elif currentMQTTMoveMessage.topic == \
                            'bot/base/move/home':
                        # Calculate where to go from there (not avoiding
                        # obstacles...)
                        _, (x, y, _) = mr.TransToRp(
                            mr.TransInv(self.base.Tsb))
                        # pi - self.base.currentPhi
                        phi = - 1 * self.base.currentPhi
                        self.base.moveBaseXYPhi(
                            endBaseConfig=(x, y, phi),
                            pid=pid,
                            velocity_factor=velocity_factor,
                            time_scaling_method=time_scaling_method,
                            dryrun=dryrun)

                    elif currentMQTTMoveMessage.topic == \
                            'bot/arm/move/rest':
                        self.eventLoop.run_in_executor(
                            self.eventLoopArmExecutors,
                            functools.partial(
                                self.arm.moveToAngle,
                                armConfigVector=self.arm.armRestPosition,
                                dryrun=dryrun))

                    elif currentMQTTMoveMessage.topic == \
                            'bot/arm/move/zero':
                        self.eventLoop.run_in_executor(
                            self.eventLoopArmExecutors,
                            functools.partial(
                                self.arm.moveToAngle,
                                armConfigVector=self.arm.armZeroPosition,
                                dryrun=dryrun))

                    elif currentMQTTMoveMessage.topic == \
                            'bot/arm/move/theta':
                        self.eventLoop.run_in_executor(
                            self.eventLoopArmExecutors,
                            functools.partial(
                                self.arm.moveToAngle,
                                armConfigVector=[msgdict["theta1"],
                                                 msgdict["theta2"]],
                                dryrun=dryrun))

                    elif currentMQTTMoveMessage.topic == \
                            'bot/gripper/open':
                        # Already an async call (non blocking)
                        self.arm.openEndEffector()

                    elif currentMQTTMoveMessage.topic == \
                            'bot/gripper/close':
                        # Already an async call (non blocking)
                        self.arm.closeEndEffector()

                    elif currentMQTTMoveMessage.topic == \
                            'bot/gripper/position':
                        # Call function to move this thing
                        Pco = np.array(msgdict["P"])
                        angle = msgdict["grip_angle"]
                        Toe_grip = mr.RpToTrans(
                            R=np.array([[cos(angle), 0, sin(angle)],
                                        [0, 1, 0],
                                        [-sin(angle), 0, cos(angle)]]),
                            p=[0, 0, 0])
                        Rco, _ = mr.TransToRp(Toe_grip)  # Rco=camera-object
                        # Rco, _ = mr.TransToRp(self.Toe_grip)
                        Tco_desired = mr.RpToTrans(R=Rco, p=Pco)
                        Tbo_desired = self.Tbc @ Tco_desired
                        # Initial guess - based on the angle received
                        thetalist0 = (0, 0, pi / 8, pi / 4)   # pi / 4, pi / 2)

                        log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                  msg=f'Running inverse kinematics to get '
                                      f'required joint angles.')
                        thetalist, success = mr.IKinBody(
                            Blist=self.arm.bList,
                            M=self.M0e,
                            T=Tbo_desired,
                            thetalist0=thetalist0,
                            eomg=0.02,   # 0.08 rad = 5 deg uncerainty
                            ev=0.025)     # 4cm error?
                        if success:
                            thetalist = thetalist.round(6)
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'IK Solved with joint thetas '
                                          f'found {thetalist}')
                            # 0. Open gripper
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Opening or ensuring end effector '
                                          f'is opened.')
                            self.arm.openEndEffector()
                            # 1. Move arm to 0 position - launch async.
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Moving arm to driving'
                                          f'position')
                            self.arm.moveToAngle(
                                armConfigVector=self.arm.armDrivePosition,
                                dryrun=dryrun)
                            # 2. Move base by X first
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Moving body by X '
                                          f'{thetalist[0]:.2f}')
                            self.base.moveBaseX(
                                distance=thetalist[0],
                                pid=pid,
                                velocity_factor=velocity_factor,
                                dryrun=dryrun)
                            # 3. Rotate base by Phi
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Rotating base by phi '
                                          f'{thetalist[1]:.2f}')
                            self.base.rotateBasePhi(
                                phi=thetalist[1],
                                pid=pid,
                                velocity_factor=velocity_factor,
                                dryrun=dryrun)
                            # 4a. Move to standoff position
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Moving arm joints to standoff '
                                          f'position')
                            # 4. Move other joint in position - need to block
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Moving arm joints into position '
                                          f'for picking up object j3 = '
                                          f'{degrees(thetalist[2]):.2f}, '
                                          f'j4 = {degrees(thetalist[3]):.2f}')
                            self.arm.moveToAngle(armConfigVector=thetalist[2:],
                                                 dryrun=dryrun)
                            # 5. Grab object
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Closing end effector')
                            self.arm.closeEndEffector()
                            # Rest b/c the previous call is non blocking,
                            # and blocking causes issues on the wait
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Async Sleep for 1 second...')
                            await asyncio.sleep(1)
                            # 6. Set to driving position
                            log.debug(LOGGER_DDR_IMPLMOVELOOP,
                                      msg=f'Moving arm to driving position.')
                            self.arm.moveToAngle(
                                armConfigVector=self.arm.armDrivePosition,
                                dryrun=dryrun)
                            log.debug(LOGGER_DDR_IMPLMOVELOOP, f'Done!')
                        else:
                            log.warning(LOGGER_DDR_IMPLMOVELOOP,
                                        msg=f'Unable to calculate joint and '
                                            f'wheel angles to achieve the '
                                            f'required position.')

                    elif currentMQTTMoveMessage.topic == \
                            'bot/logger':
                        # Changing the logging level on the fly...
                        log.setLevel(msgdict['logger'], lvl=msgdict['level'])
                    elif currentMQTTMoveMessage.topic == \
                            'bot/logger/multiple':
                        # Changing the logging level on the fly for multiple loggers at a time
                        for logger, level in msgdict.items():
                            log.setLevel(logger, level)
                    else:
                        raise NotImplementedError
            except NotImplementedError:
                log.warning(LOGGER_DDR_IMPLMOVELOOP,
                            msg=f'MQTT message not implemented.')
            except asyncio.futures.CancelledError:
                log.warning(LOGGER_DDR_IMPLMOVELOOP,
                            msg=f'Cancelled the MQTT dequeing task.')
            except:
                log.error(LOGGER_DDR_IMPLMOVELOOP,
                          msg=f'Error : {traceback.print_exc()}')

    async def asyncVoltageCheckLoop(self, loopDelay):
        """
        run the voltage loop check
        """
        while True:
            try:
                await asyncio.sleep(loopDelay)
                if self.currentVoltage < LOW_VOLTAGE_THRESHOLD:
                    raise LowVoltageException
            except asyncio.futures.CancelledError:
                log.warning(LOGGER_DDR_VOLTAGECHECK,
                            msg=f'Cancelled the Voltage Check task.')
            except LowVoltageException:
                log.critical(LOGGER_DDR_VOLTAGECHECK,
                             msg=f'Voltage low on device '
                                 f'({self.currentVoltage:.2f}V - '
                                 f'threshold  = {LOW_VOLTAGE_THRESHOLD:.2f}V)'
                                 f' - consider charging battery or shutting '
                                 f'down.')

    async def asyncOdometryReporting(self, loopDelay):
        """
        run the odometry loop and printout
        """
        try:
            while True:
                await asyncio.sleep(loopDelay)
                log.info(LOGGER_DDR_ODOMETRY,
                         msg=f'Joint theta (degrees) = {self.arm}')
                log.info(LOGGER_DDR_ODOMETRY,
                         msg=f'Latest Vb6 = {self.base.Vb6} ; '
                             f'Phi(degrees) = '
                             f'{degrees(self.base.currentPhi):.2f}')
                log.info(LOGGER_DDR_ODOMETRY,
                         msg=f'Latest Base SE3 (self.base.Tsb) = \n'
                             f'{self.base.Tsb}')
                # Don't read actual position, there's a race condition
                left = self.base.oldLeftPhi
                right = self.base.oldRightPhi
                log.debug(LOGGER_DDR_ODOMETRY,
                          msg=f'Left/Right Position:{left:.2f}/{right:.2f}')
                theoretical_dist = radians((left + right) / 2) * \
                    self.base.wheelRadius
                log.debug(LOGGER_DDR_ODOMETRY,
                          msg=f'Theoretical distance:{theoretical_dist:.4f}')

        except asyncio.futures.CancelledError:
            log.warning(LOGGER_DDR_ODOMETRY,
                        msg=f'Cancelled the Voltage Check task.')

    def asyncExecutorImplKillSwitch(self):
        """ Detects click and double clicks"""
        try:
            button = self.sensors["killSwitch"]
            log.info(LOGGER_DDR_EXECUTORKILLSWITCH,
                     msg=f'Initialized async process for kill switch.')
            while True:
                firstClick = time.time()
                button.wait_for_bump()
                secondClick = time.time()
                if secondClick - firstClick < DOUBLECLICK:
                    log.warning(LOGGER_DDR_EXECUTORKILLSWITCH,
                                msg=f'DoubleClick caught.')
                    self.exceptionQueue.put(SystemExit)
                    # Return is there to end this thread and return it
                    # to the pool (run_in_executor).
                    return
                else:
                    self.killSwitch()
                log.warning(LOGGER_DDR_EXECUTORKILLSWITCH,
                            msg=f'Button pressed. Urgent Stop switch '
                                f'flipped. Threads using motors should '
                                f'stop.')
        except:
            log.warning(LOGGER_DDR_EXECUTORKILLSWITCH,
                        msg=f'Error in the kill switch routing : '
                            f'{traceback.print_exc()}')

    def __adjustConfigDict(self, confDict):
        '''
        adjustConfigDict :
        param:confDict:parameters read in the config file for the robot
        param:confDict:type:parameter dictionary
        returns : modified confDict
        '''
        for key, value in confDict.items():
            if not isinstance(value, dict):
                if not isinstance(value, list):
                    if value in LEGO_ROBOT_DEFINITIONS:
                        confDict[key] = LEGO_ROBOT_DEFINITIONS[value]
            else:
                confDict[key] = self.__adjustConfigDict(confDict[key])
        return confDict

    def killSwitch(self):
        # Empty movement queue
        try:
            log.warning(LOGGER_DDR_KILLSWITCH, 'Killing all motor motion.')
            self.urgentStop = True
            time.sleep(1)
            self.urgentStop = False
        except:
            pass

    def threadImplMQTT(self):
        """
        MQTT Thread launching the loop and subscripbing to the right topics
        """
        mqtt_default_qos = 2
        self.mqtt_topics = [
            (topic, mqtt_default_qos) for topic in
            self.robotConfiguration["mqtt"]["subscribedTopics"]]

        def on_connect(client, userdata, flags, rc):
            log.warning(LOGGER_DDR_THREADIMPLMQTT,
                        msg=f'Connected to MQTT broker. '
                            f'Result code {rc}')
            mqtt_connect_result, self.mqtt_connect_mid = \
                client.subscribe(self.mqtt_topics)
            if mqtt_connect_result == mqtt.MQTT_ERR_SUCCESS:
                log.warning(LOGGER_DDR_THREADIMPLMQTT,
                            msg=f'Successfully subscribed '
                                f'to {self.mqtt_topics}')
            else:
                log.error(LOGGER_DDR_THREADIMPLMQTT,
                          msg=f'MQTT Broker subscription problem.')

        def on_message(client, userdata, message):
            """ callback function used for the mqtt client (called when
            a new message is publisehd to one of the queues we subscribe to)
            """
            log.info(LOGGER_DDR_THREADIMPLMQTT,
                     msg=f'Received MID {message.mid} : '
                         f'{str(message.payload)} on topic '
                         f'{message.topic} with QoS {message.qos}')
            self.mqttMoveQueue.put_nowait(message)

        def on_disconnect(client, userdata, rc=0):
            """callback for handling disconnects
            """
            log.info(LOGGER_DDR_THREADIMPLMQTT,
                     msg=f'Disconnected MQTT result code = {rc}.'
                         f'Should automatically re-connect to broker')

        def on_subscribe(client, userdata, mid, granted_qos):
            if mid == self.mqtt_connect_mid:
                log.warning(LOGGER_DDR_THREADIMPLMQTT,
                            msg=f'Subscribed to topics. Granted QOS '
                                f'= {granted_qos}')
            else:
                log.error(LOGGER_DDR_THREADIMPLMQTT,
                          msg=f'Strange... MID doesn\'t match '
                              f'self.mqtt_connect_mid')

        self.mqttClient = mqtt.Client(
            client_id="mybot",
            clean_session=True,
            transport=self.robotConfiguration["mqtt"]["brokerProto"])
        self.mqttClient.enable_logger(
            logger=RoboLogger.getSpecificLogger(LOGGER_DDR_THREADIMPLMQTT))
        self.mqttClient.on_subscribe = on_subscribe
        self.mqttClient.on_connect = on_connect
        self.mqttClient.on_disconnect = on_disconnect
        self.mqttClient.on_message = on_message
        self.mqttClient.connect(
            host=self.robotConfiguration["mqtt"]["brokerIP"],
            port=self.robotConfiguration["mqtt"]["brokerPort"])
        self.mqttClient.loop_start()
Beispiel #31
0
def train_network(prototypes, train_data = 'train_data.p', val_data = 'validation_data.p'):
    """
    Legacy Code, doesnt work anymore
    Trains a single network (eithers arm of eye model)
    Also plots some of information about loss and accuracy.
    Input: prototypes of the model, train data, validation data
    Output: -
    """
    
    network, train_fn, val_fn = create_network(prototypes)

    epochs = 150
    means = np.zeros(epochs)
    stds = np.zeros(epochs)
    train_losses = np.zeros(epochs)
    val_losses = np.zeros(epochs)
    dists = np.zeros(epochs)

    arm = Arm(origin=0, visualize=False)

    print "Train network"

    for e in tqdm(range(epochs)):
        #Train epoch

        for input_batch, output_batch in iterate_data(data_file = train_data):
            pred, train_loss = train_fn(input_batch, output_batch)


        total_mean = 0
        total_std = 0
        total_dist = 0
        n = 0
        for inp_val, out_val in iterate_data(data_file = val_data):

            #validation epoch
            predictions, loss = val_fn(inp_val, out_val)
            dist, mean, std = evaluate(predictions, out_val)
            arm_positions = np.array([arm.move_arm(shoudler, elbow) for [shoudler, elbow] in predictions])
            eye_error_dist, mean_eye_error, std_eye_error = evaluate(arm_positions, inp_val)

            n += 1
            total_mean += mean
            total_std += std
            total_dist += mean_eye_error


        means[e] = total_mean/n
        stds[e] = total_std/n
        train_losses[e] = train_loss
        val_losses[e] = loss
        dists[e] = total_dist/n
        np.save('network_epoch' + str(e), layers.get_all_param_values(network))   

    #Plots
    plt.figure()
    distplot, = plt.plot(dists, label = 'arm distance error')
    plt.legend(handles = [distplot])
    plt.savefig('../images/arm_error.png')
    plt.show()

    plt.figure()
    meanplot, = plt.plot(means, label = 'mean')
    stdplot, = plt.plot(stds, label = 'std')
    plt.legend(handles = [meanplot, stdplot])
    plt.savefig('../images/arm_angles.png')
    plt.show()

    plt.figure()
    trainplot, = plt.plot(train_losses, label = 'train loss')
    valplot, = plt.plot(val_losses, label = 'val loss')
    plt.legend(handles = [trainplot, valplot])
    plt.savefig('../images/arm_losses.png')
    plt.show()
    
    print "saving network"
    np.save('network_arm', layers.get_all_param_values(network))
    print "done saving"


    
    return network, predictions
Beispiel #32
0
def main(argv):
    rclpy.init(args=argv)
    arm = Arm()

    arm.move_to(+0.2000, +0.0000, +0.2400, accelerate=2.0)
    arm.move_joints(joint4=+0.7000, path_time=1.0)
    time.sleep(2.0)
    arm.move_to(+0.2800, +0.0000, +0.2400, accelerate=0.3)
    time.sleep(2.0)
    arm.open()
    arm.close()
    arm.open()
    arm.close()

    time.sleep(3.0)
    arm.move_to(+0.1796, +0.0000, +0.1500, accelerate=0.3)
    time.sleep(3.0)
    arm.open()
    arm.close()
    time.sleep(3.0)
    # return to neutral
    arm.move_to(+0.1360, +0.0000, +0.2324, accelerate=0.5)