# arm_theater.py # # Sample Webots controller file for driving a two-link arm with two driven # joints. This example provides inverse kinematics for performing # position-controlled trajectories. # No copyright, 2020-2024, Garth Zeglin. This file is # explicitly placed in the public domain. print("arm_theater.py waking up.") # Import the Webots simulator API. from controller import Robot # Import the standard Python math library. import math, random, time # Define the time step in milliseconds between controller updates. EVENT_LOOP_DT = 100 ################################################################ class TwoLink(Robot): def __init__(self): super(TwoLink, self).__init__() self.robot_name = self.getName() print("%s: controller connected." % (self.robot_name)) # Attempt to randomize the random library sequence. random.seed(time.time()) # Initialize geometric constants. These should match # the current geometry of the robot. self.link1_length = 0.5 self.link2_length = 0.5 # Fetch handle for the 'base' and 'elbow' joint motors. self.motor1 = self.getDevice('motor1') self.motor2 = self.getDevice('motor2') # Adjust the motor controller properties. self.motor1.setAvailableTorque(15.0) self.motor2.setAvailableTorque(10.0) # Adjust the low-level controller gains. print("%s: setting PID gains." % (self.robot_name)) self.motor1.setControlPID(50.0, 0.0, 15.0) self.motor2.setControlPID(50.0, 0.0, 15.0) # Fetch handles for the joint sensors. self.joint1 = self.getDevice('joint1') self.joint2 = self.getDevice('joint2') # Specify the sampling rate for the joint sensors. self.joint1.enable(EVENT_LOOP_DT) self.joint2.enable(EVENT_LOOP_DT) # Connect to the end sensor. self.end_sensor = self.getDevice("endRangeSensor") self.end_sensor.enable(EVENT_LOOP_DT) # set sampling period in milliseconds self.end_sensor_interval = 1000 self.end_sensor_timer = 1000 # Initialize behavior state machines. self.state_timer = 2*EVENT_LOOP_DT self.state_index = 0 return #================================================================ def forward_kinematics(self, q): """Compute the forward kinematics. Returns the body-coordinate XY Cartesian position of the elbow and endpoint for a given joint angle vector. Note that in the arm theater model the body X coordinate is straight up, body Y is to the left. :param q: two-element list with [q1, q2] joint angles :return: tuple (elbow, end) of two-element lists with [x,y] locations """ elbow = [self.link1_length * math.cos(q[0]), self.link1_length * math.sin(q[0])] end = [elbow[0] + self.link2_length * math.cos(q[0]+q[1]), elbow[1] + self.link2_length * math.sin(q[0]+q[1])] return elbow, end #================================================================ def endpoint_inverse_kinematics(self, target): """Compute two inverse kinematics solutions for a target end position. The target is a XY Cartesian position vector in body coordinates, and the result vectors are joint angles as lists [q0, q1]. If the target is out of reach, returns the closest pose. """ # find the position of the point in polar coordinates radiussq = target[0]**2 + target[1]**2 radius = math.sqrt(radiussq) # theta is the angle of target point w.r.t. X axis, same origin as arm theta = math.atan2(target[1], target[0]) # use the law of cosines to compute the elbow angle # R**2 = l1**2 + l2**2 - 2*l1*l2*cos(pi - elbow) # both elbow and -elbow are valid solutions acosarg = (radiussq - self.link1_length**2 - self.link2_length**2) / (-2 * self.link1_length * self.link2_length) if acosarg < -1.0: elbow_supplement = math.pi elif acosarg > 1.0: elbow_supplement = 0.0 else: elbow_supplement = math.acos(acosarg) # use the law of sines to find the angle at the bottom vertex of the triangle defined by the links # radius / sin(elbow_supplement) = l2 / sin(alpha) if radius > 0.0: alpha = math.asin(self.link2_length * math.sin(elbow_supplement) / radius) else: alpha = 0.0 # compute the two solutions with opposite elbow sign soln1 = [theta - alpha, math.pi - elbow_supplement] soln2 = [theta + alpha, elbow_supplement - math.pi] return soln1, soln2 #================================================================ # motion primitives def go_joints(self, target): """Issue a position command to move to the given endpoint position expressed in joint angles.""" # arbitrarily pick the first solution self.motor1.setPosition(target[0]) self.motor2.setPosition(target[1]) print("%s: moving to (%f, %f)" % (self.robot_name, target[0], target[1])); def go_target(self, target): """Issue a position command to move to the given endpoint position expressed in Cartesian coordinates.""" p1, p2 = self.endpoint_inverse_kinematics(target) # arbitrarily pick the first solution self.motor1.setPosition(p1[0]) self.motor2.setPosition(p1[1]) print("%s: target (%f, %f), moving to (%f, %f)" % (self.robot_name, target[0], target[1], p1[0], p1[1])) #================================================================ # Polling function to process sensor input at different timescales. def poll_sensors(self): self.end_sensor_timer -= EVENT_LOOP_DT if self.end_sensor_timer < 0: self.end_sensor_timer += self.end_sensor_interval # read the distance sensor distance = self.end_sensor.getValue() if distance < 0.9: print("%s: range sensor detected obstacle at %f." % (self.robot_name, distance)) #================================================================ # Define joint-space movement sequences. For convenience the joint angles # are specified in degrees, then converted to radians for the controllers. _right_poses = [[0, 0], [0, 60], [60, 60], [60, 0], ] _left_poses = [[0, 0], [0, -60], [-60, -60], [-60, 0], ] #================================================================ def poll_sequence_activity(self): """State machine update function to walk through a series of poses at regular intervals.""" # Update the state timer self.state_timer -= EVENT_LOOP_DT # If the timer has elapsed, reset the timer and update the outputs. if self.state_timer < 0: self.state_timer += 2000 # Look up the next pose. if 'left' in self.robot_name: next_pose = self._left_poses[self.state_index] self.state_index = (self.state_index + 1) % len(self._left_poses) else: next_pose = self._right_poses[self.state_index] self.state_index = (self.state_index + 1) % len(self._right_poses) # Convert the pose to radians and issue to the motor controllers. angles = [math.radians(next_pose[0]), math.radians(next_pose[1])] self.go_joints(angles) #================================================================ def run(self): # Run loop to execute a periodic script until the simulation quits. # If the controller returns -1, the simulator is quitting. while self.step(EVENT_LOOP_DT) != -1: # Read simulator clock time. self.sim_time = self.getTime() # Read sensor values. self.poll_sensors() # Update the activity state machine. self.poll_sequence_activity() ################################################################ # Start the script. robot = TwoLink() robot.run()