def sendfr(x):
    command = JointCommand()
    command.header.seq = 0
    command.header.stamp = rospy.Time.now()
    command.header.frame_id = ''

    command.name = [
        'motorFR1_motorFR2', 'motorFR2_thigh', 'FRthigh_calf', 'FRcalf_foot'
    ]

    command.position = [0, x[0][0], x[0][1], x[0][2]]

    command.velocity = [5] * 4
    command.effort = [5] * 4
    command.stiffness = [0] * 4
    command.damping = [0] * 4
    command.ctrl_mode = [1] * 4
    command.aux_name = ''
    command.aux = [0] * 4

    pub.publish(command)
def sendall(x, y):
    command = JointCommand()
    command.header.seq = 0
    command.header.stamp = rospy.Time.now()
    command.header.frame_id = ''

    command.name = [
        'motorFL1_motorFL2',  #0
        'motorFL2_thigh',  #1
        'FLthigh_calf',  #2
        'motorFR1_motorFR2',  #3
        'motorFR2_thigh',  #4
        'FRthigh_calf',  #5
        'motorRL1_motorRL2',  #6
        'motorRL2_motorRL3',  #7
        'motorRL3_thigh',  #8
        'RLthigh_calf',  #9
        'motorRR1_motorRR2',  #10
        'motorRR2_motorRR3',  #11
        'motorRR3_thigh',  #12
        'RRthigh_calf',  #13
        'FLcalf_foot',  #14
        'FRcalf_foot',  #15
        'RLcalf_foot',  #16
        'RRcalf_foot'
    ]  #17

    command.position = [
        0,
        x[0][0],
        x[0][1],  #FL
        0,
        x[0][0],
        x[0][1],  #FR
        0,
        0,
        y[0][0],
        y[0][1],  #RL
        0,
        0,
        y[0][0],
        y[0][1],  #RR
        x[0][2],
        x[0][2],
        y[0][2],
        y[0][2]
    ]  #Feet

    #home = [0,-0.811034394288,-1.389669322,0,-0.811034394288,-1.389669322,0,0,-0.744537254074,-1.50666291141,0,0,-0.744537254074,-1.50666291141,0.632296369706,0.632296369706,0.682792819072,0.682792819072]

    command.velocity = [5] * 18
    command.effort = [5] * 18
    command.stiffness = [0] * 18
    command.damping = [0] * 18
    command.ctrl_mode = [1] * 18
    command.aux_name = ''
    command.aux = [0] * 18

    #time.sleep(0.1)

    pub.publish(command)
Example #3
0
def angle():

    pub = rospy.Publisher('/xbotcore/command', JointCommand, queue_size=10)
    time.sleep(0.5)

    command = JointCommand()
    command.header.seq = 0
    command.header.stamp = rospy.Time.now()
    command.header.frame_id = ''

    #calf lower="-2.77507351067" upper="-0.610865238198"
    command.name = [
        'motorFL1_motorFL2',  #0
        'motorFL2_thigh',  #1
        'FLthigh_calf',  #2
        'motorFR1_motorFR2',  #3
        'motorFR2_thigh',  #4
        'FRthigh_calf',  #5
        'motorRL1_motorRL2',  #6
        'motorRL2_motorRL3',  #7
        'motorRL3_thigh',  #8
        'RLthigh_calf',  #9
        'motorRR1_motorRR2',  #10
        'motorRR2_motorRR3',  #11
        'motorRR3_thigh',  #12
        'RRthigh_calf',  #13
        'FLcalf_foot',  #14
        'FRcalf_foot',  #15
        'RLcalf_foot',  #16
        'RRcalf_foot'
    ]  #17

    command.position = [
        0,
        1.57,
        -3.14,  #FL
        0,
        1.57,
        -3.14,  #FR
        0,
        0,
        1.57,
        -3.14,  #RL
        0,
        0,
        1.57,
        -3.14,  #RR
        0,
        0,
        0,
        0
    ]  #Feet
    #1.57 = 90deg
    command.velocity = [5] * 18
    command.effort = [5] * 18
    command.stiffness = [0] * 18
    command.damping = [0] * 18
    command.ctrl_mode = [1] * 18
    command.aux_name = ''
    command.aux = [0] * 18
    print command.name
    print command.position

    command.position = [
        0,
        -0.011956099111670504,
        -1.700533245580413,  #FL
        0,
        -0.011956099111670504,
        -1.700533245580413,  #FR
        0,
        0,
        0.07105991293761194,
        -1.4947300880632601,  #RL
        0,
        0,
        0.07105991293761194,
        -1.4947300880632601,  #RR
        0.14408199828187618,
        0.14408199828187618,
        -0.14473717128455932,
        -0.14473717128455932
    ]  #Feet

    pub.publish(command)

    quit()
