def motor_pub(): p1 = rospy.Publisher('/gBot/left_wheel/cmd', JointState) p2 = rospy.Publisher('/gBot/right_wheel/cmd', JointState) rospy.init_node('motorPub', anonymous=True) rospy.on_shutdown(motor_stop) #Message for left_wheel msg1 = JointState() msg1.name = ["left_wheel"] msg1.position = [0.0] msg1.velocity = [-100] #Message for right_wheel msg2 = JointState() msg2.name = ["right_wheel"] msg2.position = [0.0] msg2.velocity = [-100] while not rospy.is_shutdown(): msg1.header.stamp = rospy.Time.now() p1.publish(msg1) msg2.header.stamp = rospy.Time.now() p2.publish(msg2) rospy.loginfo('Left Motor velocity') rospy.loginfo(msg1.velocity) rospy.loginfo('Right Motor velocity') rospy.loginfo(msg2.velocity) rospy.sleep(0.1)
def execute(self, userdata): # rospy.loginfo('Executing state %s', self.__class__.__name__) # self.active = True # self.first_call = True # # wait for some time to read from the topic # # TODO: replace with a service call # rospy.sleep(3) # userdata.obj_list = self.obj_list # self.active = False # return 'succeeded' rospy.loginfo("Executing state %s", self.__class__.__name__) self.obj_list = [] i = 1 for view in userdata.view_list: rospy.loginfo("%i view: set PTU to %s", i, view) joint_state = JointState() joint_state.header.frame_id = "tessdaf" joint_state.name = ["pan", "tilt"] joint_state.position = [float(view[0]), float(view[1])] joint_state.velocity = [float(1.0), float(1.0)] joint_state.effort = [float(1.0), float(1.0)] self.ptu_cmd.publish(joint_state) # wait until PTU has finished or point cloud get screwed up rospy.sleep(2) rospy.loginfo("%i view: receive from semantic camera", i) self.active = True self.first_call = True # wait for some time to read once from the topic and store it onto self.pointcloud # TODO: replace with a service call rospy.sleep(2) self.active = False i = i + 1 userdata.obj_list = self.obj_list # back to original position joint_state = JointState() joint_state.header.frame_id = "tessdaf" joint_state.name = ["pan", "tilt"] joint_state.position = [float(0.0), float(0.0)] joint_state.velocity = [float(1.0), float(1.0)] joint_state.effort = [float(1.0), float(1.0)] self.ptu_cmd.publish(joint_state) return "succeeded"
def execute(self, goal): pickle.dump(goal, open("trajecoryGoalMsgVrep.data", "wb")) joint_names = goal.trajectory.joint_names trajectory_points = goal.trajectory.points num_points = len(trajectory_points) rospy.loginfo("Received %i trajectory points" % num_points) pprint(goal.trajectory) end_time = trajectory_points[-1].time_from_start.to_sec() control_rate = rospy.Rate(self._control_rate) i=num_points rospy.loginfo("the num of point is %i" % i ) #self.jointTrajectoryPublisher.publish(goal.trajectory) for trajectory_point in trajectory_points: #Publish for vrepp jsVrep = JointState() jsVrep.name = copy.deepcopy(joint_names) jsVrep.position = trajectory_point.positions for j in range(len(joint_names)): jsVrep.name[j] = self.vrep_joint_names[joint_names[j]] vrep_joint_index = int(jsVrep.name[j][-1]) - 1 print vrep_joint_index if vrep_joint_index == 4: self.jointStatePublisherForVrep[vrep_joint_index].publish((-1*jsVrep.position[j]) + self.vrep_position_offset[vrep_joint_index]) else: self.jointStatePublisherForVrep[vrep_joint_index].publish(jsVrep.position[j] + self.vrep_position_offset[vrep_joint_index]) #Publisher for real robot js = JointState() #rospy.loginfo("the length of js.name is %i" %len(js.name)) js.name=copy.deepcopy(joint_names) for j in range(len(joint_names)): #rospy.loginfo("j is %i" % j) js.name[j] = js.name[j]+'_'+str(i) i=i-1 js.position = trajectory_point.positions self.jointStatePublisher.publish(js) control_rate.sleep() start_time = rospy.get_time() now_from_start = rospy.get_time() - start_time last_idx = -1 self._result.error_code = self._result.SUCCESSFUL self.server.set_succeeded(self._result) rospy.sleep(5) self.execution.publish("OK")
def gripper_cb(self, data): js = JointState() js.header = data.header js.name = ["Gripper"] js.position = [data.current_pos] js.velocity = [data.velocity] self.gripper_pub.publish(js)
def handstate_callback(handstate_msg): assert len(handstate_msg.positions) == 4 assert len(handstate_msg.inner_links) == 3 assert len(handstate_msg.outer_links) == 3 jointstate_msg = JointState() jointstate_msg.header.stamp = handstate_msg.header.stamp jointstate_msg.name = [ joint_prefix + '00', # Joint j01 is a mimic joint that is published by robot_state_publisher. joint_prefix + '01', joint_prefix + '11', joint_prefix + '21', # TODO: These are not currently being published. See below. #joint_prefix + '02', #joint_prefix + '12', #joint_prefix + '22', ] jointstate_msg.position += [ handstate_msg.positions[3] ] jointstate_msg.position += handstate_msg.inner_links # TODO: These positions look very wrong, so we'll let robot_state_publisher # use the mimic ratios in HERB's URDF to compute the distal joint angles. #jointstate_msg.position += handstate_msg.outer_links jointstate_pub.publish(jointstate_msg)
def joint_trajectory_to_joint_state(self, joint_trajectory_msg, index): joint_states_msg = JointState() joint_states_msg.name = joint_trajectory_msg.joint_names joint_states_msg.position = joint_trajectory_msg.points[index].positions joint_states_msg.velocity = joint_trajectory_msg.points[index].velocities joint_states_msg.effort = joint_trajectory_msg.points[index].effort return joint_states_msg
def execute(self, goal): #pickle.dump(goal, open("trajecoryGoalMsg3.data", "wb")) joint_names = goal.trajectory.joint_names trajectory_points = goal.trajectory.points num_points = len(trajectory_points) rospy.loginfo("Received %i trajectory points" % num_points) pprint(goal.trajectory) end_time = trajectory_points[-1].time_from_start.to_sec() control_rate = rospy.Rate(self._control_rate) i=num_points rospy.loginfo("the num of point is %i" % i ) self.jointTrajectoryPublisher.publish(goal.trajectory) for trajectory_point in trajectory_points: js = JointState() #rospy.loginfo("the length of js.name is %i" %len(js.name)) js.name=copy.deepcopy(joint_names) for j in range(len(joint_names)): #rospy.loginfo("j is %i" % j) js.name[j] = js.name[j]+'_'+str(i) i=i-1 #js.position = trajectory_point.positions #js.velocity=trajectory_point.velocities #self.jointStatePublisher.publish(js) control_rate.sleep() start_time = rospy.get_time() now_from_start = rospy.get_time() - start_time last_idx = -1 self._result.error_code = self._result.SUCCESSFUL self.server.set_succeeded(self._result)
def rc_talker(): '''init pyxhook''' #Create hookmanager hookman = pyxhook.HookManager() #Define our callback to fire when a key is pressed down hookman.KeyDown = key_down_event hookman.KeyUp = key_up_event #Hook the keyboard hookman.HookKeyboard() #Start our listener hookman.start() pub = rospy.Publisher('rc_sender', String, queue_size=10) # 2016-10-26 test urdf joint control # controls a file in ~/toy_code/toy_ros_space/src/rc_sim_oub/test.launch #roslaunch test.launch model:='$(find urdf_tutorial)/urdf/07-physics.urdf' gui:=True pub_2 = rospy.Publisher('test_joint', JointState, queue_size=10) rospy.init_node('rc_talker') rate = rospy.Rate(50) while not rospy.is_shutdown(): #hello_str = "hello world %s" % rospy.get_time() pub.publish(''.join(rc_keys)) # 2016-10-26 test urdf joint control msg = JointState() msg.header.stamp = rospy.Time.now() msg.name = ['base_to_left_arm', 'base_to_right_arm'] msg.position = [-0.3*int(rc_keys[0]),0.3*int(rc_keys[1])] pub_2.publish(msg) #print rc_keys rate.sleep() if running == False: #Close the listener when we are done hookman.cancel() break
def eyelidServosPut(self,params): msg = JointState() msg.name = list() msg.position = list() msg.velocity = list() msg.effort = list() if params.has_key('leftEyelid_position'): msg.name.append('left_eyelid_joint') msg.position.append(math.radians(float(params['leftEyelid_position'])-float(90))) if params.has_key('leftEyelid_speed'): #Anadir velocuidad msg.velocity.append(params['leftEyelid_speed']) else: msg.velocity.append(3.0) if params.has_key('rightEyelid_position'): msg.name.append('right_eyelid_joint') msg.position.append(math.radians(float(params['rightEyelid_position'])-float(90))) if params.has_key('rightEyelid_speed'): #Anadir velocuidad msg.velocity.append(params['rightEyelid_speed']) else: msg.velocity.append(3.0) msg.header.stamp = rospy.Time.now() self.cmd_joints_pub.publish(msg)
def sim_robot(): rospy.init_node("simulated_robot") pub = rospy.Publisher("joint_states", JointState, queue_size=5) rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0)) rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1)) rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2)) rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3)) rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4)) rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5)) msg = JointState() msg.header = Header(stamp=rospy.Time.now()) msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"] msg.position = [0] * 6 msg.velocity = [0, 0, 0, 0, 0, 0] msg.effort = [0, 0, 0, 0, 0, 0] def joint_cb(msg_in, id): msg.header = Header(stamp=rospy.Time.now()) msg.position[id] = msg_in.data rate = rospy.Rate(10) while not rospy.is_shutdown(): pub.publish(msg) rate.sleep()
def generateJointStatesFromRegisterStateAndPublish(): rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, updateLocalJointState) global currentJointState currentJointState = JointState() currentJointState.header.frame_id = "robotiq_s_model from real-time state data" currentJointState.name = ["finger_1_joint_1", "finger_1_joint_2", "finger_1_joint_3", "finger_2_joint_1", "finger_2_joint_2", "finger_2_joint_3", "finger_middle_joint_1", "finger_middle_joint_2", "finger_middle_joint_3", "palm_finger_1_joint", "palm_finger_2_joint"] currentJointState.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] currentJointState.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] currentJointState.effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] pub = rospy.Publisher('joint_states', JointState, queue_size=1) #no buffering? rospy.init_node('robotiq_s_model_joint_state_publisher', anonymous=False) rate = rospy.Rate(100) # publishes at 100Hz while not rospy.is_shutdown(): currentJointState.header.stamp = rospy.Time.now() pub.publish(currentJointState) rate.sleep() # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def joint_state_publisher(self): try: joint_state_msg = JointState() joint_fb_msg = FollowJointTrajectoryFeedback() time = rospy.Time.now() with self.lock: #Joint states joint_state_msg.header.stamp = time joint_state_msg.name = self.joint_names joint_state_msg.position = self.motion_ctrl.get_joint_positions() #self.joint_state_pub.publish(joint_state_msg) #Joint feedback joint_fb_msg.header.stamp = time joint_fb_msg.joint_names = self.joint_names joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions() #self.joint_feedback_pub.publish(joint_fb_msg) except Exception as e: rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def js_cb(self, a): rospy.loginfo('Received array') js = JointState() js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2'] jList = a.data jMatrix = np.reshape(jList,(jList.shape[0]/15,15)) i = 0 for pos in jMatrix: rospy.loginfo(pos) js.position = pos gsvr = GetStateValidityRequest() rs = RobotState() rs.joint_state = js gsvr.robot_state = rs gsvr.group_name = "both_arms" resp = self.coll_client(gsvr) if (resp.valid == False): rospy.loginfo('false') rospy.loginfo(i) self.js_pub.publish(0) return i = i + 1 self.js_pub.publish(1) rospy.loginfo('true') return
def set_stiffness(self): stiffness_message = JointState() stiffness_message.name = ["HeadYaw", "HeadPitch"] stiffness_message.position = [0,0] stiffness_message.velocity = [0,0] stiffness_message.effort = [1,1] self.stiffness_publisher.publish(stiffness_message)
def publish_position(): '''joint position publisher''' msg = JointState() msg.header.stamp = rospy.Time.now() msg.name = JOINT_NAME_ARRAY msg.position = joints_pos pub_js.publish(msg)
def resethead(): # rotate the head(not PTU) pub = rospy.Publisher('/head/commanded_state', JointState) head_command = JointState() head_command.name=["HeadPan"] head_command.position=[0.0] #forwards pub.publish(head_command) eyeplay(0)# reset eyes
def __on_packet(self, buf): global last_joint_states, last_joint_states_lock now = rospy.get_rostime() #stateRT = RobotStateRT.unpack(buf) stateRT = RobotStateRT_V32.unpack(buf) self.last_stateRT = stateRT msg = JointState() msg.header.stamp = now msg.header.frame_id = "From real-time state data" msg.name = joint_names msg.position = [0.0] * 6 for i, q in enumerate(stateRT.q_actual): msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0) msg.velocity = stateRT.qd_actual msg.effort = [0]*6 pub_joint_states.publish(msg) with last_joint_states_lock: last_joint_states = msg wrench_msg = WrenchStamped() wrench_msg.header.stamp = now wrench_msg.wrench.force.x = stateRT.tcp_force[0] wrench_msg.wrench.force.y = stateRT.tcp_force[1] wrench_msg.wrench.force.z = stateRT.tcp_force[2] wrench_msg.wrench.torque.x = stateRT.tcp_force[3] wrench_msg.wrench.torque.y = stateRT.tcp_force[4] wrench_msg.wrench.torque.z = stateRT.tcp_force[5] pub_wrench.publish(wrench_msg)
def _move_to(self, point, dur): rospy.sleep(dur) msg = JointState() with self.lock: if not self.sig_stop: self.joint_positions = point.positions[:] self.joint_velocities = point.velocities[:] msg.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7'] #msg.name = self.joint_names msg.position = self.joint_positions msg.velocity = self.joint_velocities #[0.2]*7 msg.header.stamp = rospy.Time.now() self.joint_command_pub.publish(msg) #rospy.loginfo('Moved to position: %s in %s', str(self.joint_positions), str(dur)) else: rospy.loginfo('Stoping motion immediately, clearing stop signal') self.sig_stop = False
def default(self, ci="unused"): """ Publish the data of the posture sensor as a ROS JointState message """ js = JointState() js.header = self.get_ros_header() js.name = [ "kuka_arm_0_joint", "kuka_arm_1_joint", "kuka_arm_2_joint", "kuka_arm_3_joint", "kuka_arm_4_joint", "kuka_arm_5_joint", "kuka_arm_6_joint", "head_pan_joint", "head_tilt_joint", ] js.position = [ self.data["seg0"], self.data["seg1"], self.data["seg2"], self.data["seg3"], self.data["seg4"], self.data["seg5"], self.data["seg6"], self.data["pan"], self.data["tilt"], ] # js.velocity = [1, 1, 1, 1, 1, 1, 1] # js.effort = [50, 50, 50, 50, 50, 50, 50] self.publish(js)
def update(self): if not self.driver_status == 'DISCONNECTED': # Get Joint Positions self.current_joint_positions = self.rob.getj() msg = JointState() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = "robot_secondary_interface_data" msg.name = self.JOINT_NAMES msg.position = self.current_joint_positions msg.velocity = [0]*6 msg.effort = [0]*6 self.joint_state_publisher.publish(msg) # Get TCP Position tcp_angle_axis = self.rob.getl() # Create Frame from XYZ and Angle Axis T = PyKDL.Frame() axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5]) # Get norm and normalized axis angle = axis.Normalize() # Make frame T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2]) T.M = PyKDL.Rotation.Rot(axis,angle) # Create Pose self.current_tcp_pose = tf_c.toMsg(T) self.current_tcp_frame = T self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
def computePositionsForNextJoint(self, currPositions, jointsToSweep, maxs, mins, joint_list): if len(jointsToSweep) != 0: for kk in range(0, int(1.0/self.resolutionOfSweeps)): # The actual sweeping currPositions[joint_list.index(jointsToSweep[0])] currPositions[joint_list.index(jointsToSweep[0])] = mins[joint_list.index(jointsToSweep[0])]+(self.resolutionOfSweeps*kk)*(maxs[joint_list.index(jointsToSweep[0])]-mins[joint_list.index(jointsToSweep[0])]) if (len(jointsToSweep) > 1): self.computePositionsForNextJoint(currPositions, jointsToSweep[1:len(jointsToSweep)], maxs, mins, joint_list) # Combine message and publish msg = JointState() msg.header.stamp = rospy.Time.now() msg.name = self.joint_list # print self.free_joints msg.position = currPositions msg.velocity = [0.0]*len(maxs) msg.effort = [0.0]*len(maxs) time.sleep(0.0001) self.pub.publish(msg) # Get End Effector Positions rate = rospy.Rate(20.0) try: (trans,rot) = self.listener.lookupTransform(self.baseLink, self.finalLink, rospy.Time(0)) if (self.currX != trans[0]) or (self.currY != trans[1] or self.currZ != trans[2]): self.csvWriter.writerow(trans) self.currX = trans[0] self.currY = trans[1] self.currZ = trans[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "ERROR: looking up tf" time.sleep(0.5) # Sleep for a bit to let system catch up continue
def generate_views(self): rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose) try: resp = self.nav_goals(10, self.inflation_radius, self.roi) if not resp.goals.poses: self.views = [] return for j in range(len(resp.goals.poses)): for i in range(0,self.views_at_pose): pose = resp.goals.poses[j] pan = random.uniform(self.min_pan, self.max_pan) tilt = random.uniform(self.min_tilt, self.max_tilt) jointstate = JointState() jointstate.name = ['pan', 'tilt'] jointstate.position = [pan, tilt] jointstate.velocity = [self.velocity, self.velocity] jointstate.effort = [float(1.0), float(1.0)] resp_ptu_pose = self.ptu_fk(pan,tilt,pose) p = Pose() view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose) self.views.append(view) except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
def publishJointState(self): msg = JointState() msg.header.stamp = rospy.Time.now() msg.name = self.jointNames msg.position = self.getJointAngles() msg.effort = self.getLoads() self.jointStatePub.publish(msg)
def publish(eventStop): # for arbotix pub1 = rospy.Publisher("q1/command", Float64, queue_size=5) pub2 = rospy.Publisher("q2/command", Float64, queue_size=5) pub3 = rospy.Publisher("q3/command", Float64, queue_size=5) pub4 = rospy.Publisher("q4/command", Float64, queue_size=5) pub5 = rospy.Publisher("q5/command", Float64, queue_size=5) # for visualization jointPub = rospy.Publisher( "/joint_states", JointState, queue_size=5) jmsg = JointState() jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5'] while not rospy.is_shutdown() and not eventStop.is_set(): jmsg.header.stamp = rospy.Time.now() jmsg.position = self.q if self.sim: jointPub.publish(jmsg) pub1.publish(self.q[0]) pub2.publish(self.q[1]) pub3.publish(self.q[2]) pub4.publish(self.q[3]) pub5.publish(self.q[4]) eventStop.wait(self.dt)
def head_wander(): rospy.init_node('head_wander') joints_pub = rospy.Publisher('/cmd_joints', JointState) say = rospy.ServiceProxy('/qbo_talk/festival_say', Text2Speach) seq = 0 while not rospy.is_shutdown(): seq += 1 msg = JointState() msg.header.seq = seq msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'base_footprint' msg.name = ['head_pan_joint','head_tilt_joint'] msg.position = [random.uniform(-0.9,0.9), random.uniform(-0.8,0.3)] joints_pub.publish(msg) if random.random() < 0.3: try: say(random.choice(UTTERANCES)) except rospy.ServiceException: # Ignore errors pass rospy.sleep(2.0)
def publishJointStates(self): jState = JointState() jState.header.stamp = rospy.Time.now() jState.name = self.names jState.position = self.positions jState.velocity = self.velocities jState.effort = self.efforts self.pub.publish(jState)
def __init__(self): global jspr2msg jspr2msg = JointState() jspr2msg.name = [ "left_shoulder_tilt_joint", "left_lin_act_cyl_joint", "left_upper_arm_hinge_joint","left_linear_actuator_joint", "right_shoulder_tilt_joint", "right_lin_act_cyl_joint", "right_upper_arm_hinge_joint","right_linear_actuator_joint", "torso_lift_joint", "wheel_linear_actuator_joint", "fl_anchor_rod_joint", "fl_push_rod_joint", "fl_caster_rotation_joint", "fr_anchor_rod_joint", "fr_push_rod_joint", "fr_caster_rotation_joint", "bl_anchor_rod_joint", "bl_push_rod_joint", "bl_caster_rotation_joint", "br_anchor_rod_joint", "br_push_rod_joint", "br_caster_rotation_joint"] jspr2msg.position = [0.0 for name in jspr2msg.name] jspr2msg.velocity = [0.0 for name in jspr2msg.name] rospy.loginfo("done init")
def js_pub_cb(self, params): js = JointState() js.header.stamp = rospy.Time.now() js.name = ['pan_joint', 'tilt_joint'] if self.joint_states[0] is None: return js.position = self.joint_states self.js_pub.publish(js)
def get_new_JointState(pan, tilt): j = JointState() j.header.stamp = rospy.Time.now() j.name = ["panservo", "tiltservo"] j.position = [pan, tilt] j.velocity = [] j.effort = [] return j
def calibration(portName1, simulated): print "INITIALIZING TORSO CALIBRATION..." rospy.init_node("torso") jointStates = JointState() jointStates.name = ["spine_connect","waist_connect","shoulders_connect", "shoulders_left_connect", "shoulders_right_connect"] jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0] pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose",Float32MultiArray, queue_size = 1) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1) msgCurrentPose = Float32MultiArray() msgCurrentPose.data = [0,0,0] rate = rospy.Rate(30) if not simulated: print "Torso.-> Trying to open serial port on \"" + portName1 + "\"" Roboclaw1.Open(portName1, 38400) #ttyACM0 --- M1: front --- M2: rear address = 0x80 print "Torso.-> Serial port openned on \"" + portName1 + "\" at 38400 bps (Y)" print "Torso.-> Clearing previous encoders readings" a, bumper , b = Roboclaw1.ReadEncM1(address) print "Torso.-> bumper ", bumper Roboclaw1.BackwardM2(address, 127) #Abajo- while bumper == 0 or bumper==1: a, bumper , b = Roboclaw1.ReadEncM1(address) print "Torso.->bumper ", bumper print "Torso.->bumper ", bumper Roboclaw1.BackwardM2(address, 0) torsoPos = 0 Roboclaw1.SetEncM2(address, torsoPos) Roboclaw1.WriteNVM(address) Roboclaw1.ForwardM2(address, 127) #Arriba+ a, torsoPos , b = Roboclaw1.ReadEncM2(address) a, bumper , b = Roboclaw1.ReadEncM1(address) while torsoPos < 84866 or bumper==-1: a, torsoPos , b = Roboclaw1.ReadEncM2(address) a, bumper , b = Roboclaw1.ReadEncM1(address) print "Torso.->bumper ", bumper Roboclaw1.BackwardM2(address, 0) a, torsoPos , b = Roboclaw1.ReadEncM2(address) #print "Torso.-> MAX Torso Pos: ", torsoPos Roboclaw1.SetEncM2(address, torsoPos) Roboclaw1.WriteNVM(address) Roboclaw1.Close() else: torsoPos = 84866; msgCurrentPose.data[0] = (torsoPos*0.352)/169733 #-------------------pulsos a metros msgCurrentPose.data[1] = 0.0 msgCurrentPose.data[2] = 0.0 pubTorsoPos.publish(msgCurrentPose) jointStates.header.stamp = rospy.Time.now() jointStates.position = [(torsoPos*0.352)/169733, 0.0, 0.0, 0.0, 0.0] pubJointStates.publish(jointStates)
import rospy import random import time from sensor_msgs.msg import JointState target = rospy.Publisher('joint_targets', JointState, queue_size=10) rospy.init_node('head_random') msg = JointState() msg.name = ["neck_axis0", "neck_axis1", "neck_axis2"] msg.velocity = [0, 0, 0] msg.effort = [0, 0, 0] while (not rospy.is_shutdown()): n0 = 0 % random.uniform(-0.05, 0.05) n1 = random.uniform(-0.2, 0) n2 = random.uniform(-0.3, 0.3) print("%f %f %f" % (n0, n1, n2)) msg.position = [n0, n2, n2] target.publish(msg) time.sleep(3) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def __init__(self): rclpy.init() super().__init__('state_publisher') qos_profile = QoSProfile(depth=10) self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile) self.broadcaster = TransformBroadcaster(self, qos=qos_profile) self.nodeName = self.get_name() self.get_logger().info("{0} started".format(self.nodeName)) degree = pi / 180.0 loop_rate = self.create_rate(30) # robot state tilt=0. wheel_right_joint = 0. tinc = degree wheel_left_joint = 0. angle = 0. height = 0. hinc = 0.005 # message declarations odom_trans = TransformStamped() odom_trans.header.frame_id = 'odom' odom_trans.child_frame_id = 'base_footprint' joint_state = JointState() try: while rclpy.ok(): rclpy.spin_once(self) # update joint_state now = self.get_clock().now() joint_state.header.stamp = now.to_msg() joint_state.name = ['wheel_right_joint', 'wheel_left_joint'] joint_state.position = [wheel_right_joint, wheel_left_joint] # update transform # (moving in a circle with radius=2) odom_trans.header.stamp = now.to_msg() odom_trans.transform.translation.x = cos(angle)*2 odom_trans.transform.translation.y = sin(angle)*2 odom_trans.transform.translation.z = 0.7 odom_trans.transform.rotation = \ euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw # send the joint state and transform self.joint_pub.publish(joint_state) self.broadcaster.sendTransform(odom_trans) # Create new robot state tilt += tinc if tilt < -0.5 or tilt > 0.0: tinc *= -1 height += hinc if height > 0.2 or height < 0.0: hinc *= -1 wheel_right_joint += degree angle += degree/4 # This will adjust as needed per iteration loop_rate.sleep() except KeyboardInterrupt: pass
def callback(data, args): global theta_now global pub tl = args[0] a2 = args[1] x = data.pose.position.x y = data.pose.position.y z = data.pose.position.z r2 = x**2 + y**2 + z**2 r = sqrt(r2) if r < sqrt(a2**2 + tl**2 - 1.41 * a2 * tl) or r > a2 + tl: rospy.logerr("Point exceeded workpace") return beta = abs(atan2(z, sqrt(x**2 + y**2))) theta1_list = [] theta2_list = [] theta3_list = [] theta1_list += [atan2(y, x)] theta1_list += [atan2(y, x) + 3.14] theta2_list += [abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) - beta] theta2_list += [-abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) - beta] theta2_list += [ -abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) + beta - 3.14 ] theta2_list += [ abs(acos((r2 + a2**2 - tl**2) / (2 * a2 * r))) + beta - 3.14 ] theta3_list += [acos((r2 - a2**2 - tl**2) / (2 * a2 * tl))] theta3_list += [-acos((r2 - a2**2 - tl**2) / (2 * a2 * tl))] flag1 = True flag2 = False if abs(theta1_list[0] - theta_now[0]) > 6: theta1 = theta1_list[0] flag1 = True elif abs(theta1_list[1] - theta_now[0]) > 6: theta1 = theta1_list[1] flag1 = False elif abs(theta1_list[0] - theta_now[0]) < abs(theta1_list[1] - theta_now[0]): theta1 = theta1_list[0] flag1 = True else: theta1 = theta1_list[1] flag1 = False if flag1: if abs(theta2_list[0] - theta_now[1] + 0.01) < abs(theta2_list[1] - theta_now[1]): theta2 = theta2_list[0] flag2 = True else: theta2 = theta2_list[1] flag2 = False else: if abs(theta2_list[2] - theta_now[1]) < abs(theta2_list[3] - theta_now[1]): theta2 = theta2_list[2] flag2 = False else: theta2 = theta2_list[3] flag2 = True if flag2: theta3 = theta3_list[1] else: theta3 = theta3_list[0] if abs(x) < 0.01 and abs(y) < 0.01: theta1 = theta_now[0] theta2 = theta_now[1] theta3 = theta_now[2] theta_now = [theta1, theta2, theta3] msg = JointState() msg.name = ['joint1', 'joint2', 'joint3'] msg.header.stamp = rospy.get_rostime() msg.position = theta_now pub.publish(msg)
def move_to_pose(self, target_pose, mode, move_duration=0): """ :param target_pose: [x,y,z,R,P,Y] or Pose msg :type target_pose: list or Pose :param mode: "trajectory" or "command" :type mode: str :param move_duration: time in seconds to complete move :type move_duration: float :return: """ xyz_bounds = [0.01, 0.01, 0.01] wxyz_bounds = [0.05, 0.05, 0.05] if not isinstance(target_pose, Pose): # xyz target_xyz = [0, 0, 0] for i in range(3): if target_pose[i] in ("", " "): xyz_bounds[i] = 1e5 else: target_xyz[i] = float(target_pose[i]) # RPY target_rpy = [0, 0, 0] # Tait-Byran Extrinsic xyz Euler angles for i in range(3): if target_pose[3 + i] in ("", " "): wxyz_bounds[i] = 1e5 else: target_rpy[i] = float(target_pose[3 + i]) # wxyz target_wxyz = transforms.quaternion_from_euler( target_rpy[0], target_rpy[1], target_rpy[2], axes='sxyz').tolist() else: target_xyz = [ target_pose.position.x, target_pose.position.y, target_pose.position.z ] target_wxyz = [ target_pose.orientation.w, target_pose.orientation.x, target_pose.orientation.y, target_pose.orientation.z ] print("Seed angles: {}".format( [round(ang, 4) for ang in self.hebi_wrap.get_joint_positions()])) print("Target End-Effector Pose...") print(" xyz: {}".format(target_xyz)) print(" wxyz: {}".format(target_wxyz)) print("Bounds...") print(" xyz: {}".format(xyz_bounds)) print(" wxyz: {}".format(wxyz_bounds)) # Get target joint angles target_jt_angles = self.trac_ik.get_ik( self.hebi_wrap.get_joint_positions(), target_xyz[0], target_xyz[1], target_xyz[2], target_wxyz[0], target_wxyz[1], target_wxyz[2], target_wxyz[3], xyz_bounds[0], xyz_bounds[1], xyz_bounds[2], wxyz_bounds[0], wxyz_bounds[1], wxyz_bounds[2]) if target_jt_angles is None: print("NO JOINT SOLUTION FOUND FOR END-EFFECTOR POSE " "\n(x: {}, y: {}, z: {}, W: {}, X: {}, Y: {}, Z: {})" "\nPLEASE TRY AGAIN.".format(*target_xyz + target_wxyz)) return False else: if mode == "trajectory": goal = TrajectoryGoal() start_wp = WaypointMsg() start_wp.names = self.hebi_mapping start_wp.positions = self.hebi_wrap.get_joint_positions() start_wp.velocities = [0] * len(self.hebi_mapping) start_wp.accelerations = [0] * len(self.hebi_mapping) goal.waypoints.append(start_wp) end_wp = WaypointMsg() end_wp.names = self.hebi_mapping end_wp.positions = target_jt_angles end_wp.velocities = [0] * len(self.hebi_mapping) end_wp.accelerations = [0] * len(self.hebi_mapping) goal.waypoints.append(end_wp) goal.times.extend([0, move_duration]) self._hold_position = False self.hebi_wrap.trajectory_action_client.send_goal_and_wait( goal) self._hold_joint_angles = self.hebi_wrap.get_joint_positions() self._hold_position = True else: # mode == "command" cmd = JointState() cmd.name = self.hebi_mapping cmd.position = target_jt_angles cmd.velocity = [0] * len(self.hebi_mapping) cmd.effort = [0] * len(self.hebi_mapping) self._hold_position = False self.hebi_wrap.joint_state_publisher.publish(cmd) self._hold_joint_angles = target_jt_angles self._hold_position = True return True
q = ikine_ur5(xd, q0) # Resulting position (end effector with respect to the base link) T = fkine_ur5(q) print('Obtained value:\n', np.round(T,3)) # Red marker shows the achieved position bmarker.xyz(T[0:3,3]) # Green marker shows the desired position bmarker_des.xyz(xd) # Objeto (mensaje) de tipo JointState jstate = JointState() # Asignar valores al mensaje jstate.header.stamp = rospy.Time.now() jstate.name = jnames # Add the head joint value (with value 0) to the joints jstate.position = q # Loop rate (in Hz) rate = rospy.Rate(100) # Continuous execution loop while not rospy.is_shutdown(): speed = 0.0001 + (0.001 - 0.0001) * ((joystick.axes[3] - (-1)) / (1 - (-1))) # Desired position xdnew = np.array([xd[0] + joystick.axes[0]*speed, xd[1] + joystick.axes[1]*speed, xd[2] + joystick.axes[5]*speed]) qnew = ikine_ur5(xdnew, q) if qnew is not False: xd = xdnew
def main(): print "INITIALIZING ARM NODE IN SIMULATION BY [EDD-II]" ###Connection with ROS rospy.init_node("head_simul_node") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = [ "arm_flex_joint", "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] jointStates.position = [0, 0, 0, 0] subPosition = rospy.Subscriber("/hardware/arm/goal_pose", Float32MultiArray, callbackPos) pubArmPose = rospy.Publisher("/hardware/arm/current_pose", Float32MultiArray, queue_size=1) #For test, these lines can be unneeded pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubArmBattery = rospy.Publisher("/hardware/robot_state/right_arm_battery", Float32, queue_size=1) loop = rospy.Rate(10) global goalAngles global goalGripper global speeds goalAngles = [0, 0, 0, 0] angles = [0, 0, 0, 0] speeds = [0.01, 0.01, 0.01, 0.01] goalGripper = 0 gripper = 0 gripperSpeed = 0.1 msgCurrentPose = Float32MultiArray() msgCurrentPose.data = [0, 0, 0, 0] msgCurrentGripper = Float32() msgCurrentGripper.data = 0 deltaAngles = [0, 0, 0, 0] deltaGripper = 0 while not rospy.is_shutdown(): for i in range(len(deltaAngles)): deltaAngles[i] = goalAngles[i] - angles[i] if deltaAngles[i] > speeds[i]: deltaAngles[i] = speeds[i] if deltaAngles[i] < -speeds[i]: deltaAngles[i] = -speeds[i] angles[i] += deltaAngles[i] jointStates.position[i] = angles[i] msgCurrentPose.data[i] = angles[i] deltaGripper = goalGripper - gripper if deltaGripper > gripperSpeed: deltaGripper = gripperSpeed if deltaGripper < -gripperSpeed: deltaGripper = -gripperSpeed gripper += deltaGripper jointStates.header.stamp = rospy.Time.now() pubJointStates.publish(jointStates) pubArmPose.publish(msgCurrentPose) msgBattery = Float32() msgBattery.data = 11.6 pubArmBattery.publish(msgBattery) loop.sleep()
def solveIK(targetFrame): ok, tree = parser.treeFromFile(rospkg.RosPack().get_path('rviz_animator') + "/models/robocam.xml") chain = tree.getChain('base', 'link_camera') plotter = Plotter() # 1. Solve for J4, J5_initial, J6 # First, convert quaternion orientation to XZY order Euler angles targetQuat = targetFrame.M.GetQuaternion() # Get quaternion from KDL frame (x, y, z, w) pitch, yaw, roll = tf.transformations.euler_from_quaternion(targetQuat, axes='rxzy') pitch_deg, yaw_deg, roll_deg = math.degrees(pitch), math.degrees(yaw), math.degrees(roll) J4_origin_orientation = posemath.toMsg(chain.getSegment(2).getFrameToTip()).orientation J4_offset = euler_from_quaternion([J4_origin_orientation.x, J4_origin_orientation.y, J4_origin_orientation.z, J4_origin_orientation.w])[0] J4_raw, J5_initial, J6 = pitch, yaw, roll J4 = J4_raw - J4_offset # 1. Complete above print("J4: {} J5_init: {} J6: {}".format(J4, J5_initial, J6)) chainAngles = PyKDL.JntArray(8) chainAngles[5], chainAngles[6], chainAngles[7] = J4, J5_initial, J6 chainFK = PyKDL.ChainFkSolverPos_recursive(chain) purpleFrame = PyKDL.Frame() brownFrame = PyKDL.Frame() purpleSuccess = chainFK.JntToCart(chainAngles, purpleFrame) # print("Purple success {}".format(purpleSuccess)) print("Target Orientation:\n{}".format(targetFrame.M)) print("Result Orientation:\n{}".format(purpleFrame.M)) brownSuccess = chainFK.JntToCart(chainAngles, brownFrame, segmentNr=7) # 2. Determine position of orange point # First, construct KDL chain of the 3 links involved in J4-J6 cameraOffsetChain = tree.getChain('link_pitch', 'link_camera') cameraJointAngles = PyKDL.JntArray(2) cameraJointAngles[0], cameraJointAngles[1] = J5_initial, J6 cameraOffsetChainFK = PyKDL.ChainFkSolverPos_recursive(cameraOffsetChain) cameraFrame = PyKDL.Frame() success = cameraOffsetChainFK.JntToCart(cameraJointAngles, cameraFrame) # print("FK Status: {}".format(success)) # print("Camera Frame: {}".format(cameraFrame)) # print("End Effector Joint Angles: {}".format([J4, J5_initial, J6])) orangePoint = targetFrame.p - (purpleFrame.p - brownFrame.p) plotter.addVector(targetFrame.p, "pink") plotter.addVector(orangePoint, "orange") plotter.addVector(purpleFrame.p, "purple") plotter.addVector(brownFrame.p, "brown") # print("Target Frame Position: {}".format(targetFrame.p)) # print("Camera Frame Position: {}".format(cameraFrame.p)) # print("Offset: {}".format(targetFrame.p - cameraFrame.p)) # 2. Complete: # 3. Convert orange point to cylindrical coordinates orange_X, orange_Y, orange_Z = orangePoint orange_R = math.sqrt(orange_X ** 2 + orange_Y ** 2) orange_Theta = math.atan2(orange_Y, orange_X) # Theta measured from global positive X axis # 3. Complete: (above) print("Orange R: {} Theta: {}".format(orange_R, math.degrees(orange_Theta))) # 4. Solve for J2 and J3 in the idealized R-Z plane targetVectorOrig = PyKDL.Vector(0, orange_R, orange_Z) plotter.addVector(targetVectorOrig, "targetRZOrig") # First, remove the fixed offsets from the wrist, elbow, and shoulder pieces wristEndFrame = PyKDL.Frame() wristStartFrame = PyKDL.Frame() elbowEndFrame = PyKDL.Frame() elbowStartFrame = PyKDL.Frame() shoulderEndFrame = PyKDL.Frame() shoulderStartFrame = PyKDL.Frame() chainFK.JntToCart(chainAngles, wristEndFrame, segmentNr=7) chainFK.JntToCart(chainAngles, wristStartFrame, segmentNr=5) chainFK.JntToCart(chainAngles, elbowEndFrame, segmentNr=4) chainFK.JntToCart(chainAngles, elbowStartFrame, segmentNr=3) chainFK.JntToCart(chainAngles, shoulderEndFrame, segmentNr=2) chainFK.JntToCart(chainAngles, shoulderStartFrame, segmentNr=0) plotter.addVector(wristEndFrame.p, "wristEndFrame") plotter.addVector(wristStartFrame.p, "wristStartFrame") plotter.addVector(elbowEndFrame.p, "elbowEndFrame") plotter.addVector(elbowStartFrame.p, "elbowStartFrame") plotter.addVector(shoulderEndFrame.p, "shoulderEndFrame") plotter.addVector(shoulderStartFrame.p, "shoulderStartFrame") wristOffset = wristEndFrame.p - wristStartFrame.p elbowOffset = elbowEndFrame.p - elbowStartFrame.p shoulderOffset = shoulderEndFrame.p - shoulderStartFrame.p targetVector = targetVectorOrig - wristOffset - elbowOffset - shoulderOffset plotter.addVector(targetVector, "targetRZ") # The following steps use the same labels as the classic 2D planar IK derivation a1, a2 = (shoulderEndFrame.p - elbowStartFrame.p).Norm(), (elbowEndFrame.p - wristStartFrame.p).Norm() _, ik_x, ik_y = targetVector q2_a = math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2)) q1_a = math.atan2(ik_y, ik_x) - math.atan2(a2 * math.sin(-q2_a), a1 + a2 * math.cos(-q2_a)) q2_b = -1 * math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2)) q1_b = math.atan2(ik_y, ik_x) + math.atan2(a2 * math.sin(q2_b), a1 + a2 * math.cos(q2_b)) # Choose 'better' set of q1_ab, q2_ab q1, q2 = q1_a, q2_a # TODO(JS): Is this always the better one? J2_initial = q1 J2_offset = math.radians(90) # J2's zero position is vertical, not horizontal J2 = J2_initial - J2_offset # Since we have a parallel link, the angle for J3 is not simply q2. Instead, use transversal J3_initial = q1 - q2 J3_offset = elbowStartFrame.M.GetRPY()[0] # J3's zero position is below horizontal J3 = J3_initial - J3_offset # 4. Complete (above) # print("J2: {} J3: {}".format(J2, J3)) # 5. Use the Theta from cylindrical coordinates as the J1 angle, and update J5 accordingly J1 = orange_Theta - math.radians(90) J5 = J5_initial - (orange_Theta - math.radians(90)) # 5. Complete (above) # print("J1: {} J5: {}".format(J1, J5)) jointAngles = [J1, J2, J3, J4, J5, J6] jointNames = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll'] print("\n\nFinal joint angles in radians: {}\n\n".format(jointAngles)) in_bounds = True for angle, name in zip(jointAngles, jointNames): limit = robot_urdf.joint_map[name].limit if angle < limit.lower or angle > limit.upper: in_bounds = False print("Joint {} out of bounds with angle {} not between {} and {}".format(name, angle, limit.lower, limit.upper)) print("All in bounds: {}".format(in_bounds)) solvedJoints = PyKDL.JntArray(8) solvedJoints[0], solvedJoints[1], solvedJoints[3], solvedJoints[5], solvedJoints[6], solvedJoints[7] = jointAngles solvedJoints[2], solvedJoints[4] = -1 * solvedJoints[1], -1 * solvedJoints[3] producedFrame = PyKDL.Frame() for i in range(chain.getNrOfSegments()): rc = chainFK.JntToCart(solvedJoints, producedFrame, segmentNr=i) plotter.addVector(producedFrame.p, "fk_produced_{}".format(i)) # print("Result: {}".format(rc)) # print("Output position: {}\nExpected position: {}".format(producedFrame.p, targetFrame.p)) # print("Output orientation: {}\nExpected orientation: {}".format(producedFrame.M, targetFrame.M)) # 6. (optional) Sanity check on solution: # sanityTest(BASE_TO_BASE_YAW, BASE_YAW_TO_BOTTOM_4B, targetFrame, cameraFrame, cameraOffsetChain, jointAngles) # 7. Create JointState message for return ret = JointState() ret.name = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll'] ret.header.stamp = rospy.Time.now() ret.position = jointAngles plotter.addGoal(ret) # plotter.publish() return in_bounds, ret
def handle(self): self.socket_lock = threading.Lock() self.last_joint_states = None setConnectedRobot(self) print "Handling a request" try: buf = self.recv_more() if not buf: return while True: #print "Buf:", [ord(b) for b in buf] # Unpacks the message type mtype = struct.unpack_from("!i", buf, 0)[0] buf = buf[4:] #print "Message type:", mtype if mtype == MSG_OUT: # Unpacks string message, terminated by tilde i = buf.find("~") while i < 0: buf = buf + self.recv_more() i = buf.find("~") if len(buf) > 2000: raise Exception( "Probably forgot to terminate a string: %s..." % buf[:150]) s, buf = buf[:i], buf[i + 1:] log("Out: %s" % s) elif mtype == MSG_JOINT_STATES: while len(buf) < 3 * (6 * 4): buf = buf + self.recv_more() state_mult = struct.unpack_from("!%ii" % (3 * 6), buf, 0) buf = buf[3 * 6 * 4:] state = [s / MULT_jointstate for s in state_mult] msg = JointState() msg.header.stamp = rospy.get_rostime() msg.name = joint_names msg.position = [0.0] * 6 for i, q_meas in enumerate(state[:6]): msg.position[i] = q_meas + joint_offsets.get( joint_names[i], 0.0) msg.velocity = state[6:12] msg.effort = state[12:18] self.last_joint_states = msg pub_joint_states.publish(msg) elif mtype == MSG_QUIT: print "Quitting" raise EOF("Received quit") elif mtype == MSG_WAYPOINT_FINISHED: while len(buf) < 4: buf = buf + self.recv_more() waypoint_id = struct.unpack_from("!i", buf, 0)[0] buf = buf[4:] print "Waypoint finished (not handled)" else: raise Exception("Unknown message type: %i" % mtype) if not buf: buf = buf + self.recv_more() except EOF, ex: print "Connection closed (command):", ex setConnectedRobot(None)
def main(): global iiwa0_reached global iiwa0_received_desiredEEInRob global iiwa0_desiredEEInRob_sent global iiwa1_reached global iiwa1_received_desiredEEInRob global iiwa1_desiredEEInRob_sent global exotica_complete global iiwa0_desiredEEInRob global iiwa1_desiredEEInRob global data_man_tool global iiwa0_currMsrTransform_sent global iiwa1_currMsrTransform_sent #Initialise ros # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('exotica_collision_detection', anonymous=True) rate = rospy.Rate(10) # 10hz #publisher pub_iiwa0_destJoints = rospy.Publisher('iiwa0_destJoints', JointState, queue_size=10) pub_iiwa0_destJoints_sent = rospy.Publisher('iiwa0_destJoints_sent', Bool, queue_size=10) pub_iiwa0_reached = rospy.Publisher('iiwa0_reached', Bool, queue_size=10) pub_iiwa1_destJoints = rospy.Publisher('iiwa1_destJoints', JointState, queue_size=10) pub_iiwa1_destJoints_sent = rospy.Publisher('iiwa1_destJoints_sent', Bool, queue_size=10) pub_iiwa1_reached = rospy.Publisher('iiwa1_reached', Bool, queue_size=10) pub_exotica_complete = rospy.Publisher('exotica_complete', Bool, queue_size=10) pub_iiwa0_desiredEEInRob_sent = rospy.Publisher( 'iiwa0_desiredEEInRob_sent', Bool, queue_size=10) pub_iiwa1_desiredEEInRob_sent = rospy.Publisher( 'iiwa1_desiredEEInRob_sent', Bool, queue_size=10) pub_iiwa0_currMsrTransform_sent = rospy.Publisher( 'iiwa0_currMsrTransform_sent', Bool, queue_size=10) pub_iiwa1_currMsrTransform_sent = rospy.Publisher( 'iiwa1_currMsrTransform_sent', Bool, queue_size=10) pub_iiwa0_currMsrTransform_received = rospy.Publisher( 'iiwa0_currMsrTransform_received', Bool, queue_size=10) pub_iiwa1_currMsrTransform_received = rospy.Publisher( 'iiwa1_currMsrTransform_received', Bool, queue_size=10) #subscriber rospy.Subscriber("iiwa0_desiredEEInRob", Float64MultiArray, callback_iiwa0_desiredEEInRob) rospy.Subscriber("iiwa0_reached", Bool, callback_iiwa0_reached) rospy.Subscriber("iiwa0_msrTransform", Float64MultiArray, callback_iiwa0_msrTransform) rospy.Subscriber("iiwa0_desiredEEInRob_sent", Bool, callback_iiwa0_desiredEEInRob_sent) rospy.Subscriber("iiwa1_desiredEEInRob", Float64MultiArray, callback_iiwa1_desiredEEInRob) rospy.Subscriber("iiwa1_reached", Bool, callback_iiwa1_reached) rospy.Subscriber("iiwa1_msrTransform", Float64MultiArray, callback_iiwa1_msrTransform) rospy.Subscriber("iiwa1_desiredEEInRob_sent", Bool, callback_iiwa1_desiredEEInRob_sent) rospy.Subscriber("iiwa0_currMsrTransform_sent", Bool, callback_iiwa0_currMsrTransform_sent) rospy.Subscriber("iiwa1_currMsrTransform_sent", Bool, callback_iiwa1_currMsrTransform_sent) pub_exotica_complete.publish(True) iiwa0_desiredEEInRob_sent = True ####################delete iiwa1_desiredEEInRob_sent = False ####################delete iiwa1_reached = True ####################delete iiwa1_currMsrTransform_sent = True ####################delete while (1): #if (iiwa0_desiredEEInRob_sent or iiwa1_desiredEEInRob_sent) and iiwa0_currMsrTransform_sent and iiwa1_currMsrTransform_sent: if (iiwa0_desiredEEInRob_sent or iiwa1_desiredEEInRob_sent ) and iiwa0_currMsrTransform_sent and iiwa1_currMsrTransform_sent: print iiwa0_currMsrTransform_sent iiwa0_currMsrTransform_sent = False iiwa1_currMsrTransform_sent = False pub_iiwa0_currMsrTransform_sent.publish(False) pub_iiwa1_currMsrTransform_sent.publish(False) pub_iiwa0_currMsrTransform_received.publish(True) pub_iiwa1_currMsrTransform_received.publish(True) print data_man_tool[0, 12:24] #break; pub_exotica_complete.publish(False) if iiwa0_desiredEEInRob_sent: #data_man_tool[1,12:24] = iiwa0_desiredEEInRob data_man_tool[1, 12:24] = [ -4.800266e-01, -8.635465e-01, 1.544729e-01, 1.432567e-01, 2.227581e-01, -2.903022e-01, -9.306468e-01, -7.883426e-02, 8.485006e-01, -4.123251e-01, 3.317147e-01, 5.948348e-02 ] ###########################delete if iiwa1_desiredEEInRob_sent: data_man_tool[1, 24:36] = iiwa1_desiredEEInRob if not iiwa0_desiredEEInRob_sent: data_man_tool[1, 12:24] = data_man_tool[0, 12:24] if not iiwa1_desiredEEInRob_sent: data_man_tool[1, 24:36] = data_man_tool[0, 24:36] #print "iiwa0_desiredEEInRob" #print data_man_tool[1,12:24] #print "iiwa1_desiredEEInRob" #print data_man_tool[1,24:36] exo.Setup.initRos() write_smoothed_traj(0, 0) solver = exo.Setup.loadSolver( '{stentgraft_sewing_planning}/resources/aico_trajectory.xml') problem = solver.getProblem() for t in range(0, problem.T): problem.setRho('Frame0', 1e5, t) problem.setRho('Frame1', 1e5, t) solution = solver.solve() if not is_trajectory_valid(problem, solution): print 'Trajectory invalid' save_trajectory(solution) #print type(solution) i = 0 print np.size(solution, 0) print data_man_tool[0, 12:24] break ##sending trajectories while (i < np.size(solution, 0)): if iiwa0_desiredEEInRob_sent: msg0 = JointState() msg0.name = [ "iiwa0 joint 1", "iiwa0 joint 2", "iiwa0 joint 3", "iiwa0 joint 4", "iiwa0 joint 5", "iiwa0 joint 6", "iiwa0 joint 7" ] msg0.position = solution[i, 0:7] #print msg0.position pub_iiwa0_destJoints.publish(msg0) if iiwa1_desiredEEInRob_sent: msg1 = JointState() msg1.name = [ "iiwa1 joint 1", "iiwa1 joint 2", "iiwa1 joint 3", "iiwa1 joint 4", "iiwa1 joint 5", "iiwa1 joint 6", "iiwa1 joint 7" ] msg1.position = solution[i, 7:14] #print msg1.position pub_iiwa1_destJoints.publish(msg1) rate.sleep() #print i; #print "iiwa0_reached ", #print iiwa0_reached #print "iiwa1_reached ", #print iiwa1_reached if (iiwa0_desiredEEInRob_sent and iiwa1_desiredEEInRob_sent and iiwa0_reached and iiwa1_reached): i = i + 1 iiwa0_reached = False iiwa1_reached = False pub_iiwa0_reached.publish(False) pub_iiwa1_reached.publish(False) if (iiwa0_desiredEEInRob_sent and not iiwa1_desiredEEInRob_sent and iiwa0_reached and iiwa1_reached): i = i + 1 iiwa0_reached = False iiwa1_reached = True pub_iiwa0_reached.publish(False) pub_iiwa1_reached.publish(True) if (not iiwa0_desiredEEInRob_sent and iiwa1_desiredEEInRob_sent and iiwa0_reached and iiwa1_reached): i = i + 1 iiwa0_reached = True iiwa1_reached = False pub_iiwa0_reached.publish(True) pub_iiwa1_reached.publish(False) pub_exotica_complete.publish(True) iiwa0_desiredEEInRob_sent = False iiwa1_desiredEEInRob_sent = False pub_iiwa0_desiredEEInRob_sent.publish(False) pub_iiwa1_desiredEEInRob_sent.publish(False)
def main(portName1, portBaud1): print "JustinaHardwareLeftArm.->INITIALIZING LEFT ARM NODE BY MARCOSOFT..." global objOnHand global speedGripper global torqueGripper global gripperGoal_1 global gripperGoal_2 global torqueGripperCCW1 global torqueGripperCCW2 global gripperTorqueLimit global gripperTorqueActive global goalPos global torqueMode global speedsGoal global newGoalPose global armTorqueActive global attemps ###Communication with dynamixels: global dynMan1 print "JustinaHardwareLeftArm.->Trying to open port " + portName1 + " at " + str(portBaud1) dynMan1 = Dynamixel.DynamixelMan(portName1, portBaud1) msgCurrentPose = Float32MultiArray() msgCurrentGripper = Float32() msgBatery = Float32() msgObjOnHand = Bool() msgCurrentPose.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] msgCurrentGripper.data = 0.0 msgBatery = 0.0 msgObjOnHand = False curretPos = [0,0,0,0,0,0,0,0] bitsPerRadian = (4095)/((360)*(3.141592/180)) bitValues = [0,0,0,0,0,0,0,0,0,0,0] lastValues = [0,0,0,0,0,0,0,0,0,0,0] goalPos = [0,0,0,0,0,0,0] speedsGoal=[0, 0,0,0,0,0,0] newGoalPose = False objOnHand = False torqueGripper = 0 currentLoad_D21 = 0 currentLoad_D22 = 0 posD21 = 0.0 posD22 = 0.0 gripperGoal_1 = zero_gripper[0] gripperGoal_2 = zero_gripper[1] attemps = 0 i = 0 ############################## ### Connection with ROS #### rospy.init_node("left_arm") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = ["la_1_joint", "la_2_joint", "la_3_joint", "la_4_joint", "la_5_joint", "la_6_joint", "la_7_joint", "la_grip_left", "la_grip_right"] jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] subPos = rospy.Subscriber("/hardware/left_arm/goal_pose", Float32MultiArray, callbackArmPos) subGripper = rospy.Subscriber("/hardware/left_arm/goal_gripper", Float32, callbackGripperPos) subTorqueGripper = rospy.Subscriber("/hardware/left_arm/torque_gripper", Float32, callbackGripperTorque) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1) pubArmPose = rospy.Publisher("left_arm/current_pose", Float32MultiArray, queue_size = 1) pubGripper = rospy.Publisher("left_arm/current_gripper", Float32, queue_size = 1) pubObjOnHand = rospy.Publisher("left_arm/object_on_hand", Bool, queue_size = 1) pubBatery = rospy.Publisher("/hardware/robot_state/left_arm_battery", Float32, queue_size = 1) ##################### ## Dynamixel ## for i in range(9): #dynMan1.SetDGain(i, 25) #dynMan1.SetPGain(i, 16) #dynMan1.SetIGain(i, 6) dynMan1.SetDGain(i, 0) dynMan1.SetPGain(i, 32) dynMan1.SetIGain(i, 0) ### Set servos features for i in range(9): dynMan1.SetMaxTorque(i, 1023) dynMan1.SetTorqueLimit(i, 768) dynMan1.SetHighestLimitTemperature(i, 80) dynMan1.SetAlarmShutdown(i, 0b00000100) #Dynamixel gripper on position Mode... dynMan1.SetCWAngleLimit(7, 0) dynMan1.SetCCWAngleLimit(7, 4095) dynMan1.SetCWAngleLimit(8, 0) dynMan1.SetCCWAngleLimit(8, 4095) dynMan1.SetMovingSpeed(7, 100) dynMan1.SetMovingSpeed(8, 100) # Set torque_active for each servo for i in range(7): dynMan1.SetTorqueEnable(i, 1) dynMan1.SetTorqueEnable(7, 1) dynMan1.SetTorqueEnable(8, 1) # Set initial pos for each servo #for i in range(6): # dynMan1.SetGoalPosition(i, zero_arm[i]) #dynMan1.SetGoalPosition(6, 2130) #dynMan1.SetGoalPosition(7, zero_gripper[0]) #dynMan1.SetGoalPosition(8, zero_gripper[1]) loop = rospy.Rate(30) while not rospy.is_shutdown(): #### Refresh arm_position #### if newGoalPose: newGoalPose = False for i in range(7): dynMan1.SetTorqueLimit(i, 768) dynMan1.SetTorqueEnable(i, True) dynMan1.SetMovingSpeed(i, speedsGoal[i]) dynMan1.SetGoalPosition(i, goalPos[i]) rospy.sleep(0.05) #### Refresh gripper_pos #### if attemps < 50: if gripperTorqueActive : dynMan1.SetCWAngleLimit(7, 0) dynMan1.SetCCWAngleLimit(7, 0) dynMan1.SetCWAngleLimit(8, 0) dynMan1.SetCCWAngleLimit(8, 0) dynMan1.SetTorqueLimit(7, torqueGripper) dynMan1.SetTorqueLimit(8, torqueGripper) dynMan1.SetTorqueVale(7, speedGripper, torqueGripperCCW1) dynMan1.SetTorqueVale(8, speedGripper, torqueGripperCCW2) currentLoad_D21 = dynMan1.GetPresentLoad(7) currentLoad_D22 = dynMan1.GetPresentLoad(8) else: dynMan1.SetCWAngleLimit(7, 0) dynMan1.SetCCWAngleLimit(7, 4095) dynMan1.SetCWAngleLimit(8, 0) dynMan1.SetCCWAngleLimit(8, 4095) dynMan1.SetTorqueLimit(7, 500) dynMan1.SetTorqueLimit(8, 500) dynMan1.SetMovingSpeed(7, 200) dynMan1.SetMovingSpeed(8, 200) dynMan1.SetGoalPosition(7, gripperGoal_1) dynMan1.SetGoalPosition(8, gripperGoal_2) objOnHand = False ## This counter is reseated in the callback attemps += 1 #### Refresh arms_position's readings ##### for i in range(9): bitValues[i] = dynMan1.GetPresentPosition(i) if(bitValues[i] == 0): bitValues[i] = lastValues[i] else: lastValues[i] = bitValues[i] # CurrentLoad > 1023 means the oposite sense of load if currentLoad_D21 > 1023: currentLoad_D21 -= 1023 if currentLoad_D22 > 1023: currentLoad_D22 -= 1023 if gripperTorqueActive: currentLoad_D21 = (currentLoad_D21 + currentLoad_D22)/2 if currentLoad_D21 > 200 and posD21 > -0.05 : objOnHand = True else: objOnHand = False #print "Load_left_gripper: " + str(currentLoad_D21) #print "pos_left_gripper: " + str(posD21) pos0 = float(-(zero_arm[0]-bitValues[0])/bitsPerRadian) pos1 = float(-(zero_arm[1]-bitValues[1])/bitsPerRadian) pos2 = float(-(zero_arm[2]-bitValues[2])/bitsPerRadian) pos3 = float( (zero_arm[3]-bitValues[3])/bitsPerRadian) pos4 = float(-(zero_arm[4]-bitValues[4])/bitsPerRadian) pos5 = float(-(zero_arm[5]-bitValues[5])/bitsPerRadian) pos6 = float(-(zero_arm[6]-bitValues[6])/bitsPerRadian) posD21 = float(-(zero_gripper[0]-bitValues[7])/bitsPerRadian) posD22 = float( (zero_gripper[1]-bitValues[8])/bitsPerRadian) jointStates.header.stamp = rospy.Time.now() jointStates.position[0] = pos0 jointStates.position[1] = pos1 jointStates.position[2] = pos2 jointStates.position[3] = pos3 jointStates.position[4] = pos4 jointStates.position[5] = pos5 jointStates.position[6] = pos6 jointStates.position[7] = posD21 jointStates.position[8] = posD22 msgCurrentPose.data[0] = pos0 msgCurrentPose.data[1] = pos1 msgCurrentPose.data[2] = pos2 msgCurrentPose.data[3] = pos3 msgCurrentPose.data[4] = pos4 msgCurrentPose.data[5] = pos5 msgCurrentPose.data[6] = pos6 msgCurrentGripper.data = posD21 msgObjOnHand = objOnHand pubJointStates.publish(jointStates) pubArmPose.publish(msgCurrentPose) pubGripper.publish(msgCurrentGripper) pubObjOnHand.publish(msgObjOnHand) if i == 20: msgBatery = float(dynMan1.GetPresentVoltage(2)/10.0) pubBatery.publish(msgBatery) i=0 i+=1 loop.sleep() dynMan1.SetTorqueDisable(0) dynMan1.SetTorqueDisable(1) dynMan1.SetTorqueDisable(2) dynMan1.SetTorqueDisable(3) dynMan1.SetTorqueDisable(4) dynMan1.SetTorqueDisable(5) dynMan1.SetTorqueDisable(6) dynMan1.Close()
from sensor_msgs.msg import JointState from math import pi, sin import rospy topic = "joint_states" publisher = rospy.Publisher(topic, JointState, queue_size=1) rospy.init_node("send_efforts") t = 0 msg = JointState() msg.name = [ "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", ] N = len(msg.name) msg.position = [0.0 for i in range(N)] while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() msg.effort = [10.0 * sin(t) for i in range(N)] publisher.publish(msg) t = t + 0.1 rospy.sleep(0.1)
def handleUpdateResponse(r): """Handles the response for an update request Parses the message received from the robot, then publish the result Arguments: r {bytearray} -- response from the robot Returns: RobotState -- state object """ global last_state global update_response global stored_pose_count global do_update global p_joint_states global p_robot_state i = 0 # dropping the \0 from the end r = r[:-1].split(" ") try: state = rosweld_drivers.msg.RobotState() state.speed = int(r[i]) i = i + 1 state.pose.position.x = float(r[i]) / 1000 i = i + 1 state.pose.position.y = float(r[i]) / 1000 i = i + 1 state.pose.position.z = float(r[i]) / 1000 i = i + 1 state.euler_angles.rx = float(r[i]) i = i + 1 state.euler_angles.ry = float(r[i]) i = i + 1 state.euler_angles.rz = float(r[i]) i = i + 1 orientation = tf.transformations.quaternion_from_euler( radians(state.euler_angles.rz), radians(state.euler_angles.ry), radians(state.euler_angles.rx), axes) state.pose.orientation.x = orientation[0] state.pose.orientation.y = orientation[1] state.pose.orientation.z = orientation[2] state.pose.orientation.w = orientation[3] # -1 from the step index, because Hyundai's list indexing is from 1, not zero state.step = int(r[i]) - 1 i = i + 1 state.storedPoses = int(r[i]) i = i + 1 stateId = int(r[i]) state.robotProgramState = RobotStates.toString(stateId) state.mode = "Not supported" for _ in range(6): i = i + 1 state.joints.append(float(r[i])) update_response = True state.isMoving = last_state is not None and last_state.pose != state.pose if last_state != state or do_update: js = JointState() js.name = [ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_tip"] js.position = [ state.joints[0], -1 * (state.joints[1] - pi / 2), -1 * state.joints[2], state.joints[3], -1 * state.joints[4], state.joints[5], 0.0 ] p_joint_states.publish(js) p_robot_state.publish(state) do_update = False last_state = state stored_pose_count = state.storedPoses except Exception as e: global name status(name, "Robot update failed: %s" % (str(e)), STATE.ERROR) return state
rospy.wait_for_service('/compute_ik') ik_solution = rospy.ServiceProxy('/compute_ik', GetPositionIK) # ref_frame = Header() ref_frame.frame_id = "base_link" ref_frame.stamp = rospy.Time.now() # # fk_link_names fk_link_names = ["racket"] joint_state = JointState() joint_state.header = Header() joint_state.header.frame_id = "racket" joint_state.header.stamp = rospy.Time.now() joint_state.name = [ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" ] joint_state.position = [0, 0, 0, 0, 0, 0] robot_joint_state = RobotState() robot_joint_state.joint_state = joint_state position_1 = PoseStamped() position_1.header = ref_frame position_1.pose.position.x = 0.68 position_1.pose.position.y = 0.1 position_1.pose.position.z = 0.5 position_1.pose.orientation.x = 0 position_1.pose.orientation.y = 0 position_1.pose.orientation.z = 0 position_1.pose.orientation.w = 1
listener3 = rospy.Subscriber("pdcontroller_goal_simulation", PDControllerGoal8, callbackCurrentGoal, queue_size=1) # necessary for LabelApp2 arm_id = rospy.get_param('arm_id') jointStateMsgnames_suffixes = [ "_joint1", "_joint2", "_joint3", "_joint4", "_joint5", "_joint6", "_joint7", "_finger_joint1", "_finger_joint2" ] jointStateMsgnames = [arm_id + sfx for sfx in jointStateMsgnames_suffixes] print(jointStateMsgnames) j = JointState() j.name = jointStateMsgnames j.position = [0] * len(jointStateMsgnames) j.velocity = [0] * len(jointStateMsgnames) j.effort = [0] * len(jointStateMsgnames) rate = rospy.Rate(20) while not rospy.is_shutdown(): rate.sleep() if currentRobotState is not None: j.position[0:7] = currentRobotState.q[0:7] j.velocity[0:7] = currentRobotState.dq[0:7] j.effort[0:7] = currentRobotState.tau[0:7] j.position[7] = 0.5 * currentRobotState.q[7] j.velocity[7] = 0.5 * currentRobotState.dq[7] j.effort[7] = currentRobotState.tau[7] j.position[8] = j.position[7]
else: q[i] = acosr(D[:, i + 1].T @ vref) if (D[:, i + 1].T @ v < 0): q[i] = -q[i] if (np.abs(q[i]) > qlim[i]): print('a junta', i + 1, 'excedeu') return q #configurando o Rviz pub = rospy.Publisher('joint_states', JointState, queue_size=10) rospy.init_node('joint_state_publisher') rate = rospy.Rate(100) # 100hz hello_str = JointState() hello_str.header = Header() hello_str.header.stamp = rospy.Time.now() hello_str.name = ['One_joint', 'Two_joint', 'Three_joint', 'Four_joint'\ ,'Five_joint','Six_joint','Seven_joint','L_F_joint','R_F_joint'] hello_str.position = [0, 0, 0, 0, 0, 0, 0, 0, 0] hello_str.velocity = [] hello_str.effort = [] n = 7 #numero de juntas x = vetor([1, 0, 0]) y = vetor([0, 1, 0]) z = vetor([0, 0, 1]) #Gera uma pose aleatória desejada (posição + orientação) posicaod, orientd = random_pose() orientd2 = orientacao(orientd) destino = posicaod print('destino:', np.round(destino[:, 0], 3)) print('orientação', np.round(orientd2, 3))
if not rospy.is_shutdown() and _hold_position: jointstate = JointState() jointstate.name = hebi_wrap.hebi_mapping jointstate.position = _hold_joint_position jointstate.velocity = [] jointstate.effort = [] hebi_wrap.joint_state_publisher.publish(jointstate) hebi_wrap.add_feedback_callback(_hold_position_cb) # Main loop while not rospy.is_shutdown(): # Get user input valid_input = False user_input = None while not rospy.is_shutdown() and not valid_input: print("\nPlease enter target position:") user_input = float(raw_input()) valid_input = True jointstate = JointState() jointstate.name = hebi_wrap.hebi_mapping jointstate.position = [user_input] jointstate.velocity = [] jointstate.effort = [] _hold_position = False hebi_wrap.joint_state_publisher.publish(jointstate) _hold_joint_position = jointstate.position _hold_position = True
def talker(): global getMode, get_last_vel, myflags iter = 0 iter1 = 0 get_orientation = [] get_euler = [] get_matrix = [] get_velocity = [] get_invert = [] jointTorques = [] jointTorques1 = [] init_pos = [] middle_target = [] joint_Kp = [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10] joint_Kd = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] stand_target = [0.0, -0.8, 1.6, 0.0, -0.8, 1.6, 0.0, -0.8, 1.6, 0.0, -0.8, 1.6] pub1 = rospy.Publisher('/get_js', JointState, queue_size=100) pub2 = rospy.Publisher('/imu_body', Imu, queue_size=100) pub3 = rospy.Publisher('/get_com', commandDes, queue_size=100) imu_msg = Imu() setJSMsg = JointState() com_msg = commandDes() freq = 400 rate = rospy.Rate(freq) # hz while not rospy.is_shutdown(): if myflags == 100: p.resetBasePositionAndOrientation(boxId, [0, 0, 0.2], [0, 0, 0, 1]) time.sleep(1) for j in range(16): force = 0 p.setJointMotorControl2(boxId, j, p.VELOCITY_CONTROL, force=force) get_orientation = [] pose_orn = p.getBasePositionAndOrientation(boxId) for i in range(4): get_orientation.append(pose_orn[1][i]) get_euler = p.getEulerFromQuaternion(get_orientation) get_velocity = p.getBaseVelocity(boxId) get_invert = p.invertTransform(pose_orn[0], pose_orn[1]) get_matrix = p.getMatrixFromQuaternion(get_invert[1]) # IMU data imu_msg.orientation.x = pose_orn[1][0] imu_msg.orientation.y = pose_orn[1][1] imu_msg.orientation.z = pose_orn[1][2] imu_msg.orientation.w = pose_orn[1][3] imu_msg.angular_velocity.x = get_matrix[0] * get_velocity[1][0] + get_matrix[1] * get_velocity[1][1] + get_matrix[2] * get_velocity[1][2] imu_msg.angular_velocity.y = get_matrix[3] * get_velocity[1][0] + get_matrix[4] * get_velocity[1][1] + get_matrix[5] * get_velocity[1][2] imu_msg.angular_velocity.z = get_matrix[6] * get_velocity[1][0] + get_matrix[7] * get_velocity[1][1] + get_matrix[8] * get_velocity[1][2] # calculate the acceleration of the robot linear_X = (get_velocity[0][0] - get_last_vel[0]) * freq linear_Y = (get_velocity[0][1] - get_last_vel[1]) * freq linear_Z = 9.8 + (get_velocity[0][2] - get_last_vel[2]) * freq imu_msg.linear_acceleration.x = get_matrix[0] * linear_X + get_matrix[1] * linear_Y + get_matrix[2] * linear_Z imu_msg.linear_acceleration.y = get_matrix[3] * linear_X + get_matrix[4] * linear_Y + get_matrix[5] * linear_Z imu_msg.linear_acceleration.z = get_matrix[6] * linear_X + get_matrix[7] * linear_Y + get_matrix[8] * linear_Z # joint data joint_state = p.getJointStates(boxId, motor_id_list) setJSMsg.position = [joint_state[0][0], joint_state[1][0], joint_state[2][0], joint_state[3][0], joint_state[4][0], joint_state[5][0], joint_state[6][0], joint_state[7][0], joint_state[8][0], joint_state[9][0], joint_state[10][0], joint_state[11][0]] setJSMsg.velocity = [joint_state[0][1], joint_state[1][1], joint_state[2][1], joint_state[3][1], joint_state[4][1], joint_state[5][1], joint_state[6][1], joint_state[7][1], joint_state[8][1], joint_state[9][1], joint_state[10][1], joint_state[11][1]] # com data com_msg.com_position = [pose_orn[0][0], pose_orn[0][1], pose_orn[0][2]] com_msg.com_velocity = [get_velocity[0][0], get_velocity[0][1], get_velocity[0][2]] get_last_vel.clear() get_last_vel = com_msg.com_velocity # stand up control if len(get_effort): print(get_effort) p.setJointMotorControlArray(bodyUniqueId=boxId, jointIndices=motor_id_list, controlMode=p.TORQUE_CONTROL, forces=get_effort) setJSMsg.header.stamp = rospy.Time.now() setJSMsg.name = ["abduct_fr", "thigh_fr", "knee_fr", "abduct_fl", "thigh_fl", "knee_fl", "abduct_hr", "thigh_hr", "knee_hr", "abduct_hl", "thigh_hl", "knee_hl"] if myflags % 2 == 0: pub2.publish(imu_msg) pub1.publish(setJSMsg) pub3.publish(com_msg) myflags = myflags + 1 p.setTimeStep(0.0015) p.stepSimulation() rate.sleep()
#!/usr/bin/env python import rospy from sensor_msgs.msg import JointState rospy.init_node("fake_wheel_pub") pub = rospy.Publisher('joint_states', JointState, queue_size=5) rate = 20 r = rospy.Rate(rate) msg = JointState() msg.name = ["left_wheel_joint", "right_wheel_joint"] msg.position = [0.0 for name in msg.name] msg.velocity = [0.0 for name in msg.name] while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() pub.publish(msg) r.sleep()
def set_angles(self, angles): self.check_module("direct_control_module") msg = JointState() msg.name = angles.keys() msg.position = angles.values() self._pub_joints.publish(msg)
def _command_to_state(self, joint_names, joint_values): command_msg = JointState() command_msg.header.stamp = rospy.get_rostime() command_msg.name = joint_names command_msg.position = joint_values self._command_pub.publish(command_msg)
#!/usr/bin/env python import roslib; roslib.load_manifest("cob_arm_navigation") import rospy from sensor_msgs.msg import JointState rospy.init_node("tray_fake_pub") p = rospy.Publisher('joint_states', JointState) msg = JointState() msg.name = ['tray_joint'] msg.position = [0.0 for name in msg.name] msg.velocity = [0.0 for name in msg.name] while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() p.publish(msg) rospy.sleep(0.1)
def talker(): pubHeadPitch = rospy.Publisher('/nao_dcm/HeadPitch_position_controller/command', Float64, queue_size=10) pubHeadYaw = rospy.Publisher('/nao_dcm/HeadYaw_position_controller/command', Float64, queue_size=10) pubLAnklePitch = rospy.Publisher('/nao_dcm/LAnklePitch_position_controller/command', Float64, queue_size=10) pubLAnkleRoll = rospy.Publisher('/nao_dcm/LAnkleRoll_position_controller/command', Float64, queue_size=10) pubLElbowRoll = rospy.Publisher('/nao_dcm/LElbowRoll_position_controller/command', Float64, queue_size=10) pubLElbowYaw = rospy.Publisher('/nao_dcm/LElbowYaw_position_controller/command', Float64, queue_size=10) pubLHand = rospy.Publisher('/nao_dcm/LHand_position_controller/command', Float64, queue_size=10) pubLHipPitch = rospy.Publisher('/nao_dcm/LHipPitch_position_controller/command', Float64, queue_size=10) pubLHipRoll = rospy.Publisher('/nao_dcm/LHipRoll_position_controller/command', Float64, queue_size=10) pubLHipYawPitch = rospy.Publisher('/nao_dcm/LHipYawPitch_position_controller/command', Float64, queue_size=10) pubLKneePitch = rospy.Publisher('/nao_dcm/LKneePitch_position_controller/command', Float64, queue_size=10) pubLShoulderPitch = rospy.Publisher('/nao_dcm/LShoulderPitch_position_controller/command', Float64, queue_size=10) pubLShoulderRoll = rospy.Publisher('/nao_dcm/LShoulderRoll_position_controller/command', Float64, queue_size=10) pubLWristYaw = rospy.Publisher('/nao_dcm/LWristYaw_position_controller/command', Float64, queue_size=10) pubRAnklePitch = rospy.Publisher('/nao_dcm/RAnklePitch_position_controller/command', Float64, queue_size=10) pubRAnkleRoll = rospy.Publisher('/nao_dcm/RAnkleRoll_position_controller/command', Float64, queue_size=10) pubRElbowRoll = rospy.Publisher('/nao_dcm/RElbowRoll_position_controller/command', Float64, queue_size=10) pubRElbowYaw = rospy.Publisher('/nao_dcm/RElbowYaw_position_controller/command', Float64, queue_size=10) pubRHand = rospy.Publisher('/nao_dcm/RHand_position_controller/command', Float64, queue_size=10) pubRHipPitch = rospy.Publisher('/nao_dcm/RHipPitch_position_controller/command', Float64, queue_size=10) pubRHipRoll = rospy.Publisher('/nao_dcm/RHipRoll_position_controller/command', Float64, queue_size=10) pubRHipYawPitch = rospy.Publisher('/nao_dcm/RHipYawPitch_position_controller/command', Float64, queue_size=10) pubRKneePitch = rospy.Publisher('/nao_dcm/RKneePitch_position_controller/command', Float64, queue_size=10) pubRShoulderPitch = rospy.Publisher('/nao_dcm/RShoulderPitch_position_controller/command', Float64, queue_size=10) pubRShoulderRoll = rospy.Publisher('/nao_dcm/RShoulderRoll_position_controller/command', Float64, queue_size=10) pubRWristYaw = rospy.Publisher('/nao_dcm/RWristYaw_position_controller/command', Float64, queue_size=10) rospy.init_node('jointPublisher') rate = rospy.Rate(10) # 10hz hello_str = JointState() hello_str.header = Header() listener = tf.TransformListener() listener.waitForTransform("/head_1","/torso_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/left_hip_1","/torso_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/left_knee_1","/left_hip_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/left_foot_1","/left_knee_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/right_hip_1","/torso_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/right_knee_1","/right_hip_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/right_foot_1","/right_knee_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/left_shoulder_1","/torso_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/left_elbow_1","/left_shoulder_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/left_hand_1","/left_elbow_1",rospy.Time(), rospy.Duration(4.0)) listener.waitForTransform("/right_hand_1","/right_elbow_1",rospy.Time(), rospy.Duration(4.0)) thetaLPitchO = thetaLRollO = thetaRPitchO = thetaRRollO = 0 thetaLHipPitchO = thetaRHipPitchO = 0 thetaLHipRollO = thetaRHipRollO = 0 thetaLKneeO = thetaRKneeO = 0 thetaLAnklePitchO = thetaRAnklePitchO = 0 thetaLAnkleRollO = thetaRAnkleRollO = 0 HipPitchMin = -1.535889 HipPitchMax = 0.48409 HipRollMin = -0.379472 HipRollMax = 0.790477 KneeMin = -0.092346 KneeMax = 2.112528 AnklePitchMin = -1.189516 AnklePitchMax = 0.922747 AnkleRollMin = -0.397880 AnkleRollMax = 0.769001 while not rospy.is_shutdown(): hello_str.header.stamp = rospy.Time.now() now = rospy.Time.now() listener.waitForTransform("/head_1","/torso_1",now,rospy.Duration(4.0)) (transHead, rotHead) = listener.lookupTransform("/head_1", "/torso_1", now) eulerHead = tf.transformations.euler_from_quaternion(rotHead) listener.waitForTransform("/left_hip_1","/torso_1",now,rospy.Duration(4.0)) (transLHip, rotLHip) = listener.lookupTransform('/left_hip_1','/torso_1',now) eulerLHip = tf.transformations.euler_from_quaternion(rotLHip) listener.waitForTransform("/left_knee_1","/left_hip_1",now,rospy.Duration(4.0)) (transLKnee, rotLKnee) = listener.lookupTransform('/left_knee_1','/left_hip_1',now) eulerLKnee = tf.transformations.euler_from_quaternion(rotLKnee) listener.waitForTransform("/left_foot_1","/left_knee_1",now,rospy.Duration(4.0)) (transLAnkle, rotLAnkle) = listener.lookupTransform('/left_foot_1','/left_knee_1',now) eulerLAnkle = tf.transformations.euler_from_quaternion(rotLAnkle) listener.waitForTransform("/right_hip_1","/torso_1",now,rospy.Duration(4.0)) (transRHip, rotRHip) = listener.lookupTransform('/right_hip_1','/torso_1',now) eulerRHip = tf.transformations.euler_from_quaternion(rotRHip) listener.waitForTransform("/right_knee_1","/right_hip_1",now,rospy.Duration(4.0)) (transRKnee, rotRKnee) = listener.lookupTransform('/right_knee_1','/right_hip_1',now) eulerRKnee = tf.transformations.euler_from_quaternion(rotRKnee) listener.waitForTransform("/right_foot_1","/right_knee_1",now,rospy.Duration(4.0)) (transRAnkle, rotRAnkle) = listener.lookupTransform('/right_foot_1','/right_knee_1',now) eulerRAnkle = tf.transformations.euler_from_quaternion(rotRAnkle) listener.waitForTransform("/left_shoulder_1","/torso_1",now,rospy.Duration(4.0)) (transLShoulder, rotLShoulder) = listener.lookupTransform('/left_shoulder_1','/torso_1',now) eulerLShoulder = tf.transformations.euler_from_quaternion(rotLShoulder) listener.waitForTransform("/left_elbow_1","/left_shoulder_1",now,rospy.Duration(4.0)) (transLElbow, rotLElbow) = listener.lookupTransform('/left_elbow_1','/left_shoulder_1',now) eulerLElbow = tf.transformations.euler_from_quaternion(rotLElbow) listener.waitForTransform("/left_hand_1","/left_elbow_1",now,rospy.Duration(4.0)) (transLWrist, rotLWrist) = listener.lookupTransform('/left_hand_1','/left_elbow_1',now) eulerLWrist = tf.transformations.euler_from_quaternion(rotLWrist) listener.waitForTransform("/right_shoulder_1","/torso_1",now,rospy.Duration(4.0)) (transRShoulder, rotRShoulder) = listener.lookupTransform('/right_shoulder_1','/torso_1',now) eulerRShoulder = tf.transformations.euler_from_quaternion(rotRShoulder) listener.waitForTransform("/right_elbow_1","/right_shoulder_1",now,rospy.Duration(4.0)) (transRElbow, rotRElbow) = listener.lookupTransform('/right_elbow_1','/right_shoulder_1',now) eulerRElbow = tf.transformations.euler_from_quaternion(rotRElbow) listener.waitForTransform("/right_hand_1","/right_elbow_1",now,rospy.Duration(4.0)) (transRWrist, rotRWrist) = listener.lookupTransform('/right_hand_1','/right_elbow_1',now) eulerRWrist = tf.transformations.euler_from_quaternion(rotRWrist) hello_str.name = ['HeadYaw', 'HeadPitch', 'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll', 'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'LHand', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'RHand', 'RFinger23', 'RFinger13', 'RFinger12', 'LFinger21', 'LFinger13', 'LFinger11', 'RFinger22', 'LFinger22', 'RFinger21', 'LFinger12', 'RFinger11', 'LFinger23', 'LThumb1', 'RThumb1', 'RThumb2', 'LThumb2'] hello_str.position = [eulerHead[2] , eulerHead[1],0.0,-eulerLHip[2], eulerLHip[0], eulerLKnee[0],eulerLAnkle[1] , eulerLAnkle[0], 0.0, -eulerRHip[2], eulerRHip[0], eulerRKnee[0], eulerRAnkle[1], eulerRAnkle[0], eulerLShoulder[0] + math.pi/2, -eulerLShoulder[2] + math.pi/2,eulerLElbow[0]-math.pi/2, eulerLElbow[1], eulerLWrist[2], 0.0, eulerRShoulder[0] + math.pi/2, -eulerRShoulder[2] - math.pi/2,-eulerRElbow[0]+math.pi/2, eulerRElbow[1], eulerRWrist[2], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] hello_str.velocity = [] hello_str.effort = [] #parameters roLHipPitch = hello_str.position[4] roLHipRoll = hello_str.position[3] roRHipPitch = hello_str.position[10] roRHipRoll = hello_str.position[9] roLKnee = hello_str.position[5] roRKnee = hello_str.position[11] roLShoulderPitch = hello_str.position[14] roLShoulderRoll = hello_str.position[15] roRShoulderPitch = hello_str.position[20] roRShoulderRoll = hello_str.position[21] roLElbowYaw = hello_str.position[16] roLElbowRoll = hello_str.position[17] roRElbowYaw = hello_str.position[22] roRElbowRoll = hello_str.position[23] ThetaL = np.array([roLHipPitch,roLHipRoll,roRHipPitch,roRHipRoll,roLKnee,roRKnee]) ThetaU = np.array([roLShoulderPitch, roLShoulderRoll, roRShoulderPitch, roRShoulderRoll, roLElbowYaw, roLElbowRoll, roRElbowYaw, roRElbowRoll]) result = calcAnkleLine(ThetaL, ThetaU) #result = judge(result,ThetaL, ThetaU) thetaLPitch = result[3] thetaLRoll = result[4] thetaRPitch = result[5] thetaRRoll = result[6] #define restrict for joint angles #http://doc.aldebaran.com/1-14/family/robots/joints_robot.html#robot-joints-v4-left-leg-joints if roLHipPitch < HipPitchMin or roLHipPitch > HipPitchMax: roLHipPitch = thetaLHipPitchO else: thetaLHipPitchO = roLHipPitch if roRHipPitch < HipPitchMin or roRHipPitch > HipPitchMax: roRHipPitch = thetaRHipPitchO else: thetaRHipPitchO = roRHipPitch if roLHipRoll < HipRollMin or roLHipRoll > HipRollMax: roLHipRoll = thetaLHipRollO else: thetaLHipRollO = roLHipRoll if roRHipRoll < HipRollMin or roRHipRoll > HipRollMax: roRHipRoll = thetaRHipRollO else: thetaRHipRollO = roRHipRoll if roLKnee < KneeMin or roLKnee > KneeMax: roLKnee = thetaLKneeO else: thetaLKneeO = roLKnee if roRKnee < KneeMin or roRKnee > KneeMax: roRKnee = thetaRKneeO else: thetaRKneeO = roRKnee #if ankle angle exceeds the limit, restore all angles if thetaLPitch < AnklePitchMin or thetaLPitch > AnklePitchMax \ or thetaRPitch < AnklePitchMin or thetaRPitch > AnklePitchMax \ or thetaLRoll < AnkleRollMin or thetaLRoll > AnkleRollMax \ or thetaRRoll < AnkleRollMin or thetaRRoll > AnkleRollMax: thetaLPitch = thetaLAnklePitchO thetaRPitch = thetaRAnklePitchO thetaLRoll = thetaLAnkleRollO thetaRRoll = thetaRAnkleRollO roLHipPitch = thetaLHipPitchO roRHipPitch = thetaRHipPitchO roLHipRoll = thetaLHipRollO roRHipRoll = thetaRHipRollO roLKnee = thetaLKneeO roRKnee = thetaRKneeO else: thetaLAnklePitchO = thetaLPitch thetaRAnklePitchO = thetaRPitch thetaLAnkleRollO = thetaLRoll thetaRAnkleRollO = thetaRRoll print("thetaLPitch, thetaLRoll, thetaPitch, thetaRRoll: ",thetaLPitch, thetaLRoll, thetaRPitch, thetaRRoll) #pubHeadPitch.publish(hello_str.position[1]) #pubHeadYaw.publish(hello_str.position[0]) pubLAnklePitch.publish(thetaLPitch) pubLAnkleRoll.publish(thetaLRoll) pubLElbowRoll.publish(hello_str.position[17]) pubLElbowYaw.publish(hello_str.position[16]) #pubLHand.publish(hello_str.position[19]) pubLHipPitch.publish(roLHipPitch) pubLHipRoll.publish(roLHipRoll) #pubLHipYawPitch.publish(hello_str.position[2]) pubLKneePitch.publish(roLKnee) pubLShoulderPitch.publish(hello_str.position[14]) pubLShoulderRoll.publish(hello_str.position[15]) #pubLWristYaw.publish(hello_str.position[18]) pubRAnklePitch.publish(thetaRPitch) pubRAnkleRoll.publish(thetaRRoll) pubRElbowRoll.publish(hello_str.position[23]) pubRElbowYaw.publish(hello_str.position[22]) #pubRHand.publish(hello_str.position[25]) pubRHipPitch.publish(roRHipPitch) pubRHipRoll.publish(roRHipRoll) #pubRHipYawPitch.publish(hello_str.position[8]) pubRKneePitch.publish(roRKnee) pubRShoulderPitch.publish(hello_str.position[20]) pubRShoulderRoll.publish(hello_str.position[21]) #pubRWristYaw.publish(hello_str.position[24]) rate.sleep()
def talker(): pub = rospy.Publisher('joint_states', JointState, queue_size=10) pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10) rospy.init_node('display', anonymous=True) rate = rospy.Rate(30) # 30hz # pub joint state joint_state_send = JointState() joint_state_send.header = Header() joint_state_send.name = [ 'joint2_to_joint1', 'joint3_to_joint2', 'joint4_to_joint3', 'joint5_to_joint4', 'joint6_to_joint5', 'joint6output_to_joint6' ] joint_state_send.velocity = [0] joint_state_send.effort = [] marker_ = Marker() marker_.header.frame_id = '/joint1' marker_.ns = 'my_namespace' while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() angles = mycobot.get_angles_of_radian() data_list = [] for index, value in enumerate(angles): if index != 2: value *= -1 data_list.append(value) joint_state_send.position = data_list pub.publish(joint_state_send) coords = mycobot.get_coords() rospy.loginfo('{}'.format(coords)) #marker marker_.header.stamp = rospy.Time.now() marker_.type = marker_.SPHERE marker_.action = marker_.ADD marker_.scale.x = 0.04 marker_.scale.y = 0.04 marker_.scale.z = 0.04 #marker position initial # print(coords) if not coords: coords = [0,0,0,0,0,0] rospy.loginfo('error [101]: can not get coord values') marker_.pose.position.x = coords[1] / 1000 * -1 marker_.pose.position.y = coords[0] / 1000 marker_.pose.position.z = coords[2] / 1000 marker_.color.a = 1.0 marker_.color.g = 1.0 pub_marker.publish(marker_) rate.sleep()
def timer_callback(self): phase = fract(self.t / self.period) transfer_time = 1.0 - self.beta base_x = 0.03 y = 0.09 # relative to leg base_z = -0.07 names = [] positions = [] for i in range(4): changed = False dx = 0.0 leg = self.leg_mapping[i] front, side = leg.split('_') x_sign = 1. if front == 'front' else -1. y_sign = 1. if side == 'left' else -1. leg_phase = phase - self.phi[i] if leg_phase < 0.0: leg_phase += 1.0 if leg_phase >= self.beta: # transfer changed = True transfer_phase = \ (leg_phase - self.beta) / (1 - self.beta) x = x_sign * base_x + (-self.R / 2.) + self.R * transfer_phase z = base_z + 0.05 * sin(transfer_phase * pi) elif (0.0 <= phase and phase <= (0.5 - 2 * transfer_time)): # move changed = True move_phase = phase / (0.5 - 2 * transfer_time) dx = (self.R / 2.) * move_phase x = \ x_sign * base_x + \ ((side == 'left') * (self.R / 2.)) - \ dx z = base_z # relative to leg elif (0.5 <= phase and phase <= (1.0 - 2 * transfer_time)): # move changed = True move_phase = (phase - 0.5) / (0.5 - 2 * transfer_time) dx = (self.R / 2.) * move_phase x = \ x_sign * base_x + \ ((side == 'right') * (self.R / 2.)) - \ dx z = base_z # relative to leg if changed: req = LegInvKin.Request() req.x = x req.y = y * y_sign req.z = z future = self.inv_kin_cli.call_async(req) rclpy.spin_until_future_complete(self, future) res = future.result() if res is None or not res.success: print('leg_inv_kin error') continue a, b, g = res.angles names += [ f'{leg}_base_to_{leg}_link1', f'{leg}_link1_to_{leg}_link2', f'{leg}_link2_to_{leg}_link3' ] positions += [a, b, g] self.transform.transform.translation.x += \ dx * self.timer_period msg = JointState() msg.name = names msg.position = positions self.pub.publish(msg) msg = TFMessage(transforms=[self.transform]) self.broadcaster.publish(msg) self.t += self.timer_period
target_linear_vel = 0.0 target_angular_vel = 0.0 control_linear_vel = 0.0 control_angular_vel = 0.0 servox = 0 servoy = 0 try: print(msg) while (1): key = getKey() hello_str = JointState() hello_str.header = Header() hello_str.header.stamp = rospy.Time.now() hello_str.name = ['joint1', 'joint2'] hello_str.position = [servox, servoy] hello_str.velocity = [] hello_str.effort = [] pub1.publish(hello_str) if (servox < -1.5): servox = -1.5 elif (servox > 1.5): servox = 1.5 if (servoy < -0.45): servoy = -0.45 elif (servoy > 1.5): servoy = 1.5 if key == 'w': target_linear_vel = checkLinearLimitVelocity(
def flex_ik_service_client(self, i_Limb="right", i_Pose=None, i_UseAdvanced=False): if self.is_adding_new_item == True: # Can't move if user is in close proximity manually moving the arm return False if i_Pose == None: rospy.logerr( "Error: flex_ik_service_client received 'None' in arg 'i_Pose' for target position" ) return False # Initialize IK service service_name = "ExternalTools/" + i_Limb + "/PositionKinematicsNode/IKService" ik_ServiceClient = rospy.ServiceProxy( service_name, SolvePositionIK ) # Creates a handle to some service that we want to invoke methods on, in this case the SolvePositionIK ik_ServiceReq = SolvePositionIKRequest( ) # Create a request message that will give us the inverse kinematics for some set of joints (Note that this is done at compile time) msg_header = Header( stamp=rospy.Time.now(), frame_id='base' ) # Generates a Header for the message (Think of HTTP) # Hover over the object hover_pose = copy.deepcopy(i_Pose) hover_pose.position.z = hover_pose.position.z + self.hover_dist # hover over the object # Set the goal positions for the limb that we specified (I'm pretty sure that it's for the last joint right_j6) # Set the header that we time stamped # Add Pose object that contains a Point and Quaternion goal_positions = { 'right': PoseStamped(header=msg_header, pose=hover_pose), } ############################################################################################## # # SOLVER IS SETUP TO FIND PATH USING SIMPLE INVERSE KINEMATICS # ############################################################################################## # Add the goal positions for the inverse kinematics ik_ServiceReq.pose_stamp.append(goal_positions[i_Limb]) # Request the inverse kinematics from the base to the "right_hand" link ik_ServiceReq.tip_names.append('right_hand') ############################################################################################## # # SETUP THE SOLVER TO FIND A PATH USING ADVANCED INVERSE KINEMATICS # ############################################################################################## if (i_UseAdvanced): rospy.loginfo("Using Advanced IK Service") # Define the joint seed. If the solver encounters the specified value for a joint, the it will attempt to do some optimisation ik_ServiceReq.seed_mode = ik_ServiceReq.SEED_USER seed = JointState( ) # JointState describes the state of each joint (possition, velocity, effort) seed.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] # The name of the various joints seed.position = [ 0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4 ] # The joint angle at which the solver tries to do optimisation for the respective joints # Pass the seeded angles to the Solver via the SolvePositionIKRequest object ik_ServiceReq.seed_angles.append(seed) # NOTE Once the Primary IK Task has been solved, the IK Solver will try to bias the joint angles towards the goal joint configuration # NOTE The null space represents the extra degrees of freedom that the joints can move through without affecting the results of the Primary IK Task # Here we try to solve the Primary IK Task ik_ServiceReq.use_nullspace_goal.append(True) # NOTE The null space can either be a subset or the full set of joint angles goal = JointState() # This is the subset of joints that make up the null space "Joints A, B, C ... can try to move, but can't affect the result" goal.name = ['right_j1', 'right_j2', 'right_j3'] goal.position = [ 0.1, -0.3, 0.5 ] # If these possitions are encountered, try to do some optimisation ik_ServiceReq.nullspace_goal.append(goal) ik_ServiceReq.nullspace_gain.append(0.4) ############################################################################################## # # PASS THE INVERSE KINEMATICS REQUEST TO THE SOLVER TO GET AN ACTUAL PATH FOR THE ROBOT # ############################################################################################## # rospy.loginfo("Simple IKService Solver Running...") try: rospy.wait_for_service( service_name, timeout=self.arm_timeout ) # Waits for the service (5 seconds), creates the service if it doesn't already exist response = ik_ServiceClient( ik_ServiceReq ) # Get the response from the client, contains the joint positions except (rospy.ServiceException, rospy.ROSException), ex: rospy.logerr("Service Call Failed: %s" % (ex, )) return False
rospy.get_param('/rotational_limits_joint_34'))) rotlim_45 = np.radians(np.array( rospy.get_param('/rotational_limits_joint_45'))) rotlim_56 = np.radians(np.array( rospy.get_param('/rotational_limits_joint_56'))) # Create the publisher. Name the topic "joint_angles_desired", with message type "JointState" pub_joint_angles_desired = rospy.Publisher('/joint_angles_desired', JointState, queue_size=1) # Create the message cmds = [0., -np.pi / 2., np.pi / 2., 0., 0., 0.] joint_angles_desired_msg = JointState() joint_angles_desired_msg.name = [ 'base_joint', 'shoulder_joint', 'elbow_joint', 'forearm_joint', 'wrist_joint', 'fingers_joint' ] joint_angles_desired_msg.position = cmds # upright neutral position def manual_joint_angles(): rospy.init_node('manual_joint_angles_node', anonymous=False) while not rospy.is_shutdown(): try: cmds = np.array( input( '\n Enter Joint angles list (comma separated, no brackets). Ctrl-C and Enter to exit.\nalpha0, beta1, beta2, gamma3, beta4, gamma5:\n' ))
def getJointStateCommand(self, time): cmd = JointState() cmd.name = self.names # find previous which leg we are on if (time < 0 or time > self.times[-1]): return null index = -1 for i in range(0, len(self.times) - 1): if (self.times[i] <= time and time <= self.times[i + 1]): index = i break if (index == -1): return null cmd.position = list([0, 0, 0, 0]) cmd.velocity = list([0, 0, 0, 0]) cmd.effort = list([NaN, NaN, NaN, NaN]) epsilon = 0.001 leg_duration = self.times[index + 1] - self.times[index] leg_percentage = 1.0 - ((self.times[index + 1] - time) / leg_duration) leg_percentage_epsilon = 1.0 - ((self.times[index + 1] - (time + epsilon)) / leg_duration) print leg_percentage print leg_percentage_epsilon # Calculate current workspace waypoint wayp = list([0, 0, 0, 0, 0]) wayp_epsilon = list([0, 0, 0, 0, 0]) prev_wayp = \ [self.waypoints[0][index], self.waypoints[1][index], self.waypoints[2][index], self.waypoints[3][index], self.waypoints[4][index]] next_wayp = \ [self.waypoints[0][index+1], self.waypoints[1][index+1], self.waypoints[2][index+1], self.waypoints[3][index+1], self.waypoints[4][index+1]] for dof in range(0, 5): difference = next_wayp[dof] - prev_wayp[dof] wayp[dof] = prev_wayp[dof] + (leg_percentage * difference) wayp_epsilon[dof] = prev_wayp[dof] + (leg_percentage_epsilon * difference) print wayp print wayp_epsilon cmd.position = kin.ik(wayp, self.elbow_up[index]) position_epsilon = kin.ik(wayp_epsilon, self.elbow_up[index]) for joint in range(0, 4): cmd.velocity[joint] = (cmd.position[joint] - position_epsilon[joint]) / epsilon # Calculate current joint velocity) # Calculate workspace waypoint a small time in the future wayp_epsilon = list([0, 0, 0, 0, 0]) for dof in range(0, 5): difference = next_wayp[dof] - prev_wayp[dof] wayp[dof] = prev_wayp[dof] + (leg_percentage * leg_duration) return cmd
def joint_state_define(x): js = JointState() js.name = joint_ordering js.position = tuple(x) return js
pub.publish(msg.position[idx]) # return True return self.__play_motion_publisher.publish(msg) def set_joint(self, msg): self.__play_motion_publisher.publish(msg) def cur_Joint_handler(self, msg): pass def subscribe(self, service): self.__service = service def unsubscribe(self, service): self.__service = None if __name__ == '__main__': rospy.init_node('social_motion_player_service_test') service = Service() time.sleep(2) msg = JointState() msg.name = [ 'LShoulder_Pitch', 'LShoulder_Roll', 'LElbow_Pitch', 'LElbow_Yaw', 'LWrist_Pitch', 'LFinger', 'RShoulder_Pitch', 'RShoulder_Roll', 'RElbow_Pitch', 'RElbow_Yaw', 'RWrist_Pitch', 'RFinger' ] msg.position = [0, -1.221, 0, 0, 0, 0, 0, 1.221, 0, 0, 0, 0] service.set_joint(msg)