asti robot matlab api joint does not move

Typically: "How do I... ", "How can I... " questions
Post Reply
thoriqsalafi
Posts: 3
Joined: 05 Mar 2014, 04:30

asti robot matlab api joint does not move

Post by thoriqsalafi » 09 Mar 2014, 09:12

hi,

i am using asti robot to move the joint from matlab, here is the matlab code

Code: Select all

function simpleTest()
	disp('Program started');
	%vrep=remApi('remoteApi','extApi.h'); % using the header (requires a compiler)
	vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
	vrep.simxFinish(-1); % just in case, close all opened connections
	clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);
	try
		if (clientID>-1)
			disp('Connected to remote API server');
            [retVal,leftArmJoint]=vrep.simxGetObjectHandle(clientID,'leftArmJoint1',vrep.simx_opmode_oneshot_wait);
            check(vrep,retVal, 'failed to get object', 'got object');
			[res,objs]=vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait);
            [retVal]=vrep.simxSetJointTargetPosition(clientID,leftArmJoint,-(pi/2),vrep.simx_opmode_oneshot);
            check(vrep,retVal, 'failed to set joint', 'set joint');
			if (res==vrep.simx_error_noerror)
				fprintf('Number of objects in the scene: %d\n',length(objs));
			else
				fprintf('Remote API function call returned with error code: %d\n',res);
            end
			vrep.simxFinish(clientID);
		else
			disp('Failed connecting to remote API server');
		end
		vrep.delete(); % call the destructor!
	catch retVal
		vrep.delete(); % call the destructor!
	end;
	
	disp('Program ended');
end

function check(vrep, retVal, textError, textNoError)
     if (retVal==vrep.simx_error_noerror)
         disp(textNoError);
     else
        disp(textError);
     end
end
the asti code is this

Code: Select all

if (simGetScriptExecutionCount()==0) then
simExtRemoteApiStart(19999)
	asti=simGetObjectHandle("Asti")
	lFoot=simGetObjectHandle("leftFootTarget")
	rFoot=simGetObjectHandle("rightFootTarget")
	lPath=simGetObjectHandle("leftFootPath")
	rPath=simGetObjectHandle("rightFootPath")
	lPathLength=simGetPathLength(lPath)
	rPathLength=simGetPathLength(rPath)
	ui=simGetUIHandle("astiUserInterface")
	simSetUIButtonLabel(ui,0,simGetObjectName(asti).." user interface")
	dist=0
	correction=0.0305

	minVal={0,			-- Step size
			0,			-- Walking speed
			-math.pi/2,	-- Neck 1
			-math.pi/8,	-- Neck 2
			-math.pi/2,	-- Left shoulder 1
			0,			-- Left shoulder 2
			-math.pi/2,	-- Left forearm
			-math.pi/2,	-- Right shoulder 1
			0,			-- Right shoulder 2
			-math.pi/2}	-- Right forearm
	rangeVal={	2,			-- Step size
				0.8,		-- Walking speed
				math.pi,	-- Neck 1
				math.pi/4,	-- Neck 2
				math.pi/2,	-- Left shoulder 1
				math.pi/2,	-- Left shoulder 2
				math.pi/2,	-- Left forearm
				math.pi/2,	-- Right shoulder 1
				math.pi/2,	-- Right shoulder 2
				math.pi/2}	-- Right forearm
	uiSliderIDs={3,4,5,6,7,8,9,10,11,12}

	relativeStepSize=1
	nominalVelocity=0.4
	neckJoints={simGetObjectHandle("neckJoint0"),simGetObjectHandle("neckJoint1")}
	leftArmJoints={simGetObjectHandle("leftArmJoint0"),simGetObjectHandle("leftArmJoint1"),simGetObjectHandle("leftArmJoint2")}
	rightArmJoints={simGetObjectHandle("rightArmJoint0"),simGetObjectHandle("rightArmJoint1"),simGetObjectHandle("rightArmJoint2")}
	
	-- Now apply current values to the user interface:
	simSetUISlider(ui,uiSliderIDs[1],(relativeStepSize-minVal[1])*1000/rangeVal[1])
	simSetUISlider(ui,uiSliderIDs[2],(nominalVelocity-minVal[2])*1000/rangeVal[2])
	simSetUISlider(ui,uiSliderIDs[3],(simGetJointPosition(neckJoints[1])-minVal[3])*1000/rangeVal[3])
	simSetUISlider(ui,uiSliderIDs[4],(simGetJointPosition(neckJoints[2])-minVal[4])*1000/rangeVal[4])
	simSetUISlider(ui,uiSliderIDs[5],(simGetJointPosition(leftArmJoints[1])-minVal[5])*1000/rangeVal[5])
	simSetUISlider(ui,uiSliderIDs[6],(simGetJointPosition(leftArmJoints[2])-minVal[6])*1000/rangeVal[6])
	simSetUISlider(ui,uiSliderIDs[7],(simGetJointPosition(leftArmJoints[3])-minVal[7])*1000/rangeVal[7])
	simSetUISlider(ui,uiSliderIDs[8],(simGetJointPosition(rightArmJoints[1])-minVal[8])*1000/rangeVal[8])
	simSetUISlider(ui,uiSliderIDs[9],(simGetJointPosition(rightArmJoints[2])-minVal[9])*1000/rangeVal[9])
	simSetUISlider(ui,uiSliderIDs[10],(simGetJointPosition(rightArmJoints[3])-minVal[10])*1000/rangeVal[10])