def angle():

    pub = rospy.Publisher('/xbotcore/command', JointCommand, queue_size=10)
    time.sleep(0.5)

    command = JointCommand()
    command.header.seq = 0
    command.header.stamp = rospy.Time.now()
    command.header.frame_id = ''

    #calf lower="-2.77507351067" upper="-0.610865238198"
    command.name = [
        'motorFL1_motorFL2',  #0
        'motorFL2_thigh',  #1
        'FLthigh_calf',  #2
        'motorFR1_motorFR2',  #3
        'motorFR2_thigh',  #4
        'FRthigh_calf',  #5
        'motorRL1_motorRL2',  #6
        'motorRL2_motorRL3',  #7
        'motorRL3_thigh',  #8
        'RLthigh_calf',  #9
        'motorRR1_motorRR2',  #10
        'motorRR2_motorRR3',  #11
        'motorRR3_thigh',  #12
        'RRthigh_calf',  #13
        'FLcalf_foot',  #14
        'FRcalf_foot',  #15
        'RLcalf_foot',  #16
        'RRcalf_foot'
    ]  #17

    command.position = [
        0,
        1.57,
        -3.14,  #FL
        0,
        1.57,
        -3.14,  #FR
        0,
        0,
        1.57,
        -3.14,  #RL
        0,
        0,
        1.57,
        -3.14,  #RR
        0,
        0,
        0,
        0
    ]  #Feet
    #1.57 = 90deg
    command.velocity = [5] * 18
    command.effort = [5] * 18
    command.stiffness = [0] * 18
    command.damping = [0] * 18
    command.ctrl_mode = [1] * 18
    command.aux_name = ''
    command.aux = [0] * 18
    print command.name
    print command.position
    time.sleep(0.1)
    pub.publish(command)

    time.sleep(3)

    command.position = [
        0,
        -0.20135792079,
        -2.364806627,  #FL
        0,
        -0.20135792079,
        -2.364806627,  #FR
        0,
        0,
        -0.20135792079,
        -2.364806627,  #RL
        0,
        0,
        -0.20135792079,
        -2.364806627,  #RR
        1.13,
        1.13,
        1.13,
        1.13
    ]  #Feet

    pub.publish(command)
    time.sleep(3)

    command.position = [
        0,
        -0.811034394288,
        -1.389669322,  #FL
        0,
        -0.811034394288,
        -1.389669322,  #FR
        0,
        0,
        -0.744537254074,
        -1.50666291141,  #RL
        0,
        0,
        -0.744537254074,
        -1.50666291141,  #RR
        0.632296369706,
        0.632296369706,
        0.682792819072,
        0.682792819072
    ]  #Feet

    pub.publish(command)

    quit()
Example #5
0
def angle():

	pub = rospy.Publisher('/xbotcore/command', JointCommand, queue_size=10)
	time.sleep(0.5)
	
	command = JointCommand()
	command.header.seq = 0
	command.header.stamp = rospy.Time.now()
	command.header.frame_id = ''

	#calf lower="-2.77507351067" upper="-0.610865238198"
	command.name = ['motorFL1_motorFL2', #0
			'motorFL2_thigh',    #1
			'FLthigh_calf',      #2
			'motorFR1_motorFR2', #3
			'motorFR2_thigh',    #4
			'FRthigh_calf',      #5
			'motorRL1_motorRL2', #6
			'motorRL2_motorRL3', #7
			'motorRL3_thigh',    #8
			'RLthigh_calf',      #9
			'motorRR1_motorRR2', #10
			'motorRR2_motorRR3', #11
			'motorRR3_thigh',    #12
			'RRthigh_calf',      #13
			'FLcalf_foot',       #14
			'FRcalf_foot',       #15
			'RLcalf_foot',       #16
			'RRcalf_foot']       #17

	command.position = [0,1.57,-3.14, #FL
				0,1.57,-3.14, #FR
				0,0,1.57,-3.14, #RL
				0,0,1.57,-3.14, #RR
				0,0,0,0] #Feet
	#1.57 = 90deg
	command.velocity = [5]*18
	command.effort = [5]*18
	command.stiffness = [0]*18
	command.damping = [0]*18
	command.ctrl_mode = [1]*18
	command.aux_name = ''
	command.aux = [0]*18
	print command.name
	print command.position
	time.sleep(0.1)
	pub.publish(command)

	time.sleep(3)

	command.position = [0,-0.20135792079,-2.364806627, #FL
				0,-0.20135792079,-2.364806627, #FR
				0,0,-0.20135792079,-2.364806627, #RL
				0,0,-0.20135792079,-2.364806627, #RR
				1.13,1.13,1.13,1.13] #Feet

	pub.publish(command)
	time.sleep(3)

	command.position = [0,-0.01197676887228838, -1.4138172232536512, #FL
				0,-0.01197676887228838, -1.4138172232536512, #FR
				0,0,0.07105991293761194, -1.4947300880632601, #RL
				0,0,0.07105991293761194, -1.4947300880632601, #RR
				-0.1426133542842678,-0.1426133542842678,-0.14473717128455932,-0.14473717128455932] #Feet

	pub.publish(command)
	time.sleep(3)

	command.position = [0,-0.01197676887228838, -1.4138172232536512, #FL
				0,-0.01197676887228838, -1.4138172232536512, #FR
				0,0,0.07105991293761194, -1.4947300880632601, #RL
				0,0,0.8169004854396247, -1.960029803780545, #RR
				-0.1426133542842678,-0.1426133542842678,-0.14473717128455932,-0.14473717128455932] #Feet

	pub.publish(command)
	
	quit()