Commits

German Larrain committed 6f6d14d

VehicleWithArm and RSS1 demos:
-the robotic arm has 2 links & joints intead of 3
-get_kinematic_data now returns the information of both joints

Comments (0)

Files changed (2)

 
 	VEHICLE_OFFSET = (-4,0.5,2)
 	CAMERA_POSITION = (0,8,15)
-	
+
 	LINK1_PARAMS = (1,0.8, 0.1,(0,0,0)) # (density, length, radius, center)
-	LINK2_PARAMS = (1,0.2,0.1,(0,0.5,0.2))
-	LINK3_PARAMS = (1,0.6,0.1,(0,0.9,0.4))
+	LINK2_PARAMS = (1,0.6,0.1,(0,0.7,0.2))
 	
 	kp = 10
 	speeds = ((0,0), (1,0), (5,1), (9,1), (13,0), (14,0)) # (time,speed)
 		r1_angle = self.sim.get_joint('r1').get_joint().get_angle()
 		r1_angle_rate = self.sim.get_joint('r1').get_joint().get_angle_rate()
 		r1_angle_accel = (self.r1_angle_rate_prev - r1_angle_rate) / self.sim.get_time_step()
+		
+		r2_angle = self.sim.get_joint('r2').get_joint().get_angle()
+		r2_angle_rate = self.sim.get_joint('r2').get_joint().get_angle_rate()
+		r2_angle_accel = (self.r2_angle_rate_prev - r2_angle_rate) / self.sim.get_time_step()
+		
 		end_effector_pos = self.sim.get_object(self.link2).get_position()
 		
-		return (r1_angle, r1_angle_rate, r1_angle_accel, end_effector_pos)
+		return (r1_angle, r1_angle_rate, r1_angle_accel, r2_angle, r2_angle_rate, r2_angle_accel, end_effector_pos)
 		
 
 if __name__ == '__main__':

bin/VehicleWithArm.py

 	WHEEL_L_PARAMS = (1,0.2,0.3,(0,0,0.5))
 	
 	LINK1_PARAMS = (1,0.8, 0.1,(0,0,0)) # (density, length, radius, center)
-	LINK2_PARAMS = (1,0.8,0.1,(0,0.5,0.5))
-	LINK3_PARAMS = (1,0.5,0.1,(0,0.8,1))
+	LINK2_PARAMS = (1,0.6,0.1,(0,0.7,0.2))
 	
 	def __init__(self, use_capsule_wheels=False):
 		""" Constructor, which calls the superclass constructor. """
 		#=======================================================================
 		link1 = self.sim.add_capsule(*self.LINK1_PARAMS)
 		link2 = self.sim.add_capsule(*self.LINK2_PARAMS)
-		link3 = self.sim.add_capsule(*self.LINK3_PARAMS)
 		
 		# bodies are rotated before attaching themselves through joints
 		self.sim.get_object(link1).rotate(x_axis, mut.pi/2)
-		self.sim.get_object(link3).rotate(x_axis, mut.pi/2)
+		self.sim.get_object(link2).rotate(x_axis, mut.pi/2)
 		
 		self.sim.get_object(link1).offset_by_object(self.sim.get_object(chassis))
 		self.sim.get_object(link1).offset_by_position(arm_offset)
 		self.sim.get_object(link2).offset_by_object(self.sim.get_object(link1))
-		self.sim.get_object(link3).offset_by_object(self.sim.get_object(link1))
 		
 		self.sim.add_rotary_joint('r1', 
 							self.sim.get_object(chassis), 
 							self.sim.get_object(link1), 
 							None, y_axis)
+		r2_anchor = mut.sub3(self.sim.get_object(link2).get_position(),
+							(0,self.LINK2_PARAMS[1]/2,0))
 		self.sim.add_rotary_joint('r2', 
 							self.sim.get_object(link1), 
 							self.sim.get_object(link2), 
-							None, z_axis)
-		self.sim.add_rotary_joint('r3', 
-							self.sim.get_object(link2), 
-							self.sim.get_object(link3), 
-							None, y_axis)
+							r2_anchor, z_axis)
 		
 		# test
 		#print(self.sim.get_object(wheelR).get_actor().set_color((0.8,0,0)))
 		
 		self.link1 = link1
 		self.link2 = link2
-		self.link3 = link3
 		
 	def go_forwards(self):
 		""" Rotate both powered wheels in the same direction, forwards """