end

simHandleChildScript(sim_handle_all_except_explicit)

-- Read desired values from the user interface:
relativeStepSize=minVal[1]+simGetUISlider(ui,uiSliderIDs[1])*rangeVal[1]/1000
nominalVelocity=minVal[2]+simGetUISlider(ui,uiSliderIDs[2])*rangeVal[2]/1000
simSetJointTargetPosition(neckJoints[1],minVal[3]+simGetUISlider(ui,uiSliderIDs[3])*rangeVal[3]/1000)
simSetJointTargetPosition(neckJoints[2],minVal[4]+simGetUISlider(ui,uiSliderIDs[4])*rangeVal[4]/1000)
simSetJointTargetPosition(leftArmJoints[3],minVal[7]+simGetUISlider(ui,uiSliderIDs[7])*rangeVal[7]/1000)
simSetJointTargetPosition(rightArmJoints[1],minVal[8]+simGetUISlider(ui,uiSliderIDs[8])*rangeVal[8]/1000)
simSetJointTargetPosition(rightArmJoints[2],minVal[9]+simGetUISlider(ui,uiSliderIDs[9])*rangeVal[9]/1000)
simSetJointTargetPosition(rightArmJoints[3],minVal[10]+simGetUISlider(ui,uiSliderIDs[10])*rangeVal[10]/1000)


-- Get the desired position and orientation of each foot from the paths (you can also use a table of values for that):
t=simGetSimulationTimeStep()*nominalVelocity
dist=dist+t
lPos=simGetPositionOnPath(lPath,dist/lPathLength)
lOr=simGetOrientationOnPath(lPath,dist/lPathLength)

p=simGetPathPosition(rPath)
rPos=simGetPositionOnPath(rPath,(dist+correction)/rPathLength)
rOr=simGetOrientationOnPath(rPath,(dist+correction)/rPathLength)


-- Now we have the desired absolute position and orientation for each foot.
-- Now transform the absolute position/orientation to position/orientation relative to asimo
-- Then modulate the movement forward/backward with the desired "step size"
-- Then transform back into absolute position/orientation:
astiM=simGetObjectMatrix(asti,-1)
astiMInverse=simGetInvertedMatrix(astiM)

m=simMultiplyMatrices(astiMInverse,simBuildMatrix(lPos,lOr))
m[8]=m[8]*relativeStepSize
m=simMultiplyMatrices(astiM,m)
lPos={m[4],m[8],m[12]}
lOr=simGetEulerAnglesFromMatrix(m)

m=simMultiplyMatrices(astiMInverse,simBuildMatrix(rPos,rOr))
m[8]=m[8]*relativeStepSize	
m=simMultiplyMatrices(astiM,m)
rPos={m[4],m[8],m[12]}
rOr=simGetEulerAnglesFromMatrix(m)


-- Finally apply the desired positions/orientations to each foot
-- We simply apply them to two dummy objects that are then handled
-- by the IK module to automatically calculate all leg joint desired values
-- Since the leg joints operate in hybrid mode, the IK calculation results
-- are then automatically applied as the desired values during dynamics calculation
simSetObjectPosition(lFoot,-1,lPos)
simSetObjectOrientation(lFoot,-1,lOr)

simSetObjectPosition(rFoot,-1,rPos)
simSetObjectOrientation(rFoot,-1,rOr)

i tried but the result is this

Code: Select all

asti
Program started
Running Matlab win64
make sure you use the corresponding remoteApi library
(i.e. 32bit Matlab will not work with 64bit remoteApi, and vice-versa)
Connected to remote API server
got object
failed to set joint
Number of objects in the scene: 80
Program ended
anyone can help me to make the joint move from matlab?

thank you!

coppelia
Site Admin
Posts: 7689
Joined: 14 Dec 2012, 00:25

Re: asti robot matlab api joint does not move

Post by coppelia » 09 Mar 2014, 22:37

Hello,

first of all, if you are running the 64bit version of Matlab, use this code instead (that code will be available in the next release (V3.1.1)), it corrects for a bug that affects 4-5 Matlab functions, including simxSetJointTargetPosition.

Then, you should remember that if you use the operation mode simx_opmode_oneshot, the command is non-blocking and will return immediately. It will take a few miliseconds until the command is sent to V-REP. If you kill the connection directly after sending the command, it might be that the command never arrives. To make sure that command has arrived before killing the connection, you could simple call: simxGetPingTime.

Third, in the Asti model, the joints are handled via inverse kinematics. If you try to interfere while inverse kinematics is also trying to set the joint position, you might get very strange results. You will have to disable the inverse kinematics, or simply remove the 2 IK groups. Then, you will have to set all joints to force/torque mode, enable the motors and position control.

Try maybe starting with a simple example, with a single joint, and extend from there.

Cheers

thoriqsalafi
Posts: 3
Joined: 05 Mar 2014, 04:30

Re: asti robot matlab api joint does not move

Post by thoriqsalafi » 10 Mar 2014, 05:05

thanks admin for the perfect reply, I managed to get the joints move!

Post Reply