[SequencePlayer/seqplay.h] skip removing state if state==created in remove()

[Naoki-Hiraoka]

SequencePlayerのremoveJointGroup()を未使用のJointGroupに対して呼ぶと、SequencePlayerから出力される指令関節角度の値が0に突然変化し、2.5秒かけてもとの値に戻る、という挙動を示し、危険でした。

原因は、未使用のJointGroup(state==created)はget関数のなかで何もしないことが期待されているにも関わらず、removeJointGroup()を呼ぶとstateがremovingに変化してしまい、初期値の0から2.5秒かけてもとの値に戻る補間軌道が実行されてしまうためでした。

hrpsys-base/rtc/SequencePlayer/seqplay.h

Lines 71 to 90 in 41f853a

	void get(double *full, double *dfull = NULL, bool popp=true){
	if (state == created) return;
	if (state == removing){
	double x[indices.size()];
	double v[indices.size()];
	for (size_t i=0; i<indices.size(); i++){
	x[i] = full[indices[i]];
	v[i] = dfull ? dfull[indices[i]] : 0;
	}
	inter->setGoal(x, v, time2remove);
	time2remove -= inter->deltaT();
	if (time2remove <= 0) state = removed;
	}
	double x[indices.size()], v[indices.size()];
	inter->get(x, v, popp);
	for (size_t i=0; i<indices.size(); i++){
	full[indices[i]] = x[i];
	if (dfull) dfull[indices[i]] = v[i];
	}
	}

hrpsys-base/rtc/SequencePlayer/seqplay.h

Lines 125 to 128 in 41f853a

	void remove(double time){
	state = removing;
	time2remove = time;
	}

未使用のJointGroup(state==created)は、removeJointGroup()を呼んだときに、stateremoving を経由せずに直接stateremovedに遷移するようにしました。

[Naoki-Hiraoka]

#1329